From 1765b36ee929cb52bcd8e1dccc3b8f37d08dd339 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Feb 2021 15:08:14 +0100 Subject: [PATCH 01/85] Add loadcell driver --- Makefile | 1 + src/deck/drivers/src/loadcell.c | 204 ++++++++++++++++++++++++++++++++ 2 files changed, 205 insertions(+) create mode 100644 src/deck/drivers/src/loadcell.c diff --git a/Makefile b/Makefile index 3b957bf772..9aa6d92821 100644 --- a/Makefile +++ b/Makefile @@ -216,6 +216,7 @@ PROJ_OBJ += oa.o PROJ_OBJ += multiranger.o PROJ_OBJ += lighthouse.o PROJ_OBJ += activeMarkerDeck.o +PROJ_OBJ += loadcell.o # Uart2 Link for CRTP communication is not compatible with decks using uart2 ifeq ($(UART2_LINK), 1) diff --git a/src/deck/drivers/src/loadcell.c b/src/deck/drivers/src/loadcell.c new file mode 100644 index 0000000000..6d862c4a6b --- /dev/null +++ b/src/deck/drivers/src/loadcell.c @@ -0,0 +1,204 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2021 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * loadcell.c - Deck driver for HX711 load cell + * See + * * https://learn.sparkfun.com/tutorials/load-cell-amplifier-hx711-breakout-hookup-guide + * * Code based on https://github.com/bogde/HX711 + */ + +#include +#include +#include "stm32fxxx.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "timers.h" + +#include "deck.h" +#include "param.h" +#include "log.h" + +// Hardware defines +#define CLK_PIN DECK_GPIO_IO1 +#define DAT_PIN DECK_GPIO_IO2 + +static bool isInit; +static int32_t rawWeight; +static float weight; +static bool enable = true; + +static float a = 4.22852802e-05; +static float b = -2.21784688e+01; + +static xTimerHandle timer; + +static enum hx711_gain +{ + GAIN128 = 1, + GAIN64 = 3, + GAIN32 = 2, +} gain; + +static void hx711_init(void); +static bool hx711_is_ready(void); +// static void hx711_set_gain(void); +static int32_t hx711_read(void); +// static void hx711_power_down(void); +static void hx711_power_up(void); + +static void delayMicroseconds(uint32_t us) +{ + uint64_t start = usecTimestamp(); + while (usecTimestamp() - start < us); +} + +static void hx711_init(void) +{ + pinMode(CLK_PIN, OUTPUT); + pinMode(DAT_PIN, INPUT); + // hx711_set_gain(); + hx711_power_up(); +} + +static bool hx711_is_ready(void) +{ + return digitalRead(DAT_PIN) == LOW; +} + +// static void hx711_set_gain(void) +// { +// digitalWrite(CLK_PIN, LOW); +// hx711_read(); +// } + +static int32_t hx711_read(void) +{ + // Wait for the chip to become ready. + // while (!hx711_is_ready()); + + // if (!hx711_is_ready()) { + // return 0; + // } + + // Define structures for reading data into. + int32_t value = 0; + + // Protect the read sequence from system interrupts. If an interrupt occurs during + // the time the PD_SCK signal is high it will stretch the length of the clock pulse. + // If the total pulse time exceeds 60 uSec this will cause the HX711 to enter + // power down mode during the middle of the read sequence. While the device will + // wake up when PD_SCK goes low again, the reset starts a new conversion cycle which + // forces DOUT high until that cycle is completed. + // + // The result is that all subsequent bits read by shiftIn() will read back as 1, + // corrupting the value returned by read(). + + portDISABLE_INTERRUPTS(); + + // Pulse the clock pin 24 times to read the data. + for (int i = 0; i < 24; ++i) { + digitalWrite(CLK_PIN, HIGH); + value |= digitalRead(DAT_PIN) << (24 - i); + digitalWrite(CLK_PIN, LOW); + delayMicroseconds(1); + } + + // Set the channel and the gain factor for the next reading using the clock pin. + for (int i = 0; i < gain; ++i) { + digitalWrite(CLK_PIN, HIGH); + delayMicroseconds(1); + digitalWrite(CLK_PIN, LOW); + delayMicroseconds(1); + } + + portENABLE_INTERRUPTS(); + + // Replicate the most significant bit to pad out a 32-bit signed integer + if (value & (1<<24)) { + value = 0xFF000000 | value; + } + return value; +} + +// static void hx711_power_down(void) +// { +// digitalWrite(CLK_PIN, LOW); +// digitalWrite(CLK_PIN, HIGH); +// } + +static void hx711_power_up(void) +{ + digitalWrite(CLK_PIN, LOW); +} + +static void loadcellTimer(xTimerHandle timer) +{ + if (enable && hx711_is_ready()) { + rawWeight = hx711_read(); + weight = a * rawWeight + b; + } +} + +static void loadcellInit(DeckInfo *info) +{ + if (isInit) { + return; + } + + gain = GAIN128; + hx711_init(); + + timer = xTimerCreate( "loadcellTimer", M2T(10), + pdTRUE, NULL, loadcellTimer ); + xTimerStart(timer, 100); + + isInit = true; +} + +static const DeckDriver loadcell_deck = { + .vid = 0xBC, + .pid = 0xD0, + .name = "bcLoadcell", + + .usedGpio = DECK_USING_IO_1 | DECK_USING_IO_2, + + .init = loadcellInit, +}; + +DECK_DRIVER(loadcell_deck); + +PARAM_GROUP_START(deck) +PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit) +PARAM_GROUP_STOP(deck) + +PARAM_GROUP_START(loadcell) +PARAM_ADD(PARAM_UINT8, enable, &enable) +PARAM_ADD(PARAM_FLOAT, a, &a) +PARAM_ADD(PARAM_FLOAT, b, &b) +PARAM_GROUP_STOP(loadcell) + +LOG_GROUP_START(loadcell) +LOG_ADD(LOG_INT32, rawWeight, &rawWeight) +LOG_ADD(LOG_FLOAT, weight, &weight) +LOG_GROUP_STOP(loadcell) From c79592e60faad7167fef9ea6b2059c3b0908f539 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Feb 2021 15:19:15 +0100 Subject: [PATCH 02/85] Allow to configure battery compensation with param motor.batCompensation --- src/drivers/interface/motors.h | 5 ----- src/drivers/src/motors.c | 28 +++++++++++++++++++--------- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h index a62d121bd5..30de199a8f 100644 --- a/src/drivers/interface/motors.h +++ b/src/drivers/interface/motors.h @@ -57,11 +57,6 @@ #define MOTORS_TIM_DBG_CFG DBGMCU_APB2PeriphConfig #define MOTORS_GPIO_AF_CFG(a,b,c) GPIO_PinAFConfig(a,b,c) -// Compensate thrust depending on battery voltage so it will produce about the same -// amount of thrust independent of the battery voltage. Based on thrust measurement. -// Not applied for brushless motor setup. -#define ENABLE_THRUST_BAT_COMPENSATED - //#define ENABLE_ONESHOT125 #ifdef ENABLE_ONESHOT125 diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 985a87041a..74d0e21ee4 100644 --- a/src/drivers/src/motors.c +++ b/src/drivers/src/motors.c @@ -41,6 +41,7 @@ //Logging includes #include "log.h" +#include "param.h" static uint16_t motorsBLConvBitsTo16(uint16_t bits); static uint16_t motorsBLConv16ToBits(uint16_t bits); @@ -63,6 +64,11 @@ const uint16_t testsound[NBR_OF_MOTORS] = {A4, A5, F5, D5 }; static bool isInit = false; +// Compensate thrust depending on battery voltage so it will produce about the same +// amount of thrust independent of the battery voltage. Based on thrust measurement. +// Not applied for brushless motor setup. +static uint8_t batCompensation = true; + /* Private functions */ static uint16_t motorsBLConvBitsTo16(uint16_t bits) @@ -230,19 +236,19 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust) ratio = ithrust; - #ifdef ENABLE_THRUST_BAT_COMPENSATED if (motorMap[id]->drvType == BRUSHED) { - float thrust = ((float)ithrust / 65536.0f) * 60; - float volts = -0.0006239f * thrust * thrust + 0.088f * thrust; - float supply_voltage = pmGetBatteryVoltage(); - float percentage = volts / supply_voltage; - percentage = percentage > 1.0f ? 1.0f : percentage; - ratio = percentage * UINT16_MAX; + if (batCompensation) { + float thrust = ((float)ithrust / 65536.0f) * 60; + float volts = -0.0006239f * thrust * thrust + 0.088f * thrust; + float supply_voltage = pmGetBatteryVoltage(); + float percentage = volts / supply_voltage; + percentage = percentage > 1.0f ? 1.0f : percentage; + ratio = percentage * UINT16_MAX; + } motor_ratios[id] = ratio; - } - #endif + if (motorMap[id]->drvType == BRUSHLESS) { motorMap[id]->setCompare(motorMap[id]->tim, motorsBLConv16ToBits(ratio)); @@ -330,3 +336,7 @@ LOG_ADD(LOG_UINT32, m2_pwm, &motor_ratios[1]) LOG_ADD(LOG_UINT32, m3_pwm, &motor_ratios[2]) LOG_ADD(LOG_UINT32, m4_pwm, &motor_ratios[3]) LOG_GROUP_STOP(pwm) + +PARAM_GROUP_START(motor) +PARAM_ADD(PARAM_UINT8, batCompensation, &batCompensation) +PARAM_GROUP_STOP(motor) \ No newline at end of file From fb214bde39fd5eac9d11ac84f939b4a81ec11708 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Fri, 19 Feb 2021 17:08:09 +0100 Subject: [PATCH 03/85] Added rpm measurement deck driver for the rpm proto deck. --- src/deck/drivers/src/rpm.c | 331 +++++++++++++++++++++++++++++++++++++ 1 file changed, 331 insertions(+) create mode 100644 src/deck/drivers/src/rpm.c diff --git a/src/deck/drivers/src/rpm.c b/src/deck/drivers/src/rpm.c new file mode 100644 index 0000000000..57153b77e4 --- /dev/null +++ b/src/deck/drivers/src/rpm.c @@ -0,0 +1,331 @@ +/* + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * rpm.c - Deck that measure the motor RPM using QRD1114 IR reflector-sensor. + */ +#define DEBUG_MODULE "RPM" + +#include +#include + +#include "stm32fxxx.h" +#include "config.h" +#include "deck.h" +#include "debug.h" +#include "log.h" + +//Hardware configuration +#define ET_GPIO_PERIF (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC) + +#define ET_GPIO_PORT_TX2 GPIOA +#define ET_GPIO_PIN_TX2 GPIO_Pin_2 +#define ET_GPIO_PORT_RX2 GPIOA +#define ET_GPIO_PIN_RX2 GPIO_Pin_3 +#define ET_GPIO_PORT_IO2 GPIOB +#define ET_GPIO_PIN_IO2 GPIO_Pin_5 +#define ET_GPIO_PORT_IO3 GPIOB +#define ET_GPIO_PIN_IO3 GPIO_Pin_4 + +#define ER_NBR_PINS 4 + +typedef struct _etGpio +{ + GPIO_TypeDef *port; + uint16_t pin; + char name[6]; +} EtGpio; + +EtGpio erGpio[ER_NBR_PINS] = +{ + {ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"}, + {ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"}, + {ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"}, + {ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"}, +}; + +static bool isInit; +static uint16_t lastcc1Val; +static uint16_t lastcc2Val; +static uint16_t lastcc3Val; +static uint16_t lastcc4Val; +static uint16_t m1Time[2]; +static uint16_t m2Time[2]; +static uint16_t m3Time[2]; +static uint16_t m4Time[2]; +static int m1cnt; +static int m2cnt; +static int m3cnt; +static int m4cnt; +static uint16_t m1rpm; +static uint16_t m2rpm; +static uint16_t m3rpm; +static uint16_t m4rpm; + + +bool rpmInit(void) +{ + int i; + bool status = true; + isInit = true; + + GPIO_InitTypeDef GPIO_InitStructure; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_ICInitTypeDef TIM_ICInitStructure; + NVIC_InitTypeDef NVIC_InitStructure; + + // Enable Clocks + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB, ENABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM5, ENABLE); + + // Configure optical switch input pins + for (i = 0; i < ER_NBR_PINS; i++) + { + GPIO_InitStructure.GPIO_Pin = erGpio[i].pin; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed; + GPIO_Init(erGpio[i].port, &GPIO_InitStructure); + } + + // Map timer to alternate functions + GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM5); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM5); + GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3); + GPIO_PinAFConfig(GPIOB, GPIO_PinSource4, GPIO_AF_TIM3); + + /* Time base configuration */ + TIM_TimeBaseStructure.TIM_Period = 0xFFFF; + TIM_TimeBaseStructure.TIM_Prescaler = (84 - 1); // 84 / 84M = 1uS + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; + TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); + + TIM_ICInitStructure.TIM_Channel = TIM_Channel_3; + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStructure.TIM_ICFilter = 0x0; + TIM_ICInit(TIM5, &TIM_ICInitStructure); + TIM_ICInitStructure.TIM_Channel = TIM_Channel_4; + TIM_ICInit(TIM5, &TIM_ICInitStructure); + TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; + TIM_ICInit(TIM3, &TIM_ICInitStructure); + TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; + TIM_ICInit(TIM3, &TIM_ICInitStructure); + + /* Enable the TIM5 and TIM3 global Interrupt */ + NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 10; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; + NVIC_Init(&NVIC_InitStructure); + + /* TIM enable counter */ + TIM_Cmd(TIM5, ENABLE); + TIM_Cmd(TIM3, ENABLE); + TIM_ITConfig(TIM5, TIM_IT_CC3, ENABLE); + TIM_ITConfig(TIM5, TIM_IT_CC4, ENABLE); + TIM_ITConfig(TIM5, TIM_IT_Update, ENABLE); + TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE); + TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE); + TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); + + return status; +} + +static uint16_t calcRPM(uint32_t t1, uint32_t t2) +{ + uint16_t rpm; + // (timer resolution * sec/min) / (time for one revolution) + rpm = (1000000 * 60) / (t1 + t2); + + return rpm; +} + +void __attribute__((used)) TIM5_IRQHandler(void) +{ + static int updateM1Cnt = 2; + static int updateM2Cnt = 2; + uint16_t ccVal; + + //Motor1 + if(TIM_GetITStatus(TIM5, TIM_IT_CC3) == SET) + { + /* Clear TIM5 Capture compare interrupt pending bit */ + TIM_ClearITPendingBit(TIM5, TIM_IT_CC3); + ccVal = TIM_GetCapture3(TIM5); + + if (TIM_GetFlagStatus(TIM5, TIM_FLAG_CC3OF)) + { + // Overflow + lastcc3Val = ccVal; + } + + if (ccVal > lastcc3Val) + m1Time[m1cnt & 0x01] = ccVal - lastcc3Val; + else + m1Time[m1cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc3Val); + + lastcc3Val = ccVal; + m1cnt++; + m1rpm = calcRPM(m1Time[0], m1Time[1]); + updateM1Cnt = 2; + } + + //Motor2 + if(TIM_GetITStatus(TIM5, TIM_IT_CC4) == SET) + { + /* Clear TIM5 Capture compare interrupt pending bit */ + TIM_ClearITPendingBit(TIM5, TIM_IT_CC4); + ccVal = TIM_GetCapture4(TIM5); + + if (TIM_GetFlagStatus(TIM5, TIM_FLAG_CC4OF)) + { + // Overflow + lastcc4Val = ccVal; + } + + if (ccVal > lastcc4Val) + m2Time[m2cnt & 0x01] = ccVal - lastcc4Val; + else + m2Time[m2cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc4Val); + + lastcc4Val = ccVal; + m2cnt++; + m2rpm = calcRPM(m2Time[0], m2Time[1]); + updateM2Cnt = 2; + } + + if(TIM_GetITStatus(TIM5, TIM_IT_Update) == SET) + { + TIM_ClearITPendingBit(TIM5, TIM_IT_Update); + if (--updateM1Cnt < 0) + { + m1rpm = 0; + } + if (--updateM2Cnt < 0) + { + m2rpm = 0; + } + } +} + +void __attribute__((used)) TIM3_IRQHandler(void) +{ + static int updateM3Cnt = 2; + static int updateM4Cnt = 2; + uint16_t ccVal; + + //Motor3 + if(TIM_GetITStatus(TIM3, TIM_IT_CC1) == SET) + { + /* Clear TIM3 Capture compare interrupt pending bit */ + TIM_ClearITPendingBit(TIM3, TIM_IT_CC1); + ccVal = TIM_GetCapture1(TIM3); + + if (TIM_GetFlagStatus(TIM3, TIM_FLAG_CC1OF)) + { + // Overflow + lastcc1Val = ccVal; + } + + if (ccVal > lastcc1Val) + m3Time[m3cnt & 0x01] = ccVal - lastcc1Val; + else + m3Time[m3cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc1Val); + + lastcc1Val = ccVal; + m3cnt++; + m3rpm = calcRPM(m3Time[0], m3Time[1]); + updateM3Cnt = 2; + +// rpmPutchar((m3Time >> 8) & 0x00FF); +// rpmPutchar(m3Time & 0x00FF); + } + + //Motor4 + if(TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET) + { + /* Clear TIM3 Capture compare interrupt pending bit */ + TIM_ClearITPendingBit(TIM3, TIM_IT_CC2); + ccVal = TIM_GetCapture2(TIM3); + + if (TIM_GetFlagStatus(TIM3, TIM_FLAG_CC2OF)) + { + // Overflow + lastcc2Val = ccVal; + } + + if (ccVal > lastcc2Val) + m4Time[m4cnt & 0x01] = ccVal - lastcc2Val; + else + m4Time[m4cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc2Val); + + lastcc2Val = ccVal; + m4cnt++; + m4rpm = calcRPM(m4Time[0], m4Time[1]); + updateM4Cnt = 2; + +// rpmPutchar((m4rpm >> 8) & 0x00FF); +// rpmPutchar(m4rpm & 0x00FF); + } + + if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET) + { + TIM_ClearITPendingBit(TIM3, TIM_IT_Update); + if (--updateM3Cnt < 0) + { + m3rpm = 0; + } + if (--updateM4Cnt < 0) + { + m4rpm = 0; + } + } +} + +static const DeckDriver rpm_deck = { + .vid = 0x00, + .pid = 0x00, + .name = "bcRpm", + + .usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3 | DECK_USING_TX2 | DECK_USING_RX2, + + .init = rpmInit, +}; + +DECK_DRIVER(rpm_deck); + + +LOG_GROUP_START(rpm) +LOG_ADD(LOG_UINT16, m1, &m1rpm) +LOG_ADD(LOG_UINT16, m2, &m2rpm) +LOG_ADD(LOG_UINT16, m3, &m3rpm) +LOG_ADD(LOG_UINT16, m4, &m4rpm) +LOG_GROUP_STOP(rpm) + From 544c39f63f8808fd5067f5df2242e5ce1e647301 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 26 Feb 2021 13:17:19 +0100 Subject: [PATCH 04/85] loadcell improvements --- Makefile | 4 ++ src/deck/drivers/src/loadcell.c | 61 ++++++++++++---------- src/deck/drivers/src/rpm.c | 5 +- src/hal/src/usec_time.c | 9 ++++ src/modules/interface/sitaw.h | 2 +- src/modules/src/power_distribution_stock.c | 12 ++++- src/utils/src/sleepus.c | 2 - 7 files changed, 58 insertions(+), 37 deletions(-) diff --git a/Makefile b/Makefile index 9aa6d92821..ad52ebc383 100644 --- a/Makefile +++ b/Makefile @@ -35,6 +35,9 @@ include $(CRAZYFLIE_BASE)/tools/make/platform.mk CFLAGS += -DCRAZYFLIE_FW +# Branch specific changes +CFLAGS += -DDECK_FORCE=bcLoadcell:bcRpm + ######### Stabilizer configuration ########## ## These are set by the platform (see tools/make/platforms/*.mk), can be overwritten here ESTIMATOR ?= any @@ -217,6 +220,7 @@ PROJ_OBJ += multiranger.o PROJ_OBJ += lighthouse.o PROJ_OBJ += activeMarkerDeck.o PROJ_OBJ += loadcell.o +PROJ_OBJ += rpm.o # Uart2 Link for CRTP communication is not compatible with decks using uart2 ifeq ($(UART2_LINK), 1) diff --git a/src/deck/drivers/src/loadcell.c b/src/deck/drivers/src/loadcell.c index 6d862c4a6b..dd0114128d 100644 --- a/src/deck/drivers/src/loadcell.c +++ b/src/deck/drivers/src/loadcell.c @@ -38,10 +38,11 @@ #include "deck.h" #include "param.h" #include "log.h" +#include "sleepus.h" -// Hardware defines +// Hardware defines (also update deck driver below!) #define CLK_PIN DECK_GPIO_IO1 -#define DAT_PIN DECK_GPIO_IO2 +#define DAT_PIN DECK_GPIO_IO4 static bool isInit; static int32_t rawWeight; @@ -51,7 +52,7 @@ static bool enable = true; static float a = 4.22852802e-05; static float b = -2.21784688e+01; -static xTimerHandle timer; +static void loadcellTask(void* prm); static enum hx711_gain { @@ -67,12 +68,6 @@ static int32_t hx711_read(void); // static void hx711_power_down(void); static void hx711_power_up(void); -static void delayMicroseconds(uint32_t us) -{ - uint64_t start = usecTimestamp(); - while (usecTimestamp() - start < us); -} - static void hx711_init(void) { pinMode(CLK_PIN, OUTPUT); @@ -114,25 +109,27 @@ static int32_t hx711_read(void) // The result is that all subsequent bits read by shiftIn() will read back as 1, // corrupting the value returned by read(). - portDISABLE_INTERRUPTS(); + // Since we use a high-priority task, we do not need to actually disable the interrupt + + // portDISABLE_INTERRUPTS(); // Pulse the clock pin 24 times to read the data. for (int i = 0; i < 24; ++i) { digitalWrite(CLK_PIN, HIGH); value |= digitalRead(DAT_PIN) << (24 - i); digitalWrite(CLK_PIN, LOW); - delayMicroseconds(1); + sleepus(1); } // Set the channel and the gain factor for the next reading using the clock pin. for (int i = 0; i < gain; ++i) { digitalWrite(CLK_PIN, HIGH); - delayMicroseconds(1); + sleepus(1); digitalWrite(CLK_PIN, LOW); - delayMicroseconds(1); + sleepus(1); } - portENABLE_INTERRUPTS(); + // portENABLE_INTERRUPTS(); // Replicate the most significant bit to pad out a 32-bit signed integer if (value & (1<<24)) { @@ -152,14 +149,6 @@ static void hx711_power_up(void) digitalWrite(CLK_PIN, LOW); } -static void loadcellTimer(xTimerHandle timer) -{ - if (enable && hx711_is_ready()) { - rawWeight = hx711_read(); - weight = a * rawWeight + b; - } -} - static void loadcellInit(DeckInfo *info) { if (isInit) { @@ -169,19 +158,35 @@ static void loadcellInit(DeckInfo *info) gain = GAIN128; hx711_init(); - timer = xTimerCreate( "loadcellTimer", M2T(10), - pdTRUE, NULL, loadcellTimer ); - xTimerStart(timer, 100); + // Create a task with very high priority to reduce risk that we get + // invalid data + xTaskCreate(loadcellTask, "LOADCELL", + configMINIMAL_STACK_SIZE, NULL, + /*priority*/6, NULL); isInit = true; } +static void loadcellTask(void* prm) +{ + TickType_t lastWakeTime = xTaskGetTickCount(); + + while(1) { + vTaskDelayUntil(&lastWakeTime, F2T(100)); + + if (enable && hx711_is_ready()) { + rawWeight = hx711_read(); + weight = a * rawWeight + b; + } + } +} + static const DeckDriver loadcell_deck = { - .vid = 0xBC, - .pid = 0xD0, + .vid = 0x00, + .pid = 0x00, .name = "bcLoadcell", - .usedGpio = DECK_USING_IO_1 | DECK_USING_IO_2, + .usedGpio = DECK_USING_IO_1 | DECK_USING_IO_4, .init = loadcellInit, }; diff --git a/src/deck/drivers/src/rpm.c b/src/deck/drivers/src/rpm.c index 57153b77e4..466da74374 100644 --- a/src/deck/drivers/src/rpm.c +++ b/src/deck/drivers/src/rpm.c @@ -82,10 +82,9 @@ static uint16_t m3rpm; static uint16_t m4rpm; -bool rpmInit(void) +static void rpmInit(DeckInfo *info) { int i; - bool status = true; isInit = true; GPIO_InitTypeDef GPIO_InitStructure; @@ -154,8 +153,6 @@ bool rpmInit(void) TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE); TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE); TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); - - return status; } static uint16_t calcRPM(uint32_t t1, uint32_t t2) diff --git a/src/hal/src/usec_time.c b/src/hal/src/usec_time.c index 6915b72689..ee56c12ae2 100644 --- a/src/hal/src/usec_time.c +++ b/src/hal/src/usec_time.c @@ -25,15 +25,22 @@ * usec_time.c - microsecond-resolution timer and timestamps. */ +#include + #include "usec_time.h" #include "nvicconf.h" #include "stm32fxxx.h" +static bool isInit = false; static uint32_t usecTimerHighCount; void initUsecTimer(void) { + if (isInit) { + return; + } + usecTimerHighCount = 0; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; @@ -61,6 +68,8 @@ void initUsecTimer(void) DBGMCU_APB1PeriphConfig(DBGMCU_TIM7_STOP, ENABLE); TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE); TIM_Cmd(TIM7, ENABLE); + + isInit = true; } uint64_t usecTimestamp(void) diff --git a/src/modules/interface/sitaw.h b/src/modules/interface/sitaw.h index 10b4b390ce..be0fdcf0dd 100644 --- a/src/modules/interface/sitaw.h +++ b/src/modules/interface/sitaw.h @@ -40,7 +40,7 @@ void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData, /* Enable the different functions of the situation awareness framework. */ //#define SITAW_FF_ENABLED /* Uncomment to enable */ //#define SITAW_AR_ENABLED /* Uncomment to enable */ -#define SITAW_TU_ENABLED /* Uncomment to enable */ +//#define SITAW_TU_ENABLED /* Uncomment to enable */ /* Configuration options for the 'Free Fall' detection. */ #define SITAW_FF_THRESHOLD 0.1 /* The default tolerance for AccWZ deviations from -1, indicating Free Fall. */ diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c index 5163d14bf8..1fa98d8702 100644 --- a/src/modules/src/power_distribution_stock.c +++ b/src/modules/src/power_distribution_stock.c @@ -35,7 +35,10 @@ #include "motors.h" #include "debug.h" -static bool motorSetEnable = false; +// 0 - disable +// 1 - individual motor power +// 2 - all motors use m1 variable setting +static uint8_t motorSetEnable = 0; static struct { uint32_t m1; @@ -101,12 +104,17 @@ void powerDistribution(const control_t *control) control->yaw); #endif - if (motorSetEnable) + if (motorSetEnable == 1) { motorsSetRatio(MOTOR_M1, motorPowerSet.m1); motorsSetRatio(MOTOR_M2, motorPowerSet.m2); motorsSetRatio(MOTOR_M3, motorPowerSet.m3); motorsSetRatio(MOTOR_M4, motorPowerSet.m4); + } else if (motorSetEnable == 2) { + motorsSetRatio(MOTOR_M1, motorPowerSet.m1); + motorsSetRatio(MOTOR_M2, motorPowerSet.m1); + motorsSetRatio(MOTOR_M3, motorPowerSet.m1); + motorsSetRatio(MOTOR_M4, motorPowerSet.m1); } else { diff --git a/src/utils/src/sleepus.c b/src/utils/src/sleepus.c index fbae102438..a50c031a49 100644 --- a/src/utils/src/sleepus.c +++ b/src/utils/src/sleepus.c @@ -37,8 +37,6 @@ #include "usec_time.h" -#define TICK_PER_US (FREERTOS_MCU_CLOCK_HZ / (8 * 1e6)) - static bool isInit = false; void sleepus(uint32_t us) From 7cdca480162de2296c00b028ca4f9c1e7a02ea13 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 1 Mar 2021 11:52:52 +0100 Subject: [PATCH 05/85] add nau7802 driver and Python scripts --- Makefile | 3 +- src/deck/drivers/src/loadcell_nau7802.c | 274 ++++++++++++++++++++++++ tools/system_id/README.md | 20 ++ tools/system_id/calibScale.py | 131 +++++++++++ tools/system_id/collectData.py | 182 ++++++++++++++++ tools/system_id/collectDataMaxThrust.py | 200 +++++++++++++++++ tools/system_id/plot_data.py | 21 ++ tools/system_id/system_id.py | 112 ++++++++++ 8 files changed, 942 insertions(+), 1 deletion(-) create mode 100644 src/deck/drivers/src/loadcell_nau7802.c create mode 100644 tools/system_id/README.md create mode 100644 tools/system_id/calibScale.py create mode 100644 tools/system_id/collectData.py create mode 100644 tools/system_id/collectDataMaxThrust.py create mode 100644 tools/system_id/plot_data.py create mode 100644 tools/system_id/system_id.py diff --git a/Makefile b/Makefile index ad52ebc383..00727448d1 100644 --- a/Makefile +++ b/Makefile @@ -219,7 +219,8 @@ PROJ_OBJ += oa.o PROJ_OBJ += multiranger.o PROJ_OBJ += lighthouse.o PROJ_OBJ += activeMarkerDeck.o -PROJ_OBJ += loadcell.o +# PROJ_OBJ += loadcell.o +PROJ_OBJ += loadcell_nau7802.o PROJ_OBJ += rpm.o # Uart2 Link for CRTP communication is not compatible with decks using uart2 diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c new file mode 100644 index 0000000000..ddd3dbf1be --- /dev/null +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -0,0 +1,274 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2021 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * loadcell.c - Deck driver for NAU7802 load cell + * See + * * https://learn.sparkfun.com/tutorials/qwiic-scale-hookup-guide + * * Code based on https://github.com/sparkfun/SparkFun_Qwiic_Scale_NAU7802_Arduino_Library + */ + +#include +#include +#include "nvicconf.h" +#include "stm32fxxx.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "timers.h" + +#include "deck.h" +#include "param.h" +#include "log.h" +#include "i2cdev.h" +#include "sleepus.h" +#include "debug.h" + +#define DECK_I2C_ADDRESS 0x2A + +static bool isInit; +static int32_t rawWeight; +static float weight; +static bool enable = true; + +static float a = 4.22852802e-05; +static float b = -2.21784688e+01; + + +// static void loadcellTask(void* prm); + +//////////////////////////// + +//Register Map +typedef enum +{ + NAU7802_PU_CTRL = 0x00, + NAU7802_CTRL1, + NAU7802_CTRL2, + NAU7802_OCAL1_B2, + NAU7802_OCAL1_B1, + NAU7802_OCAL1_B0, + NAU7802_GCAL1_B3, + NAU7802_GCAL1_B2, + NAU7802_GCAL1_B1, + NAU7802_GCAL1_B0, + NAU7802_OCAL2_B2, + NAU7802_OCAL2_B1, + NAU7802_OCAL2_B0, + NAU7802_GCAL2_B3, + NAU7802_GCAL2_B2, + NAU7802_GCAL2_B1, + NAU7802_GCAL2_B0, + NAU7802_I2C_CONTROL, + NAU7802_ADCO_B2, + NAU7802_ADCO_B1, + NAU7802_ADCO_B0, + NAU7802_ADC = 0x15, //Shared ADC and OTP 32:24 + NAU7802_OTP_B1, //OTP 23:16 or 7:0? + NAU7802_OTP_B0, //OTP 15:8 + NAU7802_PGA = 0x1B, + NAU7802_PGA_PWR = 0x1C, + NAU7802_DEVICE_REV = 0x1F, +} Scale_Registers; + +//Bits within the PU_CTRL register +typedef enum +{ + NAU7802_PU_CTRL_RR = 0, + NAU7802_PU_CTRL_PUD, + NAU7802_PU_CTRL_PUA, + NAU7802_PU_CTRL_PUR, + NAU7802_PU_CTRL_CS, + NAU7802_PU_CTRL_CR, + NAU7802_PU_CTRL_OSCS, + NAU7802_PU_CTRL_AVDDS, +} PU_CTRL_Bits; + +//Bits within the CTRL1 register +typedef enum +{ + NAU7802_CTRL1_GAIN = 2, + NAU7802_CTRL1_VLDO = 5, + NAU7802_CTRL1_DRDY_SEL = 6, + NAU7802_CTRL1_CRP = 7, +} CTRL1_Bits; + +//Bits within the CTRL2 register +typedef enum +{ + NAU7802_CTRL2_CALMOD = 0, + NAU7802_CTRL2_CALS = 2, + NAU7802_CTRL2_CAL_ERROR = 3, + NAU7802_CTRL2_CRS = 4, + NAU7802_CTRL2_CHS = 7, +} CTRL2_Bits; + +//Bits within the PGA register +typedef enum +{ + NAU7802_PGA_CHP_DIS = 0, + NAU7802_PGA_INV = 3, + NAU7802_PGA_BYPASS_EN, + NAU7802_PGA_OUT_EN, + NAU7802_PGA_LDOMODE, + NAU7802_PGA_RD_OTP_SEL, +} PGA_Bits; + +//Bits within the PGA PWR register +typedef enum +{ + NAU7802_PGA_PWR_PGA_CURR = 0, + NAU7802_PGA_PWR_ADC_CURR = 2, + NAU7802_PGA_PWR_MSTR_BIAS_CURR = 4, + NAU7802_PGA_PWR_PGA_CAP_EN = 7, +} PGA_PWR_Bits; + +//Allowed Low drop out regulator voltages +typedef enum +{ + NAU7802_LDO_2V4 = 0b111, + NAU7802_LDO_2V7 = 0b110, + NAU7802_LDO_3V0 = 0b101, + NAU7802_LDO_3V3 = 0b100, + NAU7802_LDO_3V6 = 0b011, + NAU7802_LDO_3V9 = 0b010, + NAU7802_LDO_4V2 = 0b001, + NAU7802_LDO_4V5 = 0b000, +} NAU7802_LDO_Values; + +//Allowed gains +typedef enum +{ + NAU7802_GAIN_128 = 0b111, + NAU7802_GAIN_64 = 0b110, + NAU7802_GAIN_32 = 0b101, + NAU7802_GAIN_16 = 0b100, + NAU7802_GAIN_8 = 0b011, + NAU7802_GAIN_4 = 0b010, + NAU7802_GAIN_2 = 0b001, + NAU7802_GAIN_1 = 0b000, +} NAU7802_Gain_Values; + +//Allowed samples per second +typedef enum +{ + NAU7802_SPS_320 = 0b111, + NAU7802_SPS_80 = 0b011, + NAU7802_SPS_40 = 0b010, + NAU7802_SPS_20 = 0b001, + NAU7802_SPS_10 = 0b000, +} NAU7802_SPS_Values; + +//Select between channel values +typedef enum +{ + NAU7802_CHANNEL_1 = 0, + NAU7802_CHANNEL_2 = 1, +} NAU7802_Channels; + +//Calibration state +typedef enum +{ + NAU7802_CAL_SUCCESS = 0, + NAU7802_CAL_IN_PROGRESS = 1, + NAU7802_CAL_FAILURE = 2, +} NAU7802_Cal_Status; + +bool nau7802_reset() +{ + bool result = true; + result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 1); + sleepus(1); + result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 0); + return result; +} + +bool nau7802_revision(uint8_t* revision) +{ + return i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_DEVICE_REV, revision); +} + +///////////////////////////// + + +static void loadcellInit(DeckInfo *info) +{ + if (isInit) { + return; + } + + isInit = true; + + i2cdevInit(I2C1_DEV); + + // isInit &= nau7802_reset(); + uint8_t revision; + isInit &= nau7802_revision(&revision); + DEBUG_PRINT("bla %d %d\n", isInit, revision); + + // // Create a task with very high priority to reduce risk that we get + // // invalid data + // xTaskCreate(loadcellTask, "LOADCELL", + // configMINIMAL_STACK_SIZE, NULL, + // /*priority*/6, NULL); + + // isInit = true; +} + +// static void loadcellTask(void* prm) +// { +// TickType_t lastWakeTime = xTaskGetTickCount(); + +// while(1) { +// vTaskDelayUntil(&lastWakeTime, F2T(100)); + +// if (enable && hx711_is_ready()) { +// rawWeight = hx711_read(); +// weight = a * rawWeight + b; +// } +// } +// } + +static const DeckDriver loadcell_deck = { + .vid = 0x00, + .pid = 0x00, + .name = "bcLoadcell", + + .init = loadcellInit, +}; + +DECK_DRIVER(loadcell_deck); + +PARAM_GROUP_START(deck) +PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit) +PARAM_GROUP_STOP(deck) + +PARAM_GROUP_START(loadcell) +PARAM_ADD(PARAM_UINT8, enable, &enable) +PARAM_ADD(PARAM_FLOAT, a, &a) +PARAM_ADD(PARAM_FLOAT, b, &b) +PARAM_GROUP_STOP(loadcell) + +LOG_GROUP_START(loadcell) +LOG_ADD(LOG_INT32, rawWeight, &rawWeight) +LOG_ADD(LOG_FLOAT, weight, &weight) +LOG_GROUP_STOP(loadcell) diff --git a/tools/system_id/README.md b/tools/system_id/README.md new file mode 100644 index 0000000000..4cbad6541a --- /dev/null +++ b/tools/system_id/README.md @@ -0,0 +1,20 @@ +# System ID for PWM <-> Thrust + + +## Setup + +* 100g Mini Load Cell, https://www.sparkfun.com/products/14727 +* Load Cell Amplifier HX711, https://learn.sparkfun.com/tutorials/load-cell-amplifier-hx711-breakout-hookup-guide +* Prototype deck, https://www.bitcraze.io/prototyping-deck/ +* Calibration weights +* M2/M3 Nylon screws +* CF Mount (3D printed) + +Mount CF upside down (to avoid downwash) + +## Measurement + +1. Mount CF +2. Flash firmware from dev-systemId branch +3. Run `python3 collectData.py` (includes prompts to calibrate using calibration weights and writes data.csv) +4. Evaluate using `python3 system_id.py` diff --git a/tools/system_id/calibScale.py b/tools/system_id/calibScale.py new file mode 100644 index 0000000000..f93a9926f8 --- /dev/null +++ b/tools/system_id/calibScale.py @@ -0,0 +1,131 @@ +""" +Example that uses a load cell to measure thrust using different RPMs +""" +import logging +import time +# from threading import Timer +# from threading import Thread + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig + +import numpy as np +import matplotlib.pyplot as plt + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + +class CalibScale: + def __init__(self, link_uri): + """ Initialize and run with the specified link_uri """ + + self.measurements = [] + + self._cf = Crazyflie(rw_cache='./cache') + + # Connect some callbacks from the Crazyflie API + self._cf.connected.add_callback(self._connected) + self._cf.disconnected.add_callback(self._disconnected) + self._cf.connection_failed.add_callback(self._connection_failed) + self._cf.connection_lost.add_callback(self._connection_lost) + + print('Connecting to %s' % link_uri) + + # Try to connect to the Crazyflie + self._cf.open_link(link_uri) + + # Variable used to keep main loop occupied until disconnect + self.is_connected = False + + def _connected(self, link_uri): + """ This callback is called form the Crazyflie API when a Crazyflie + has been connected and the TOCs have been downloaded.""" + print('Connected to %s' % link_uri) + + # The definition of the logconfig can be made before connecting + self._lg_stab = LogConfig(name='data', period_in_ms=10) + self._lg_stab.add_variable('loadcell.weight', 'float') + self._lg_stab.add_variable('loadcell.rawWeight', 'int32_t') + + # Adding the configuration cannot be done until a Crazyflie is + # connected, since we need to check that the variables we + # would like to log are in the TOC. + try: + self._cf.log.add_config(self._lg_stab) + # This callback will receive the data + self._lg_stab.data_received_cb.add_callback(self._stab_log_data) + # This callback will be called on errors + self._lg_stab.error_cb.add_callback(self._stab_log_error) + # Start the logging + self._lg_stab.start() + except KeyError as e: + print('Could not start log configuration,' + '{} not found in TOC'.format(str(e))) + except AttributeError: + print('Could not add Stabilizer log config, bad configuration.') + + self.is_connected = True + + def _stab_log_error(self, logconf, msg): + """Callback from the log API when an error occurs""" + print('Error when logging %s: %s' % (logconf.name, msg)) + + def _stab_log_data(self, timestamp, data, logconf): + """Callback froma the log API when data arrives""" + # print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + self.measurements.append(data['loadcell.rawWeight']) + + def _connection_failed(self, link_uri, msg): + """Callback when connection initial connection fails (i.e no Crazyflie + at the speficied address)""" + print('Connection to %s failed: %s' % (link_uri, msg)) + self.is_connected = False + + def _connection_lost(self, link_uri, msg): + """Callback when disconnected after a connection has been made (i.e + Crazyflie moves out of range)""" + print('Connection to %s lost: %s' % (link_uri, msg)) + + def _disconnected(self, link_uri): + """Callback when the Crazyflie is disconnected (called in all cases)""" + print('Disconnected from %s' % link_uri) + self.is_connected = False + + def measure(self, num_measurements = 100): + weights = [] + readings = [] + + while True: + data = float(input("enter weight in grams (-1 to end): ")) + if data < 0: + break + self.measurements = [] + while len(self.measurements) < num_measurements: + time.sleep(0.1) + print(np.mean(self.measurements)) + weights.append(data) + readings.append(np.mean(self.measurements)) + + self._cf.close_link() + + z = np.polyfit(readings, weights, 1) + p = np.poly1d(z) + + xp = np.linspace(readings[0], readings[-1], 100) + + + plt.plot(readings, weights, '.', xp, p(xp), '--') + plt.show() + return float(z[0]), float(z[1]) + + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + le = CalibScale("radio://0/42/2M/E7E7E7E7E7") + + while not le.is_connected: + time.sleep(0.1) + + print(le.measure()) diff --git a/tools/system_id/collectData.py b/tools/system_id/collectData.py new file mode 100644 index 0000000000..f9f66333f7 --- /dev/null +++ b/tools/system_id/collectData.py @@ -0,0 +1,182 @@ +""" +Example that uses a load cell to measure thrust using different RPMs +""" +import logging +import time +from threading import Timer +from threading import Thread + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.localization import Localization + +import calibScale + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + + +class CollectData: + """ + Simple logging example class that logs the Stabilizer from a supplied + link uri and disconnects after 5s. + """ + + def __init__(self, link_uri, calib_a, calib_b): + """ Initialize and run the example with the specified link_uri """ + self.calib_a = calib_a + self.calib_b = calib_b + + self._cf = Crazyflie(rw_cache='./cache') + + # Connect some callbacks from the Crazyflie API + self._cf.connected.add_callback(self._connected) + self._cf.disconnected.add_callback(self._disconnected) + self._cf.connection_failed.add_callback(self._connection_failed) + self._cf.connection_lost.add_callback(self._connection_lost) + + print('Connecting to %s' % link_uri) + + # Try to connect to the Crazyflie + self._cf.open_link(link_uri) + + # Variable used to keep main loop occupied until disconnect + self.is_connected = True + + def _connected(self, link_uri): + """ This callback is called form the Crazyflie API when a Crazyflie + has been connected and the TOCs have been downloaded.""" + print('Connected to %s' % link_uri) + + print(self.calib_a, self.calib_b) + self._cf.param.set_value('loadcell.a', str(self.calib_a)) + self._cf.param.set_value('loadcell.b', str(self.calib_b)) + + self._file = open("data.csv", "w+") + self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4\n"); + + # The definition of the logconfig can be made before connecting + self._lg_stab = LogConfig(name='data', period_in_ms=10) + self._lg_stab.add_variable('loadcell.weight', 'float') + self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') + self._lg_stab.add_variable('pm.vbat', 'float') + self._lg_stab.add_variable('rpm.m1', 'uint16_t') + self._lg_stab.add_variable('rpm.m2', 'uint16_t') + self._lg_stab.add_variable('rpm.m3', 'uint16_t') + self._lg_stab.add_variable('rpm.m4', 'uint16_t') + + # Adding the configuration cannot be done until a Crazyflie is + # connected, since we need to check that the variables we + # would like to log are in the TOC. + try: + self._cf.log.add_config(self._lg_stab) + # This callback will receive the data + self._lg_stab.data_received_cb.add_callback(self._stab_log_data) + # This callback will be called on errors + self._lg_stab.error_cb.add_callback(self._stab_log_error) + # Start the logging + self._lg_stab.start() + except KeyError as e: + print('Could not start log configuration,' + '{} not found in TOC'.format(str(e))) + except AttributeError: + print('Could not add Stabilizer log config, bad configuration.') + + # Start a separate thread to do the motor test. + # Do not hijack the calling thread! + Thread(target=self._ramp_motors).start() + + def _stab_log_error(self, logconf, msg): + """Callback from the log API when an error occurs""" + print('Error when logging %s: %s' % (logconf.name, msg)) + + def _stab_log_data(self, timestamp, data, logconf): + """Callback froma the log API when data arrives""" + print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + self._file.write("{},{},{},{},{},{},{}\n".format( + data['loadcell.weight'], + data['pwm.m1_pwm'], + data['pm.vbat'], + data['rpm.m1'], + data['rpm.m2'], + data['rpm.m3'], + data['rpm.m4'])) + + def _connection_failed(self, link_uri, msg): + """Callback when connection initial connection fails (i.e no Crazyflie + at the speficied address)""" + print('Connection to %s failed: %s' % (link_uri, msg)) + self.is_connected = False + + def _connection_lost(self, link_uri, msg): + """Callback when disconnected after a connection has been made (i.e + Crazyflie moves out of range)""" + print('Connection to %s lost: %s' % (link_uri, msg)) + + def _disconnected(self, link_uri): + """Callback when the Crazyflie is disconnected (called in all cases)""" + print('Disconnected from %s' % link_uri) + self.is_connected = False + + def _ramp_motors(self): + thrust_mult = 1 + thrust_step = 100 + time_step = 0.1 + thrust = 0 + pitch = 0 + roll = 0 + yawrate = 0 + + # # Unlock startup thrust protection + # for i in range(0, 100): + # self._cf.commander.send_setpoint(0, 0, 0, 0) + + localization = Localization(self._cf) + + self._cf.param.set_value('motor.batCompensation', 0) + self._cf.param.set_value('motorPowerSet.m1', 0) + self._cf.param.set_value('motorPowerSet.enable', 2) + + while self.is_connected: #thrust >= 0: + thrust += thrust_step * thrust_mult + if thrust >= 65536 or thrust < 0: + thrust_mult *= -1 + thrust += thrust_step * thrust_mult + print(thrust) + # self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) + localization.send_emergency_stop_watchdog() + self._cf.param.set_value('motorPowerSet.m1', str(thrust)) + time.sleep(time_step) + + # self._cf.commander.send_setpoint(0, 0, 0, 0) + # Make sure that the last packet leaves before the link is closed + # since the message queue is not flushed before closing + # time.sleep(0.1) + # self._cf.close_link() + + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + uri = "radio://0/42/2M/E7E7E7E7E7" + + # calibrate the scale + le = calibScale.CalibScale(uri) + + while not le.is_connected: + time.sleep(0.1) + + a, b = le.measure() + + # a,b = 4.046543357726483e-05, -29.212331441036078 + + # collect data + le = CollectData(uri, a, b) + + # The Crazyflie lib doesn't contain anything to keep the application alive, + # so this is where your application should do something. In our case we + # are just waiting until we are disconnected. + while le.is_connected: + time.sleep(1) diff --git a/tools/system_id/collectDataMaxThrust.py b/tools/system_id/collectDataMaxThrust.py new file mode 100644 index 0000000000..14841da46f --- /dev/null +++ b/tools/system_id/collectDataMaxThrust.py @@ -0,0 +1,200 @@ +""" +Example that uses a load cell to measure thrust using different RPMs +""" +import logging +import time +from threading import Timer +from threading import Thread +import numpy as np + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.localization import Localization + +import calibScale + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + + +class CollectData: + """ + Simple logging example class that logs the Stabilizer from a supplied + link uri and disconnects after 5s. + """ + + def __init__(self, link_uri, calib_a, calib_b): + """ Initialize and run the example with the specified link_uri """ + self.measurements = [] + self.desiredThrust = 0 + self.calib_a = calib_a + self.calib_b = calib_b + + self._cf = Crazyflie(rw_cache='./cache') + + # Connect some callbacks from the Crazyflie API + self._cf.connected.add_callback(self._connected) + self._cf.disconnected.add_callback(self._disconnected) + self._cf.connection_failed.add_callback(self._connection_failed) + self._cf.connection_lost.add_callback(self._connection_lost) + + print('Connecting to %s' % link_uri) + + # Try to connect to the Crazyflie + self._cf.open_link(link_uri) + + # Variable used to keep main loop occupied until disconnect + self.is_connected = True + + def _connected(self, link_uri): + """ This callback is called form the Crazyflie API when a Crazyflie + has been connected and the TOCs have been downloaded.""" + print('Connected to %s' % link_uri) + + print(self.calib_a, self.calib_b) + self._cf.param.set_value('loadcell.a', str(self.calib_a)) + self._cf.param.set_value('loadcell.b', str(self.calib_b)) + + self._file = open("data.csv", "w+") + self._file.write("thrust[g],pwm,vbat[V],maxThrust[g],maxThrustVbat[V]\n"); + + # The definition of the logconfig can be made before connecting + self._lg_stab = LogConfig(name='data', period_in_ms=10) + self._lg_stab.add_variable('loadcell.weight', 'float') + self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') + self._lg_stab.add_variable('pm.vbat', 'float') + self._lg_stab.add_variable('rpm.m1', 'uint16_t') + self._lg_stab.add_variable('rpm.m2', 'uint16_t') + self._lg_stab.add_variable('rpm.m3', 'uint16_t') + self._lg_stab.add_variable('rpm.m4', 'uint16_t') + + # Adding the configuration cannot be done until a Crazyflie is + # connected, since we need to check that the variables we + # would like to log are in the TOC. + try: + self._cf.log.add_config(self._lg_stab) + # This callback will receive the data + self._lg_stab.data_received_cb.add_callback(self._stab_log_data) + # This callback will be called on errors + self._lg_stab.error_cb.add_callback(self._stab_log_error) + # Start the logging + self._lg_stab.start() + except KeyError as e: + print('Could not start log configuration,' + '{} not found in TOC'.format(str(e))) + except AttributeError: + print('Could not add Stabilizer log config, bad configuration.') + + self._localization = Localization(self._cf) + + # Start a separate thread to do the motor test. + # Do not hijack the calling thread! + Thread(target=self._ramp_motors).start() + + # # Start a timer to disconnect in 10s + # t = Timer(5, self._cf.close_link) + # t.start() + + def _stab_log_error(self, logconf, msg): + """Callback from the log API when an error occurs""" + print('Error when logging %s: %s' % (logconf.name, msg)) + + def _stab_log_data(self, timestamp, data, logconf): + """Callback froma the log API when data arrives""" + # print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + # self._file.write("{},{},{}\n".format(data['loadcell.weight'], data['pwm.m1_pwm'], data['pm.vbat'])) + if self.desiredThrust == data['pwm.m1_pwm']: + self.measurements.append(np.array([data['loadcell.weight'], data['pm.vbat']])) + # pass + + def _connection_failed(self, link_uri, msg): + """Callback when connection initial connection fails (i.e no Crazyflie + at the speficied address)""" + print('Connection to %s failed: %s' % (link_uri, msg)) + self.is_connected = False + + def _connection_lost(self, link_uri, msg): + """Callback when disconnected after a connection has been made (i.e + Crazyflie moves out of range)""" + print('Connection to %s lost: %s' % (link_uri, msg)) + + def _disconnected(self, link_uri): + """Callback when the Crazyflie is disconnected (called in all cases)""" + print('Disconnected from %s' % link_uri) + self.is_connected = False + + def _measure(self, thrust, min_samples = 50): + self.desiredThrust = thrust + self.measurements = [] + self._cf.param.set_value('motorPowerSet.m1', str(thrust)) + while len(self.measurements) < min_samples: + self._localization.send_emergency_stop_watchdog() + time.sleep(0.1) + m = np.array(self.measurements) + # only return the last few samples + return m[40:] + + def _ramp_motors(self): + + self._cf.param.set_value('motor.batCompensation', 0) + self._cf.param.set_value('motorPowerSet.m1', 0) + self._cf.param.set_value('motorPowerSet.enable', 2) + + while self.is_connected: #thrust >= 0: + # randomply sample PWM + thrust = int(np.random.uniform(30000, 65535)) + measurements = self._measure(thrust) + # print(measurements) + achievedThrust = np.mean(measurements[:,0]) + vbat = np.mean(measurements[:,1]) + + # go to full thrust + measurements = self._measure(65535) + # print(measurements) + maxThrust = np.mean(measurements[:,0]) + vbatAtMaxThrust = np.mean(measurements[:,1]) + + # write result + print(achievedThrust, thrust, vbat, maxThrust, vbatAtMaxThrust) + self._file.write("{},{},{},{},{}\n".format( + achievedThrust, + thrust, + vbat, + maxThrust, + vbatAtMaxThrust)) + + if vbatAtMaxThrust < 2.8: + break + + # self._cf.commander.send_setpoint(0, 0, 0, 0) + # Make sure that the last packet leaves before the link is closed + # since the message queue is not flushed before closing + time.sleep(0.1) + self._cf.close_link() + self._file.close() + + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + uri = "radio://0/42/2M/E7E7E7E7E7" + + # calibrate the scale + le = calibScale.CalibScale(uri) + + while not le.is_connected: + time.sleep(0.1) + + a, b = le.measure() + # a,b = 4.046543357726483e-05, -29.212331441036078 + + # collect data + le = CollectData(uri, a, b) + + # The Crazyflie lib doesn't contain anything to keep the application alive, + # so this is where your application should do something. In our case we + # are just waiting until we are disconnected. + while le.is_connected: + time.sleep(1) diff --git a/tools/system_id/plot_data.py b/tools/system_id/plot_data.py new file mode 100644 index 0000000000..eeb8add62c --- /dev/null +++ b/tools/system_id/plot_data.py @@ -0,0 +1,21 @@ +import numpy as np +import matplotlib.pyplot as plt + +def loadFile(filename): + fileData = np.loadtxt(filename, delimiter=',', skiprows=1, ndmin=2) + return fileData + +if __name__ == '__main__': + data = loadFile("data.csv") + + thrust = data[:,0] / 4 # g, per motor + pwm = data[:,1] # PWM value + vbat = data[:,2] # V, battery voltage, + rpm = np.mean(data[:,3:7],axis=1) # average over all motors + + fig,ax = plt.subplots() + ax.plot(rpm, thrust) + ax.set_xlabel('RPM') + ax.set_ylabel('Thrust [g]') + + plt.show() diff --git a/tools/system_id/system_id.py b/tools/system_id/system_id.py new file mode 100644 index 0000000000..8a00263e54 --- /dev/null +++ b/tools/system_id/system_id.py @@ -0,0 +1,112 @@ +import numpy as np +from sklearn.linear_model import LinearRegression +import matplotlib.pyplot as plt + +def loadFiles(filenames): + if len(filenames) == 0: + return None + + Data = np.empty((0,5)) + for filename in filenames: + fileData = np.loadtxt(filename, delimiter=',', skiprows=1) + lastValidIdx = np.argwhere( (fileData[:,2] < 3.7) & (fileData[:,1] == 0)) + if len(lastValidIdx) > 0: + lastValidIdx = lastValidIdx[0][0] + else: + lastValidIdx = -1 + print(lastValidIdx) + fileData = fileData[0:lastValidIdx] + print(fileData) + Data = np.vstack((Data, fileData)) + + filteredData = Data[ (Data[:,1] >= 8000) & (Data[:,1] <= 0.96 * 65536) ] + + X = filteredData[:, 1] / 65536 # PWM + Y = filteredData[:, 2] / 4.2 # Vol + Z = filteredData[:, 0] / 4 # Thrust (gram) + maxThrust = filteredData[:, 3] / 4 + + return X, Y, Z, maxThrust + +def system_id(filenames, validations=[]): + X, Y, Z, maxThrust = loadFiles(filenames) + if len(validations) > 0: + X_val, Y_val, Z_val, maxThrust_val = loadFiles(validations) + + plt.scatter(X, Z, label="training data") + plt.xlabel("PWM") + plt.ylabel("Thrust") + + if len(validations) > 0: + plt.scatter(X_val, Z_val, label="validation data") + plt.xlabel("PWM") + plt.ylabel("Thrust") + plt.legend() + plt.show() + + Input = np.zeros((X.shape[0], 4)) + Input[:, 0] = X + Input[:, 1] = Y + Input[:, 2] = X ** 2 + Input[:, 3] = X * Y + + if len(validations) > 0: + Input_val = np.zeros((X_val.shape[0], 4)) + Input_val[:, 0] = X_val + Input_val[:, 1] = Y_val + Input_val[:, 2] = X_val ** 2 + Input_val[:, 3] = X_val * Y_val + + reg = LinearRegression().fit(Input, Z) + print("******************************") + print("C_00 = " + str(reg.intercept_)) + print("C_10 = " + str(reg.coef_[0])) + print("C_01 = " + str(reg.coef_[1])) + print("C_20 = " + str(reg.coef_[2])) + print("C_11 = " + str(reg.coef_[3])) + print("score = " + str(reg.score(Input, Z))) + if len(validations) > 0: + Error = reg.predict(Input_val) - Z_val.reshape(X_val.shape[0]) + print("validation score = " + str(reg.score(Input_val, Z_val))) + print("validation max error = " + str(np.max(np.abs(Error))) + " gram") + print("validation mean error = " + str(np.mean(np.abs(Error))) + " gram") + + Input = np.zeros((X.shape[0], 4)) + Input[:, 0] = Z + Input[:, 1] = Y + Input[:, 2] = Z ** 2 + Input[:, 3] = Z * Y + + if len(validations) > 0: + Input_val = np.zeros((X_val.shape[0], 4)) + Input_val[:, 0] = Z_val + Input_val[:, 1] = Y_val + Input_val[:, 2] = Z_val ** 2 + Input_val[:, 3] = Z_val * Y_val + + reg = LinearRegression().fit(Input, X) + print("******************************") + print("d00: " + str(reg.intercept_)) + print("d10: " + str(reg.coef_[0])) + print("d01: " + str(reg.coef_[1])) + print("d20: " + str(reg.coef_[2])) + print("d11: " + str(reg.coef_[3])) + print("score = " + str(reg.score(Input, X))) + if len(validations) > 0: + Error = reg.predict(Input_val) - X_val.reshape(X_val.shape[0]) + print("validation score = " + str(reg.score(Input_val, X_val))) + print("validation max error = " + str(np.max(np.abs(Error))) + " pwm") + print("validation mean error = " + str(np.mean(np.abs(Error))) + " pwm") + + Input = np.zeros((X.shape[0], 2)) + Input[:, 0] = X + Input[:, 1] = Y + + reg = LinearRegression().fit(Input, maxThrust) + print("******************************") + print("e00: " + str(reg.intercept_)) + print("e10: " + str(reg.coef_[0])) + print("e01: " + str(reg.coef_[1])) + print("score = " + str(reg.score(Input, maxThrust))) + +system_id(["cf3_1.csv", "cf3_2.csv"], validations=["cf3_1.csv", "cf3_2.csv"]) From ebcd9ff3cd2e14645b424d2e892e837985b99db0 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 5 Mar 2021 09:59:05 +0100 Subject: [PATCH 06/85] loadcell drv: do not call i2cdevInit --- src/deck/drivers/src/loadcell_nau7802.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c index ddd3dbf1be..bad0f03210 100644 --- a/src/deck/drivers/src/loadcell_nau7802.c +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -218,13 +218,13 @@ static void loadcellInit(DeckInfo *info) isInit = true; - i2cdevInit(I2C1_DEV); - + sleepus(1000 * 1000); // isInit &= nau7802_reset(); uint8_t revision; isInit &= nau7802_revision(&revision); DEBUG_PRINT("bla %d %d\n", isInit, revision); + // // Create a task with very high priority to reduce risk that we get // // invalid data // xTaskCreate(loadcellTask, "LOADCELL", From d4d872535915bc8e3b6bf752be81f7ef79aa93c8 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Fri, 5 Mar 2021 11:21:56 +0100 Subject: [PATCH 07/85] Re-arranged eeprom connecton tests that failed and didn't make sense. --- Makefile | 4 ++-- src/utils/src/configblockeeprom.c | 6 +----- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/Makefile b/Makefile index 9c23eb9c92..d66e2f084b 100644 --- a/Makefile +++ b/Makefile @@ -36,7 +36,7 @@ include $(CRAZYFLIE_BASE)/tools/make/platform.mk CFLAGS += -DCRAZYFLIE_FW # Branch specific changes -CFLAGS += -DDECK_FORCE=bcLoadcell:bcRpm +CFLAGS += -DDECK_FORCE=bcLoadcell ######### Stabilizer configuration ########## ## These are set by the platform (see tools/make/platforms/*.mk), can be overwritten here @@ -221,7 +221,7 @@ PROJ_OBJ += lighthouse.o PROJ_OBJ += activeMarkerDeck.o # PROJ_OBJ += loadcell.o PROJ_OBJ += loadcell_nau7802.o -PROJ_OBJ += rpm.o +#PROJ_OBJ += rpm.o # Uart2 Link for CRTP communication is not compatible with decks using uart2 ifeq ($(UART2_LINK), 1) diff --git a/src/utils/src/configblockeeprom.c b/src/utils/src/configblockeeprom.c index 950bd332b5..4f363a169d 100644 --- a/src/utils/src/configblockeeprom.c +++ b/src/utils/src/configblockeeprom.c @@ -124,10 +124,6 @@ int configblockInit(void) eepromInit(I2C1_DEV); - // Because of strange behavior from I2C device during expansion port test - // the first read needs to be discarded - eepromTestConnection(); - if (eepromTestConnection()) { if (eepromReadBuffer((uint8_t *)&configblock, 0, sizeof(configblock))) @@ -197,7 +193,7 @@ int configblockInit(void) bool configblockTest(void) { - return eepromTest(); + return cb_ok; } static bool configblockCheckMagic(configblock_t *configblock) From 31ded2c517188ea6ad9ccbb193f8b4f1ff93769e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 5 Mar 2021 14:52:47 +0100 Subject: [PATCH 08/85] initial nau7802 device driver (untested) --- src/deck/drivers/src/loadcell_nau7802.c | 172 ++++++++++++++++++++---- 1 file changed, 149 insertions(+), 23 deletions(-) diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c index bad0f03210..ce3368f21c 100644 --- a/src/deck/drivers/src/loadcell_nau7802.c +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -54,7 +54,7 @@ static float a = 4.22852802e-05; static float b = -2.21784688e+01; -// static void loadcellTask(void* prm); +static void loadcellTask(void* prm); //////////////////////////// @@ -193,6 +193,7 @@ typedef enum NAU7802_CAL_FAILURE = 2, } NAU7802_Cal_Status; +//Resets all registers to Power Of Defaults bool nau7802_reset() { bool result = true; @@ -202,9 +203,119 @@ bool nau7802_reset() return result; } +//Power up digital and analog sections of scale +bool nau7802_powerUp() +{ + bool result = true; + result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUD, 1); + result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUA, 1); + + //Wait for Power Up bit to be set - takes approximately 200us + for (int i = 0; i < 200; ++i) { + uint8_t bit; + result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUR, &bit); + if (result && bit) { + return true; + } + sleepus(1); + } + return false; +} + +//Set the onboard Low-Drop-Out voltage regulator to a given value +bool nau7802_setLDO(NAU7802_LDO_Values ldoValue) +{ + bool result = true; + //Set the value of the LDO + result &= i2cdevWriteBits(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, 3, 3, ldoValue); + //Enable the internal LDO + result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_AVDDS, 1); + return result; +} + +//Set the gain +bool nau7802_setGain(NAU7802_Gain_Values gainValue) +{ + bool result = true; + result &= i2cdevWriteBits(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, 0, 3, gainValue); + return result; +} + +//Set the readings per second +bool nau7802_setSampleRate(NAU7802_SPS_Values rate) +{ + bool result = true; + result &= i2cdevWriteBits(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, 4, 3, rate); + return result; +} + +//Check calibration status. +NAU7802_Cal_Status nau7802_calAFEStatus() +{ + bool result = true; + uint8_t bit; + result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CALS, &bit); + if (result && bit) { + return NAU7802_CAL_IN_PROGRESS; + } + result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CAL_ERROR, &bit); + if (result && bit) { + return NAU7802_CAL_FAILURE; + } + + if (result) { + // Calibration passed + return NAU7802_CAL_SUCCESS; + } + return NAU7802_CAL_FAILURE; +} + +//Calibrate analog front end of system. Returns true if CAL_ERR bit is 0 (no error) +//Takes approximately 344ms to calibrate; wait up to 1000ms. +//It is recommended that the AFE be re-calibrated any time the gain, SPS, or channel number is changed. +bool nau7802_calibrateAFE() +{ + bool result = true; + //Begin asynchronous calibration of the analog front end. + result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CALS, 1); + + for (int i = 0; i < 1000; ++i) { + if (nau7802_calAFEStatus() == NAU7802_CAL_SUCCESS) { + return true; + } + vTaskDelay(M2T(1)); + } + return false; +} + +//Returns true if Cycle Ready bit is set (conversion is complete) +bool nau7802_hasMeasurement() +{ + bool result = true; + uint8_t bit; + result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_CR, &bit); + return result && bit; +} + +//Returns 24-bit reading +//Assumes CR Cycle Ready bit (ADC conversion complete) has been checked to be 1 +bool nau7802_getMeasurement(int32_t* measurement) +{ + bool result = true; + uint32_t valueRaw; + result &= i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADCO_B2, 3, (uint8_t*)&valueRaw); + // recover the sign + int32_t valueShifted = (int32_t)(valueRaw << 8); + *measurement = (valueShifted >> 8); + return result; +} + bool nau7802_revision(uint8_t* revision) { - return i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_DEVICE_REV, revision); + bool result = true; + result &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_DEVICE_REV, revision); + *revision = *revision & 0x0F; + return result; } ///////////////////////////// @@ -216,37 +327,52 @@ static void loadcellInit(DeckInfo *info) return; } - isInit = true; - sleepus(1000 * 1000); + // sleepus(1000 * 1000); // isInit &= nau7802_reset(); - uint8_t revision; - isInit &= nau7802_revision(&revision); - DEBUG_PRINT("bla %d %d\n", isInit, revision); + // uint8_t revision; + // isInit &= nau7802_revision(&revision); + // DEBUG_PRINT("bla %d %d\n", isInit, revision); + + if (true) { + + isInit &= nau7802_reset(); + isInit &= nau7802_powerUp(); + isInit &= nau7802_setLDO(NAU7802_LDO_3V3); + isInit &= nau7802_setGain(NAU7802_GAIN_128); + isInit &= nau7802_setSampleRate(NAU7802_SPS_80); + // Turn off CLK_CHP. From 9.1 power on sequencing. + isInit &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADC, 0x30); + // Enable 330pF decoupling cap on chan 2. From 9.14 application circuit note. + isInit &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PGA_PWR, NAU7802_PGA_PWR_PGA_CAP_EN, 1); + isInit &= nau7802_calibrateAFE(); + } - // // Create a task with very high priority to reduce risk that we get - // // invalid data - // xTaskCreate(loadcellTask, "LOADCELL", - // configMINIMAL_STACK_SIZE, NULL, - // /*priority*/6, NULL); + if (isInit) { + // Create a task + xTaskCreate(loadcellTask, "LOADCELL", + configMINIMAL_STACK_SIZE, NULL, + /*priority*/0, NULL); + } + isInit = true; // isInit = true; } -// static void loadcellTask(void* prm) -// { -// TickType_t lastWakeTime = xTaskGetTickCount(); +static void loadcellTask(void* prm) +{ + TickType_t lastWakeTime = xTaskGetTickCount(); -// while(1) { -// vTaskDelayUntil(&lastWakeTime, F2T(100)); + while(1) { + vTaskDelayUntil(&lastWakeTime, F2T(100)); -// if (enable && hx711_is_ready()) { -// rawWeight = hx711_read(); -// weight = a * rawWeight + b; -// } -// } -// } + if (enable && nau7802_hasMeasurement()) { + nau7802_getMeasurement(&rawWeight); + weight = a * rawWeight + b; + } + } +} static const DeckDriver loadcell_deck = { .vid = 0x00, From bf9eb042a51ae07bc32a420a97ba55a31360fdea Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Fri, 5 Mar 2021 16:41:48 +0100 Subject: [PATCH 09/85] I2C @ 100k and small change to loadcell driver. --- src/deck/drivers/src/loadcell_nau7802.c | 36 +++++++++++++++++-------- src/drivers/src/i2c_drv.c | 2 +- 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c index ce3368f21c..c1a82891a2 100644 --- a/src/deck/drivers/src/loadcell_nau7802.c +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -207,8 +207,11 @@ bool nau7802_reset() bool nau7802_powerUp() { bool result = true; - result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUD, 1); - result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUA, 1); + + result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, 0x06); //(PU analog and PU digital) + +// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUD, 1); +// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUA, 1); //Wait for Power Up bit to be set - takes approximately 200us for (int i = 0; i < 200; ++i) { @@ -302,11 +305,16 @@ bool nau7802_hasMeasurement() bool nau7802_getMeasurement(int32_t* measurement) { bool result = true; - uint32_t valueRaw; + + uint8_t valueRaw[3]; result &= i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADCO_B2, 3, (uint8_t*)&valueRaw); // recover the sign - int32_t valueShifted = (int32_t)(valueRaw << 8); - *measurement = (valueShifted >> 8); + int32_t valueShifted = (int32_t)(valueRaw[2] << 16) | + (valueRaw[1] << 8) | + (valueRaw[0] << 0); + if ((int8_t)valueRaw[2] < 0) *measurement = -valueShifted; + else *measurement = valueShifted; + return result; } @@ -327,6 +335,7 @@ static void loadcellInit(DeckInfo *info) return; } + isInit = true; // sleepus(1000 * 1000); // isInit &= nau7802_reset(); @@ -337,27 +346,32 @@ static void loadcellInit(DeckInfo *info) if (true) { isInit &= nau7802_reset(); + DEBUG_PRINT("Reset [%d]\n", isInit); isInit &= nau7802_powerUp(); - isInit &= nau7802_setLDO(NAU7802_LDO_3V3); + DEBUG_PRINT("Power up [%d]\n", isInit); + isInit &= nau7802_setLDO(NAU7802_LDO_2V7); + DEBUG_PRINT("LDO [%d]\n", isInit); isInit &= nau7802_setGain(NAU7802_GAIN_128); + DEBUG_PRINT("Gain [%d]\n", isInit); isInit &= nau7802_setSampleRate(NAU7802_SPS_80); + DEBUG_PRINT("Sample rate [%d]\n", isInit); // Turn off CLK_CHP. From 9.1 power on sequencing. isInit &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADC, 0x30); + DEBUG_PRINT("CLK_CHP [%d]\n", isInit); // Enable 330pF decoupling cap on chan 2. From 9.14 application circuit note. isInit &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PGA_PWR, NAU7802_PGA_PWR_PGA_CAP_EN, 1); + DEBUG_PRINT("CAP [%d]\n", isInit); isInit &= nau7802_calibrateAFE(); + DEBUG_PRINT("Cal [%d]\n", isInit); } - if (isInit) { + if (1) { // Create a task xTaskCreate(loadcellTask, "LOADCELL", configMINIMAL_STACK_SIZE, NULL, - /*priority*/0, NULL); + /*priority*/1, NULL); } - - isInit = true; - // isInit = true; } static void loadcellTask(void* prm) diff --git a/src/drivers/src/i2c_drv.c b/src/drivers/src/i2c_drv.c index d6a9b4bc41..23a9c6d9e0 100644 --- a/src/drivers/src/i2c_drv.c +++ b/src/drivers/src/i2c_drv.c @@ -58,7 +58,7 @@ #define I2C_DEFAULT_SENSORS_CLOCK_SPEED 400000 // Definition of eeprom and deck I2C buss -#define I2C_DEFAULT_DECK_CLOCK_SPEED 400000 +#define I2C_DEFAULT_DECK_CLOCK_SPEED 100000 // Misc constants. #define I2C_NO_BLOCK 0 From 214949894e4d697c02ac42145e7e86427a236c8e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 5 Mar 2021 17:29:13 +0100 Subject: [PATCH 10/85] nau7802: minor tweaks --- src/deck/drivers/src/loadcell_nau7802.c | 13 +++++++------ src/utils/interface/sleepus.h | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c index c1a82891a2..f0c9e8ba94 100644 --- a/src/deck/drivers/src/loadcell_nau7802.c +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -331,13 +331,13 @@ bool nau7802_revision(uint8_t* revision) static void loadcellInit(DeckInfo *info) { - if (isInit) { - return; - } + // if (isInit) { + // return; + // } isInit = true; - // sleepus(1000 * 1000); + sleepus(1000 * 1000); // isInit &= nau7802_reset(); // uint8_t revision; // isInit &= nau7802_revision(&revision); @@ -345,10 +345,11 @@ static void loadcellInit(DeckInfo *info) if (true) { - isInit &= nau7802_reset(); - DEBUG_PRINT("Reset [%d]\n", isInit); isInit &= nau7802_powerUp(); DEBUG_PRINT("Power up [%d]\n", isInit); + + isInit &= nau7802_reset(); + DEBUG_PRINT("Reset [%d]\n", isInit); isInit &= nau7802_setLDO(NAU7802_LDO_2V7); DEBUG_PRINT("LDO [%d]\n", isInit); isInit &= nau7802_setGain(NAU7802_GAIN_128); diff --git a/src/utils/interface/sleepus.h b/src/utils/interface/sleepus.h index 5d46636bf3..cb07676c7e 100644 --- a/src/utils/interface/sleepus.h +++ b/src/utils/interface/sleepus.h @@ -26,6 +26,6 @@ #ifndef __SLEEPNS_H__ #define __SLEEPNS_H__ -void sleepus(uint32_t ns); +void sleepus(uint32_t us); #endif // __SLEEPNS_H__ From c97b52292df1eaef1a7e4f3e3f9993dfd156c2d9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 9 Mar 2021 10:01:08 +0100 Subject: [PATCH 11/85] NAU7802 reporting measurements --- Makefile | 2 +- src/deck/drivers/src/loadcell_nau7802.c | 31 ++++++++++++++----------- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/Makefile b/Makefile index d66e2f084b..38e342b610 100644 --- a/Makefile +++ b/Makefile @@ -221,7 +221,7 @@ PROJ_OBJ += lighthouse.o PROJ_OBJ += activeMarkerDeck.o # PROJ_OBJ += loadcell.o PROJ_OBJ += loadcell_nau7802.o -#PROJ_OBJ += rpm.o +PROJ_OBJ += rpm.o # Uart2 Link for CRTP communication is not compatible with decks using uart2 ifeq ($(UART2_LINK), 1) diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c index f0c9e8ba94..d8e6b967e1 100644 --- a/src/deck/drivers/src/loadcell_nau7802.c +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -306,14 +306,18 @@ bool nau7802_getMeasurement(int32_t* measurement) { bool result = true; - uint8_t valueRaw[3]; - result &= i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADCO_B2, 3, (uint8_t*)&valueRaw); + uint8_t data[3]; + result &= i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADCO_B2, 3, (uint8_t*)&data); + // DEBUG_PRINT("M: %d %d, %d, %d\n", result, data[0], data[1], data[2]); + + int32_t valueRaw = ((int32_t)data[0] << 16) | + ((int32_t)data[1] << 8) | + ((int32_t)data[2] << 0); // recover the sign - int32_t valueShifted = (int32_t)(valueRaw[2] << 16) | - (valueRaw[1] << 8) | - (valueRaw[0] << 0); - if ((int8_t)valueRaw[2] < 0) *measurement = -valueShifted; - else *measurement = valueShifted; + int32_t valueShifted = (valueRaw << 8); + int32_t value = (valueShifted >> 8); + + *measurement = value; return result; } @@ -337,19 +341,20 @@ static void loadcellInit(DeckInfo *info) isInit = true; - sleepus(1000 * 1000); + // sleepus(1000 * 1000); // isInit &= nau7802_reset(); - // uint8_t revision; - // isInit &= nau7802_revision(&revision); - // DEBUG_PRINT("bla %d %d\n", isInit, revision); if (true) { isInit &= nau7802_powerUp(); DEBUG_PRINT("Power up [%d]\n", isInit); - isInit &= nau7802_reset(); - DEBUG_PRINT("Reset [%d]\n", isInit); + uint8_t revision; + isInit &= nau7802_revision(&revision); + DEBUG_PRINT("revision [%d]: %d\n", isInit, revision); + + // isInit &= nau7802_reset(); + // DEBUG_PRINT("Reset [%d]\n", isInit); isInit &= nau7802_setLDO(NAU7802_LDO_2V7); DEBUG_PRINT("LDO [%d]\n", isInit); isInit &= nau7802_setGain(NAU7802_GAIN_128); From 57b695eee14649cd569d0ba583ce0147b2ca1f70 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 12 Mar 2021 13:04:15 +0100 Subject: [PATCH 12/85] add support for data rdy --- src/deck/drivers/src/loadcell_nau7802.c | 115 ++++++++++++++++++++---- 1 file changed, 98 insertions(+), 17 deletions(-) diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c index d8e6b967e1..f871da3a23 100644 --- a/src/deck/drivers/src/loadcell_nau7802.c +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -43,16 +43,22 @@ #include "sleepus.h" #include "debug.h" +// Hardware defines (also update deck driver below!) #define DECK_I2C_ADDRESS 0x2A +#define DATA_READY_PIN DECK_GPIO_IO1 static bool isInit; static int32_t rawWeight; static float weight; -static bool enable = true; static float a = 4.22852802e-05; static float b = -2.21784688e+01; +static uint8_t sampleRateDesired = 0; +static uint8_t sampleRate = 0; + +static xSemaphoreHandle dataReady; +static StaticSemaphore_t dataReadyBuffer; static void loadcellTask(void* prm); @@ -209,12 +215,13 @@ bool nau7802_powerUp() bool result = true; result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, 0x06); //(PU analog and PU digital) + DEBUG_PRINT("pu %d\n", result); // result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUD, 1); // result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUA, 1); //Wait for Power Up bit to be set - takes approximately 200us - for (int i = 0; i < 200; ++i) { + for (int i = 0; i < 500; ++i) { uint8_t bit; result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUR, &bit); if (result && bit) { @@ -332,6 +339,34 @@ bool nau7802_revision(uint8_t* revision) ///////////////////////////// +static void setupDataReadyInterrupt(void) +{ + pinMode(DATA_READY_PIN, INPUT); + + dataReady = xSemaphoreCreateBinaryStatic(&dataReadyBuffer); + + // TODO: EXTI_PortSourceGPIOB and EXTI_PinSource8 should be part of deckGPIOMapping! + SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource8); + + EXTI_InitTypeDef EXTI_InitStructure; + EXTI_InitStructure.EXTI_Line = EXTI_Line8; + EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; + EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; + EXTI_InitStructure.EXTI_LineCmd = ENABLE; + // portDISABLE_INTERRUPTS(); + EXTI_Init(&EXTI_InitStructure); + // EXTI_ClearITPendingBit(EXTI_Line15); + // portENABLE_INTERRUPTS(); +} + +void __attribute__((used)) EXTI8_Callback(void) +{ + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(dataReady, &xHigherPriorityTaskWoken); + if (xHigherPriorityTaskWoken) { + portYIELD(); + } +} static void loadcellInit(DeckInfo *info) { @@ -346,6 +381,9 @@ static void loadcellInit(DeckInfo *info) if (true) { + // isInit &= nau7802_reset(); + // DEBUG_PRINT("Reset [%d]\n", isInit); + isInit &= nau7802_powerUp(); DEBUG_PRINT("Power up [%d]\n", isInit); @@ -353,13 +391,11 @@ static void loadcellInit(DeckInfo *info) isInit &= nau7802_revision(&revision); DEBUG_PRINT("revision [%d]: %d\n", isInit, revision); - // isInit &= nau7802_reset(); - // DEBUG_PRINT("Reset [%d]\n", isInit); isInit &= nau7802_setLDO(NAU7802_LDO_2V7); DEBUG_PRINT("LDO [%d]\n", isInit); isInit &= nau7802_setGain(NAU7802_GAIN_128); DEBUG_PRINT("Gain [%d]\n", isInit); - isInit &= nau7802_setSampleRate(NAU7802_SPS_80); + isInit &= nau7802_setSampleRate(NAU7802_SPS_10); DEBUG_PRINT("Sample rate [%d]\n", isInit); // Turn off CLK_CHP. From 9.1 power on sequencing. isInit &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADC, 0x30); @@ -372,6 +408,9 @@ static void loadcellInit(DeckInfo *info) } + // Set up Interrupt + setupDataReadyInterrupt(); + if (1) { // Create a task xTaskCreate(loadcellTask, "LOADCELL", @@ -382,24 +421,66 @@ static void loadcellInit(DeckInfo *info) static void loadcellTask(void* prm) { - TickType_t lastWakeTime = xTaskGetTickCount(); + // TickType_t lastWakeTime = xTaskGetTickCount(); + + // while (1) + // { + // vTaskDelayUntil(&lastWakeTime, F2T(20)); + // int rdy = digitalRead(DATA_READY_PIN); + // if (rdy) { + // int32_t measurement; + // bool result = nau7802_getMeasurement(&measurement); + // DEBUG_PRINT("m %d %ld\n", result, measurement); + // } else { + // DEBUG_PRINT("p %d\n", rdy); + // } + + // } + + while (1) { + BaseType_t semResult = xSemaphoreTake(dataReady, M2T(500)); + if (semResult == pdTRUE || digitalRead(DATA_READY_PIN)) + { + int32_t measurement; + bool result = nau7802_getMeasurement(&measurement); + if (result) { + rawWeight = measurement; + weight = a * rawWeight + b; + } + } + if (sampleRate != sampleRateDesired) { + switch (sampleRateDesired) { + case NAU7802_SPS_320: + case NAU7802_SPS_80: + case NAU7802_SPS_40: + case NAU7802_SPS_20: + case NAU7802_SPS_10: + { + bool result = nau7802_setSampleRate(sampleRateDesired); + if (result) { + DEBUG_PRINT("switched sample rate!\n"); + } + } + break; + default: + DEBUG_PRINT("Unknown sample rate!\n"); + break; + } + sampleRate = sampleRateDesired; + } - while(1) { - vTaskDelayUntil(&lastWakeTime, F2T(100)); - if (enable && nau7802_hasMeasurement()) { - nau7802_getMeasurement(&rawWeight); - weight = a * rawWeight + b; } } -} static const DeckDriver loadcell_deck = { - .vid = 0x00, - .pid = 0x00, - .name = "bcLoadcell", + .vid = 0x00, + .pid = 0x00, + .name = "bcLoadcell", + + .usedGpio = DECK_USING_IO_1, - .init = loadcellInit, + .init = loadcellInit, }; DECK_DRIVER(loadcell_deck); @@ -409,9 +490,9 @@ PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit) PARAM_GROUP_STOP(deck) PARAM_GROUP_START(loadcell) -PARAM_ADD(PARAM_UINT8, enable, &enable) PARAM_ADD(PARAM_FLOAT, a, &a) PARAM_ADD(PARAM_FLOAT, b, &b) +PARAM_ADD(PARAM_UINT8, sampleRate, &sampleRateDesired) PARAM_GROUP_STOP(loadcell) LOG_GROUP_START(loadcell) From f35d92b367cb5159acc9665585b01833b1c2d049 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 12 Mar 2021 13:04:27 +0100 Subject: [PATCH 13/85] exti cleanup --- src/deck/drivers/src/locodeck.c | 16 ++++++---------- src/drivers/src/exti.c | 7 +++++++ 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/deck/drivers/src/locodeck.c b/src/deck/drivers/src/locodeck.c index 3083334dd2..8965f0b104 100644 --- a/src/deck/drivers/src/locodeck.c +++ b/src/deck/drivers/src/locodeck.c @@ -73,14 +73,12 @@ #define EXTI_PortSource EXTI_PortSourceGPIOB #define EXTI_PinSource EXTI_PinSource5 #define EXTI_LineN EXTI_Line5 - #define EXTI_IRQChannel EXTI9_5_IRQn #else #define GPIO_PIN_IRQ DECK_GPIO_RX1 #define GPIO_PIN_RESET DECK_GPIO_TX1 #define EXTI_PortSource EXTI_PortSourceGPIOC #define EXTI_PinSource EXTI_PinSource11 #define EXTI_LineN EXTI_Line11 - #define EXTI_IRQChannel EXTI15_10_IRQn #endif @@ -434,7 +432,6 @@ static void spiRead(dwDevice_t* dev, const void *header, size_t headerLength, { portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - NVIC_ClearPendingIRQ(EXTI_IRQChannel); EXTI_ClearITPendingBit(EXTI_LineN); // Unlock interrupt handling task @@ -473,7 +470,6 @@ static dwOps_t dwOps = { static void dwm1000Init(DeckInfo *info) { EXTI_InitTypeDef EXTI_InitStructure; - NVIC_InitTypeDef NVIC_InitStructure; spiBegin(); @@ -542,12 +538,12 @@ static void dwm1000Init(DeckInfo *info) memoryRegisterHandler(&memDef); - // Enable interrupt - NVIC_InitStructure.NVIC_IRQChannel = EXTI_IRQChannel; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_VERY_HIGH_PRI; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + // // Enable interrupt + // NVIC_InitStructure.NVIC_IRQChannel = EXTI_IRQChannel; + // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_VERY_HIGH_PRI; + // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + // NVIC_Init(&NVIC_InitStructure); algoSemaphore= xSemaphoreCreateMutex(); diff --git a/src/drivers/src/exti.c b/src/drivers/src/exti.c index 561e443675..86675d7a08 100644 --- a/src/drivers/src/exti.c +++ b/src/drivers/src/exti.c @@ -91,36 +91,42 @@ bool extiTest(void) void __attribute__((used)) EXTI0_IRQHandler(void) { + NVIC_ClearPendingIRQ(EXTI0_IRQn); EXTI_ClearITPendingBit(EXTI_Line0); EXTI0_Callback(); } void __attribute__((used)) EXTI1_IRQHandler(void) { + NVIC_ClearPendingIRQ(EXTI1_IRQn); EXTI_ClearITPendingBit(EXTI_Line1); EXTI1_Callback(); } void __attribute__((used)) EXTI2_IRQHandler(void) { + NVIC_ClearPendingIRQ(EXTI2_IRQn); EXTI_ClearITPendingBit(EXTI_Line2); EXTI2_Callback(); } void __attribute__((used)) EXTI3_IRQHandler(void) { + NVIC_ClearPendingIRQ(EXTI3_IRQn); EXTI_ClearITPendingBit(EXTI_Line3); EXTI3_Callback(); } void __attribute__((used)) EXTI4_IRQHandler(void) { + NVIC_ClearPendingIRQ(EXTI4_IRQn); EXTI_ClearITPendingBit(EXTI_Line4); EXTI4_Callback(); } void __attribute__((used)) EXTI9_5_IRQHandler(void) { + NVIC_ClearPendingIRQ(EXTI9_5_IRQn); if (EXTI_GetITStatus(EXTI_Line5) == SET) { EXTI_ClearITPendingBit(EXTI_Line5); EXTI5_Callback(); @@ -149,6 +155,7 @@ void __attribute__((used)) EXTI9_5_IRQHandler(void) void __attribute__((used)) EXTI15_10_IRQHandler(void) { + NVIC_ClearPendingIRQ(EXTI15_10_IRQn); if (EXTI_GetITStatus(EXTI_Line10) == SET) { EXTI_ClearITPendingBit(EXTI_Line10); EXTI10_Callback(); From 7e3dc50fe4c547b12f44c2fdcd7ed9a7abf2f506 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Fri, 12 Mar 2021 17:25:45 +0100 Subject: [PATCH 14/85] bcRpm: Fixed rmp mapping and increased crtp queue size. --- src/deck/drivers/src/rpm.c | 60 +++++++++++++++++++------------------- src/modules/src/crtp.c | 2 +- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/deck/drivers/src/rpm.c b/src/deck/drivers/src/rpm.c index 466da74374..4879d7eff5 100644 --- a/src/deck/drivers/src/rpm.c +++ b/src/deck/drivers/src/rpm.c @@ -167,7 +167,7 @@ static uint16_t calcRPM(uint32_t t1, uint32_t t2) void __attribute__((used)) TIM5_IRQHandler(void) { static int updateM1Cnt = 2; - static int updateM2Cnt = 2; + static int updateM4Cnt = 2; uint16_t ccVal; //Motor1 @@ -194,7 +194,7 @@ void __attribute__((used)) TIM5_IRQHandler(void) updateM1Cnt = 2; } - //Motor2 + //Motor4 if(TIM_GetITStatus(TIM5, TIM_IT_CC4) == SET) { /* Clear TIM5 Capture compare interrupt pending bit */ @@ -208,14 +208,14 @@ void __attribute__((used)) TIM5_IRQHandler(void) } if (ccVal > lastcc4Val) - m2Time[m2cnt & 0x01] = ccVal - lastcc4Val; + m4Time[m4cnt & 0x01] = ccVal - lastcc4Val; else - m2Time[m2cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc4Val); + m4Time[m4cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc4Val); lastcc4Val = ccVal; - m2cnt++; - m2rpm = calcRPM(m2Time[0], m2Time[1]); - updateM2Cnt = 2; + m4cnt++; + m4rpm = calcRPM(m4Time[0], m4Time[1]); + updateM4Cnt = 2; } if(TIM_GetITStatus(TIM5, TIM_IT_Update) == SET) @@ -225,20 +225,20 @@ void __attribute__((used)) TIM5_IRQHandler(void) { m1rpm = 0; } - if (--updateM2Cnt < 0) + if (--updateM4Cnt < 0) { - m2rpm = 0; + m4rpm = 0; } } } void __attribute__((used)) TIM3_IRQHandler(void) { + static int updateM2Cnt = 2; static int updateM3Cnt = 2; - static int updateM4Cnt = 2; uint16_t ccVal; - //Motor3 + //Motor2 if(TIM_GetITStatus(TIM3, TIM_IT_CC1) == SET) { /* Clear TIM3 Capture compare interrupt pending bit */ @@ -252,20 +252,20 @@ void __attribute__((used)) TIM3_IRQHandler(void) } if (ccVal > lastcc1Val) - m3Time[m3cnt & 0x01] = ccVal - lastcc1Val; + m2Time[m2cnt & 0x01] = ccVal - lastcc1Val; else - m3Time[m3cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc1Val); + m2Time[m2cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc1Val); lastcc1Val = ccVal; - m3cnt++; - m3rpm = calcRPM(m3Time[0], m3Time[1]); - updateM3Cnt = 2; + m2cnt++; + m2rpm = calcRPM(m2Time[0], m2Time[1]); + updateM2Cnt = 2; -// rpmPutchar((m3Time >> 8) & 0x00FF); -// rpmPutchar(m3Time & 0x00FF); +// rpmPutchar((m2Time >> 8) & 0x00FF); +// rpmPutchar(m2Time & 0x00FF); } - //Motor4 + //Motor3 if(TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET) { /* Clear TIM3 Capture compare interrupt pending bit */ @@ -279,29 +279,29 @@ void __attribute__((used)) TIM3_IRQHandler(void) } if (ccVal > lastcc2Val) - m4Time[m4cnt & 0x01] = ccVal - lastcc2Val; + m3Time[m3cnt & 0x01] = ccVal - lastcc2Val; else - m4Time[m4cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc2Val); + m3Time[m3cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc2Val); lastcc2Val = ccVal; - m4cnt++; - m4rpm = calcRPM(m4Time[0], m4Time[1]); - updateM4Cnt = 2; + m3cnt++; + m3rpm = calcRPM(m3Time[0], m3Time[1]); + updateM3Cnt = 2; -// rpmPutchar((m4rpm >> 8) & 0x00FF); -// rpmPutchar(m4rpm & 0x00FF); +// rpmPutchar((m3rpm >> 8) & 0x00FF); +// rpmPutchar(m3rpm & 0x00FF); } if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET) { TIM_ClearITPendingBit(TIM3, TIM_IT_Update); - if (--updateM3Cnt < 0) + if (--updateM2Cnt < 0) { - m3rpm = 0; + m2rpm = 0; } - if (--updateM4Cnt < 0) + if (--updateM3Cnt < 0) { - m4rpm = 0; + m3rpm = 0; } } } diff --git a/src/modules/src/crtp.c b/src/modules/src/crtp.c index 63e671db8a..12465bed2e 100644 --- a/src/modules/src/crtp.c +++ b/src/modules/src/crtp.c @@ -70,7 +70,7 @@ static struct { static xQueueHandle txQueue; #define CRTP_NBR_OF_PORTS 16 -#define CRTP_TX_QUEUE_SIZE 120 +#define CRTP_TX_QUEUE_SIZE 200 #define CRTP_RX_QUEUE_SIZE 16 static void crtpTxTask(void *param); From 29d77fac3378bbf4011a65bcf5400fafef62e0a8 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Fri, 12 Mar 2021 17:28:01 +0100 Subject: [PATCH 15/85] Added ACS37800 driver for system-id deck. --- src/deck/drivers/src/acs37800.c | 281 ++++++++++++++++++++++++++++++++ 1 file changed, 281 insertions(+) create mode 100644 src/deck/drivers/src/acs37800.c diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c new file mode 100644 index 0000000000..78d8250e40 --- /dev/null +++ b/src/deck/drivers/src/acs37800.c @@ -0,0 +1,281 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2021 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * asc37800.c - Deck driver for ACS37800 power monitoring IC + */ + +#include +#include +#include "stm32fxxx.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "timers.h" + +#include "debug.h" +#include "deck.h" +#include "param.h" +#include "log.h" +#include "sleepus.h" +#include "i2cdev.h" + + +// EEPROM +#define ACSREG_E_TRIM 0x0B +#define ACSREG_E_OFFS_AVG 0x0C +#define ACSREG_E_FAULT 0x0D +#define ACSREG_E_UNDR_OVR_V 0x0E +#define ACSREG_E_IO_SEL 0x0F + +// SHADOW +#define ACSREG_S_TRIM 0x1B +#define ACSREG_S_OFFS_AVG 0x1C +#define ACSREG_S_FAULT 0x1D +#define ACSREG_S_UNDR_OVR_V 0x1E +#define ACSREG_S_IO_SEL 0x1F + +#define ACSREG_I_V_RMS 0x20 +#define ACSREG_P_ACTIVE 0x21 +#define ACSREG_P_APPARENT 0x22 +#define ACSREG_NUM_PTS_OUT 0x25 +#define ACSREG_VRMS_AVGS 0x26 +#define ACSREG_VRMS_AVGM 0x27 +#define ACSREG_P_ACT_AVGS 0x28 +#define ACSREG_P_ACT_AVGM 0x29 +#define ACSREG_I_V_CODES 0x2A +#define ACSREG_P_INSTANT 0x2C +#define ACSREG_DSP_STATUS 0x2D +#define ACSREG_ACCESS_CODE 0x2F + +#define ACS_I2C_ADDR 0x7F +#define ACS_ACCESS_CODE 0x4F70656E + +#define SCALE_V (27.23f) +#define SCALE_I (618.51f) + + +static bool isInit; +static uint32_t viBatRaw; +static uint32_t vavgBatRaw; +static float vBat; +static float vavgBat; +static float iBat; +static float iavgBat; +static uint32_t pBatRaw; +static float pBat; + +static void asc37800Task(void* prm); + +/* + * Sign extend a bitfield which if right justified + * + * data - the bitfield to be sign extended + * width - the width of the bitfield + * returns - the sign extended bitfield + */ +int32_t signExtendBitfield(uint32_t data, uint16_t width) +{ + // If the bitfield is the width of the variable, don't bother trying to sign extend (it already is) + if (width == 32) + { + return (int32_t)data; + } + + int32_t x = (int32_t)data; + int32_t mask = 1L << (width - 1); + + x = x & ((1 << width) - 1); // make sure the upper bits are zero + + return (int32_t)((x ^ mask) - mask); +} + +/* + * Convert an unsigned bitfield which is right justified, into a floating point number + * + * data - the bitfield to be converted + * binaryPoint - the binary point (the bit to the left of the binary point) + * width - the width of the bitfield + * returns - the floating point number + */ +float convertUnsignedFixedPoint(uint32_t inputValue, uint16_t binaryPoint, uint16_t width) +{ + uint32_t mask; + + if (width == 32) + { + mask = 0xFFFFFFFF; + } + else + { + mask = (1UL << width) - 1UL; + } + + return (float)(inputValue & mask) / (float)(1L << binaryPoint); +} + +/* + * Convert a signed bitfield which is right justified, into a floating point number + * + * data - the bitfield to be sign extended then converted + * binaryPoint - the binary point (the bit to the left of the binary point) + * width - the width of the bitfield + * returns - the floating point number + */ +float convertSignedFixedPoint(uint32_t inputValue, uint16_t binaryPoint, uint16_t width) +{ + int32_t signedValue = signExtendBitfield(inputValue, width); + return (float)signedValue / (float)(1L << binaryPoint); +} + +static bool asc37800Read32(uint8_t reg, uint32_t *data32) +{ + i2cdevReadReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)data32); +} + +static bool asc37800Write32(uint8_t reg, uint32_t data32) +{ + i2cdevWriteReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)&data32); +} + +static bool ascFindAndSetAddress(void) +{ + uint8_t startAddr; + uint32_t dummy = 0; + bool isRepying; + + for (startAddr = 96; startAddr <= 127; startAddr++) + { + isRepying = i2cdevWrite(I2C1_DEV, startAddr, 1, (uint8_t *)&dummy); + + if (isRepying) + { + // Unlock EEPROM + dummy = ACS_ACCESS_CODE; + i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_ACCESS_CODE, 4, (uint8_t *)&dummy); + // EERPOM: write and lock i2c address to 0x7F; + dummy = (0x7F << 2) | (0x01 << 9); + i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy); + + DEBUG_PRINT("ACS37800 found on: %d. Setting address to 0x7E. Power cycle needed!\n", startAddr); + } + } +} + + +static void asc37800Init(DeckInfo *info) +{ + uint8_t dummy; + uint32_t val; + + if (isInit) { + return; + } + + if (i2cdevWrite(I2C1_DEV, ACS_I2C_ADDR, 1, (uint8_t *)&dummy)) + { + asc37800Write32(ACSREG_ACCESS_CODE, ACS_ACCESS_CODE); + DEBUG_PRINT("ACS37800 I2C [OK]\n"); + } + else + { + ascFindAndSetAddress(); + } + + // Enable bypass in shadow reg and set N to 32. + asc37800Write32(ACSREG_S_IO_SEL, (1 << 24) | (32 << 14) | (0x01 << 9) | (0x7F << 2)); + // Set current and voltage for averaging and keep device specific (trim) settings. + asc37800Read32(ACSREG_S_TRIM, &val); + asc37800Write32(ACSREG_S_TRIM, (val | (0 << 23) | (1 << 22))); + // Set average to 100 samples. + asc37800Write32(ACSREG_S_OFFS_AVG, ((100 << 7) | (100 << 0))); + + DEBUG_PRINT("---------------\n"); + for (int reg = 0x0B; reg <= 0x0F; reg++) + { + bool res; + + res = asc37800Read32(reg, &val); + DEBUG_PRINT("%d:R:0x%.2X:%X\n", res, reg, val); + res = asc37800Read32(reg+0x10, &val); + DEBUG_PRINT("%d:R:0x%.2X:%X\n\n", res, reg+0x10, val); + } + + + xTaskCreate(asc37800Task, "asc37800", + 2*configMINIMAL_STACK_SIZE, NULL, + /*priority*/2, NULL); + + isInit = true; +} + +static void asc37800Task(void* prm) +{ + systemWaitStart(); + + TickType_t lastWakeTime = xTaskGetTickCount(); + + while(1) { + vTaskDelayUntil(&lastWakeTime, M2T(10)); + + asc37800Read32(ACSREG_I_V_RMS, &viBatRaw); + asc37800Read32(ACSREG_P_ACTIVE, &pBatRaw); + asc37800Read32(ACSREG_VRMS_AVGS, &vavgBatRaw); + + vBat = convertUnsignedFixedPoint((viBatRaw & 0xFFFF), 16, 16); + iBat = convertSignedFixedPoint((viBatRaw >> 16 & 0xFFFF), 15, 16); + vavgBat = convertSignedFixedPoint((vavgBatRaw & 0xFFFF), 15, 16); + iavgBat = convertSignedFixedPoint((vavgBatRaw >> 16 & 0xFFFF), 15, 16); + pBat = convertSignedFixedPoint((pBatRaw & 0xFFFF), 15, 16); + +// DEBUG_PRINT("V: %.3f I: %.3f P: %.3f\n", vBat, iBat, pBat); +// DEBUG_PRINT("V: %f\tI: %f\tP: %f\n", vBat, iBat, pBat); +// DEBUG_PRINT("Vc: %f\tIc: %f\tV: %f\tI: %f\tP: %f\n", vavgBat, iavgBat, vBat, iBat, pBat); + } +} + +static const DeckDriver asc37800_deck = { + .vid = 0x00, + .pid = 0x00, + .name = "bcACS37800", + + .usedGpio = DECK_USING_SCL | DECK_USING_SDA, + + .init = asc37800Init, +}; + +DECK_DRIVER(asc37800_deck); + +PARAM_GROUP_START(deck) +PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcACS37800, &isInit) +PARAM_GROUP_STOP(deck) + +PARAM_GROUP_START(asc37800) +PARAM_GROUP_STOP(asc37800) + +LOG_GROUP_START(asc37800) +LOG_ADD(LOG_FLOAT, v, &vBat) +LOG_ADD(LOG_FLOAT, v_avg, &vavgBat) +LOG_ADD(LOG_FLOAT, i, &iBat) +LOG_ADD(LOG_FLOAT, i_avg, &iavgBat) +LOG_ADD(LOG_FLOAT, p, &pBat) +LOG_GROUP_STOP(asc37800) From 5c1dae5233c27b653a5e57d5867ced0d28f80ad4 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 12 Mar 2021 17:35:53 +0100 Subject: [PATCH 16/85] nau7802: fix i2c bugs caused by wrong i2cwritebits function --- src/deck/drivers/src/loadcell_nau7802.c | 220 +++++++++++++++--------- 1 file changed, 143 insertions(+), 77 deletions(-) diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c index f871da3a23..717aa780d5 100644 --- a/src/deck/drivers/src/loadcell_nau7802.c +++ b/src/deck/drivers/src/loadcell_nau7802.c @@ -42,6 +42,7 @@ #include "i2cdev.h" #include "sleepus.h" #include "debug.h" +#include "statsCnt.h" // Hardware defines (also update deck driver below!) #define DECK_I2C_ADDRESS 0x2A @@ -51,11 +52,16 @@ static bool isInit; static int32_t rawWeight; static float weight; -static float a = 4.22852802e-05; -static float b = -2.21784688e+01; +static float a[2]; +static float b[2]; +// static uint8_t currentChannel = 0; static uint8_t sampleRateDesired = 0; static uint8_t sampleRate = 0; +static uint8_t channelDesired = 0; +static uint8_t channel = 0; + +static STATS_CNT_RATE_DEFINE(rate, 1000); static xSemaphoreHandle dataReady; static StaticSemaphore_t dataReadyBuffer; @@ -64,6 +70,14 @@ static void loadcellTask(void* prm); //////////////////////////// +typedef struct +{ + uint8_t pu_ctrl; + uint8_t ctrl1; + uint8_t ctrl2; +} nau7802_t; +static nau7802_t nau7802; + //Register Map typedef enum { @@ -199,29 +213,37 @@ typedef enum NAU7802_CAL_FAILURE = 2, } NAU7802_Cal_Status; -//Resets all registers to Power Of Defaults -bool nau7802_reset() +void nau7802_init(nau7802_t* ctx) { - bool result = true; - result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 1); - sleepus(1); - result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 0); - return result; + ctx->pu_ctrl = 0; + ctx->ctrl1 = 0; + ctx->ctrl2 = 0; } +// //Resets all registers to Power Of Defaults +// bool nau7802_reset(nau7802_t *ctx) +// { +// bool result = true; +// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 1); +// sleepus(1); +// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 0); +// return result; +// } + //Power up digital and analog sections of scale -bool nau7802_powerUp() +bool nau7802_powerUp(nau7802_t *ctx) { bool result = true; - result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, 0x06); //(PU analog and PU digital) + ctx->pu_ctrl = 0x06; //(PU analog and PU digital) + result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, ctx->pu_ctrl); DEBUG_PRINT("pu %d\n", result); // result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUD, 1); // result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUA, 1); //Wait for Power Up bit to be set - takes approximately 200us - for (int i = 0; i < 500; ++i) { + for (int i = 0; i < 200; ++i) { uint8_t bit; result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUR, &bit); if (result && bit) { @@ -233,34 +255,53 @@ bool nau7802_powerUp() } //Set the onboard Low-Drop-Out voltage regulator to a given value -bool nau7802_setLDO(NAU7802_LDO_Values ldoValue) +bool nau7802_setLDO(nau7802_t *ctx, NAU7802_LDO_Values ldoValue) { bool result = true; //Set the value of the LDO - result &= i2cdevWriteBits(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, 3, 3, ldoValue); + ctx->ctrl1 &= 0b11000111; //Clear LDO bits + ctx->ctrl1 |= ldoValue << 3; //Mask in new LDO bits + result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, ctx->ctrl1); //Enable the internal LDO - result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_AVDDS, 1); + DEBUG_PRINT("l1 %d\n", ctx->pu_ctrl); + ctx->pu_ctrl |= (1 << NAU7802_PU_CTRL_AVDDS); + result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, ctx->pu_ctrl); + DEBUG_PRINT("l2 %d\n", ctx->pu_ctrl); return result; } //Set the gain -bool nau7802_setGain(NAU7802_Gain_Values gainValue) +bool nau7802_setGain(nau7802_t *ctx, NAU7802_Gain_Values gainValue) { bool result = true; - result &= i2cdevWriteBits(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, 0, 3, gainValue); + ctx->ctrl1 &= 0b11111000; //Clear gain bits + ctx->ctrl1 |= gainValue; //Mask in new bits + result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, ctx->ctrl1); return result; } //Set the readings per second -bool nau7802_setSampleRate(NAU7802_SPS_Values rate) +bool nau7802_setSampleRate(nau7802_t* ctx, NAU7802_SPS_Values rate) +{ + bool result = true; + ctx->ctrl2 &= 0b10001111; // Clear CRS bits + ctx->ctrl2 |= rate << 4; //Mask in new CRS bits + result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, ctx->ctrl2); + return result; +} + +//Set channel +bool nau7802_setChannel(nau7802_t *ctx, NAU7802_Channels channel) { bool result = true; - result &= i2cdevWriteBits(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, 4, 3, rate); + ctx->ctrl2 &= 0b01111111; // Clear CHS bits + ctx->ctrl2 |= channel << 7; //Mask in new CHS bits + result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, ctx->ctrl2); return result; } //Check calibration status. -NAU7802_Cal_Status nau7802_calAFEStatus() +NAU7802_Cal_Status nau7802_calAFEStatus(nau7802_t *ctx) { bool result = true; uint8_t bit; @@ -283,14 +324,14 @@ NAU7802_Cal_Status nau7802_calAFEStatus() //Calibrate analog front end of system. Returns true if CAL_ERR bit is 0 (no error) //Takes approximately 344ms to calibrate; wait up to 1000ms. //It is recommended that the AFE be re-calibrated any time the gain, SPS, or channel number is changed. -bool nau7802_calibrateAFE() +bool nau7802_calibrateAFE(nau7802_t *ctx) { bool result = true; //Begin asynchronous calibration of the analog front end. result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CALS, 1); for (int i = 0; i < 1000; ++i) { - if (nau7802_calAFEStatus() == NAU7802_CAL_SUCCESS) { + if (nau7802_calAFEStatus(ctx) == NAU7802_CAL_SUCCESS) { return true; } vTaskDelay(M2T(1)); @@ -299,7 +340,7 @@ bool nau7802_calibrateAFE() } //Returns true if Cycle Ready bit is set (conversion is complete) -bool nau7802_hasMeasurement() +bool nau7802_hasMeasurement(nau7802_t *ctx) { bool result = true; uint8_t bit; @@ -309,7 +350,7 @@ bool nau7802_hasMeasurement() //Returns 24-bit reading //Assumes CR Cycle Ready bit (ADC conversion complete) has been checked to be 1 -bool nau7802_getMeasurement(int32_t* measurement) +bool nau7802_getMeasurement(nau7802_t *ctx, int32_t *measurement) { bool result = true; @@ -329,7 +370,7 @@ bool nau7802_getMeasurement(int32_t* measurement) return result; } -bool nau7802_revision(uint8_t* revision) +bool nau7802_revision(nau7802_t *ctx, uint8_t *revision) { bool result = true; result &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_DEVICE_REV, revision); @@ -381,21 +422,22 @@ static void loadcellInit(DeckInfo *info) if (true) { + nau7802_init(&nau7802); // isInit &= nau7802_reset(); // DEBUG_PRINT("Reset [%d]\n", isInit); - isInit &= nau7802_powerUp(); + isInit &= nau7802_powerUp(&nau7802); DEBUG_PRINT("Power up [%d]\n", isInit); uint8_t revision; - isInit &= nau7802_revision(&revision); + isInit &= nau7802_revision(&nau7802, & revision); DEBUG_PRINT("revision [%d]: %d\n", isInit, revision); - isInit &= nau7802_setLDO(NAU7802_LDO_2V7); + isInit &= nau7802_setLDO(&nau7802, NAU7802_LDO_2V7); DEBUG_PRINT("LDO [%d]\n", isInit); - isInit &= nau7802_setGain(NAU7802_GAIN_128); + isInit &= nau7802_setGain(&nau7802, NAU7802_GAIN_128); DEBUG_PRINT("Gain [%d]\n", isInit); - isInit &= nau7802_setSampleRate(NAU7802_SPS_10); + isInit &= nau7802_setSampleRate(&nau7802, NAU7802_SPS_10); DEBUG_PRINT("Sample rate [%d]\n", isInit); // Turn off CLK_CHP. From 9.1 power on sequencing. isInit &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADC, 0x30); @@ -403,9 +445,17 @@ static void loadcellInit(DeckInfo *info) // Enable 330pF decoupling cap on chan 2. From 9.14 application circuit note. isInit &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PGA_PWR, NAU7802_PGA_PWR_PGA_CAP_EN, 1); DEBUG_PRINT("CAP [%d]\n", isInit); - isInit &= nau7802_calibrateAFE(); + // calibrate + isInit &= nau7802_calibrateAFE(&nau7802); DEBUG_PRINT("Cal [%d]\n", isInit); + uint8_t v; + isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, &v); + DEBUG_PRINT("puctrl: %d\n", v); + isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, &v); + DEBUG_PRINT("ctrl1: %d\n", v); + isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, &v); + DEBUG_PRINT("ctrl2: %d\n", v); } // Set up Interrupt @@ -421,57 +471,70 @@ static void loadcellInit(DeckInfo *info) static void loadcellTask(void* prm) { - // TickType_t lastWakeTime = xTaskGetTickCount(); - - // while (1) - // { - // vTaskDelayUntil(&lastWakeTime, F2T(20)); - // int rdy = digitalRead(DATA_READY_PIN); - // if (rdy) { - // int32_t measurement; - // bool result = nau7802_getMeasurement(&measurement); - // DEBUG_PRINT("m %d %ld\n", result, measurement); - // } else { - // DEBUG_PRINT("p %d\n", rdy); - // } - - // } - - while (1) { - BaseType_t semResult = xSemaphoreTake(dataReady, M2T(500)); - if (semResult == pdTRUE || digitalRead(DATA_READY_PIN)) - { - int32_t measurement; - bool result = nau7802_getMeasurement(&measurement); - if (result) { - rawWeight = measurement; - weight = a * rawWeight + b; - } + while (1) { + BaseType_t semResult = xSemaphoreTake(dataReady, M2T(500)); + if (semResult == pdTRUE || digitalRead(DATA_READY_PIN)) + { + int32_t measurement; + bool result = nau7802_getMeasurement(&nau7802, &measurement); + if (result) { + rawWeight = measurement; + weight = a[channel] * rawWeight + b[channel]; + // currentChannel = (currentChannel + 1) % 2; + // nau7802_setChannel(&nau7802, currentChannel); + + // nau7802.pu_ctrl |= (1 << NAU7802_PU_CTRL_CS); + // i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, nau7802.pu_ctrl); + + // nau7802.ctrl2 &= 0b11101111; // Clear CS bit + // i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, nau7802.pu_ctrl); + + STATS_CNT_RATE_EVENT(&rate); + } + } + if (sampleRate != sampleRateDesired) { + switch (sampleRateDesired) { + case NAU7802_SPS_320: + case NAU7802_SPS_80: + case NAU7802_SPS_40: + case NAU7802_SPS_20: + case NAU7802_SPS_10: + { + bool result = nau7802_setSampleRate(&nau7802, sampleRateDesired); + if (result) { + DEBUG_PRINT("switched sample rate!\n"); + } + } + break; + default: + DEBUG_PRINT("Unknown sample rate!\n"); + break; } - if (sampleRate != sampleRateDesired) { - switch (sampleRateDesired) { - case NAU7802_SPS_320: - case NAU7802_SPS_80: - case NAU7802_SPS_40: - case NAU7802_SPS_20: - case NAU7802_SPS_10: + sampleRate = sampleRateDesired; + } + if (channel != channelDesired) { + switch (channelDesired) { + case NAU7802_CHANNEL_1: + case NAU7802_CHANNEL_2: + { + bool result = nau7802_setChannel(&nau7802, channelDesired); + result &= nau7802_calibrateAFE(&nau7802); + if (result) { - bool result = nau7802_setSampleRate(sampleRateDesired); - if (result) { - DEBUG_PRINT("switched sample rate!\n"); - } + DEBUG_PRINT("switched and calibrated channel!\n"); } - break; - default: - DEBUG_PRINT("Unknown sample rate!\n"); - break; - } - sampleRate = sampleRateDesired; + } + break; + default: + DEBUG_PRINT("Unknown channel!\n"); + break; } + channel = channelDesired; + } - } } +} static const DeckDriver loadcell_deck = { .vid = 0x00, @@ -490,12 +553,15 @@ PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit) PARAM_GROUP_STOP(deck) PARAM_GROUP_START(loadcell) -PARAM_ADD(PARAM_FLOAT, a, &a) -PARAM_ADD(PARAM_FLOAT, b, &b) +PARAM_ADD(PARAM_FLOAT, a, &a[0]) +PARAM_ADD(PARAM_FLOAT, b, &b[0]) PARAM_ADD(PARAM_UINT8, sampleRate, &sampleRateDesired) +PARAM_ADD(PARAM_UINT8, channel, &channelDesired) PARAM_GROUP_STOP(loadcell) LOG_GROUP_START(loadcell) LOG_ADD(LOG_INT32, rawWeight, &rawWeight) LOG_ADD(LOG_FLOAT, weight, &weight) + +STATS_CNT_RATE_LOG_ADD(rate, &rate) LOG_GROUP_STOP(loadcell) From 8f1ff0205aa5271fad2e41d75a2b028249d8e165 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 12 Mar 2021 17:36:22 +0100 Subject: [PATCH 17/85] add log_start_v2 function to allow sample periods < 10ms --- src/modules/src/log.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/src/log.c b/src/modules/src/log.c index 9366d91c25..3288e1e135 100644 --- a/src/modules/src/log.c +++ b/src/modules/src/log.c @@ -113,6 +113,9 @@ struct ops_setting_v2 { uint16_t id; } __attribute__((packed)); +struct control_start_block_v2 { + uint16_t period_in_ms; +} __attribute__((packed)); #define TOC_CH 0 #define CONTROL_CH 1 @@ -131,6 +134,7 @@ struct ops_setting_v2 { #define CONTROL_RESET 5 #define CONTROL_CREATE_BLOCK_V2 6 #define CONTROL_APPEND_BLOCK_V2 7 +#define CONTROL_START_BLOCK_V2 8 #define BLOCK_ID_FREE -1 @@ -412,6 +416,12 @@ void logControlProcess() (struct ops_setting_v2*)&p.data[2], (p.size-2)/sizeof(struct ops_setting_v2) ); break; + case CONTROL_START_BLOCK_V2: + { + struct control_start_block_v2* args = (struct control_start_block_v2 *)&p.data[2]; + ret = logStartBlock(p.data[1], args->period_in_ms); + } + break; } //Commands answer From add55d7eee226a5616228ec6822f6f2edd3c013e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 12 Mar 2021 17:36:43 +0100 Subject: [PATCH 18/85] add motor delay test script --- tools/system_id/collect_data_motor_delay.py | 175 ++++++++++++++++++++ tools/system_id/plot_motor_delay.py | 63 +++++++ 2 files changed, 238 insertions(+) create mode 100644 tools/system_id/collect_data_motor_delay.py create mode 100644 tools/system_id/plot_motor_delay.py diff --git a/tools/system_id/collect_data_motor_delay.py b/tools/system_id/collect_data_motor_delay.py new file mode 100644 index 0000000000..53e49f9e3b --- /dev/null +++ b/tools/system_id/collect_data_motor_delay.py @@ -0,0 +1,175 @@ +""" +Example that uses a load cell to measure thrust using different RPMs +""" +import logging +import time +from threading import Timer +from threading import Thread + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.localization import Localization + + +import calibScale + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + + +class CollectData: + """ + Simple logging example class that logs the Stabilizer from a supplied + link uri and disconnects after 5s. + """ + + def __init__(self, link_uri, calib_a, calib_b): + """ Initialize and run the example with the specified link_uri """ + self.calib_a = calib_a + self.calib_b = calib_b + + self._cf = Crazyflie(rw_cache='./cache') + + # Connect some callbacks from the Crazyflie API + self._cf.connected.add_callback(self._connected) + self._cf.disconnected.add_callback(self._disconnected) + self._cf.connection_failed.add_callback(self._connection_failed) + self._cf.connection_lost.add_callback(self._connection_lost) + + print('Connecting to %s' % link_uri) + + # Try to connect to the Crazyflie + self._cf.open_link(link_uri) + + # Variable used to keep main loop occupied until disconnect + self.is_connected = True + + def _connected(self, link_uri): + """ This callback is called form the Crazyflie API when a Crazyflie + has been connected and the TOCs have been downloaded.""" + print('Connected to %s' % link_uri) + + print(self.calib_a, self.calib_b) + self._cf.param.set_value('loadcell.a', str(self.calib_a)) + self._cf.param.set_value('loadcell.b', str(self.calib_b)) + # self._cf.param.set_value('loadcell.sampleRate', 7) # fastest sample rate: 320 Hz; + self._cf.param.set_value('loadcell.sampleRate', 4) + + self._file = open("data.csv", "w+") + self._file.write("time[ms],weight[g],pwm,vbat[V]\n"); + + # The definition of the logconfig can be made before connecting + self._lg_stab = LogConfig(name='data', period_in_ms=5) + self._lg_stab.add_variable('loadcell.weight', 'float') + self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') + self._lg_stab.add_variable('pm.vbat', 'float') + + # Adding the configuration cannot be done until a Crazyflie is + # connected, since we need to check that the variables we + # would like to log are in the TOC. + try: + self._cf.log.add_config(self._lg_stab) + # This callback will receive the data + self._lg_stab.data_received_cb.add_callback(self._stab_log_data) + # This callback will be called on errors + self._lg_stab.error_cb.add_callback(self._stab_log_error) + # Start the logging + self._lg_stab.start_v2() # Use special v2 version to be able to sample > 100 Hz + except KeyError as e: + print('Could not start log configuration,' + '{} not found in TOC'.format(str(e))) + except AttributeError: + print('Could not add Stabilizer log config, bad configuration.') + + self._localization = Localization(self._cf) + + # Start a separate thread to do the motor test. + # Do not hijack the calling thread! + Thread(target=self._ramp_motors).start() + + # # Start a timer to disconnect in 10s + # t = Timer(5, self._cf.close_link) + # t.start() + + def _stab_log_error(self, logconf, msg): + """Callback from the log API when an error occurs""" + print('Error when logging %s: %s' % (logconf.name, msg)) + + def _stab_log_data(self, timestamp, data, logconf): + """Callback froma the log API when data arrives""" + print('[%d][%s]: %s' % (timestamp, logconf.name, data)) + self._file.write("{},{},{},{}\n".format(timestamp,data['loadcell.weight'], data['pwm.m1_pwm'], data['pm.vbat'])) + # pass + + def _connection_failed(self, link_uri, msg): + """Callback when connection initial connection fails (i.e no Crazyflie + at the speficied address)""" + print('Connection to %s failed: %s' % (link_uri, msg)) + self.is_connected = False + + def _connection_lost(self, link_uri, msg): + """Callback when disconnected after a connection has been made (i.e + Crazyflie moves out of range)""" + print('Connection to %s lost: %s' % (link_uri, msg)) + + def _disconnected(self, link_uri): + """Callback when the Crazyflie is disconnected (called in all cases)""" + print('Disconnected from %s' % link_uri) + self.is_connected = False + + def _applyThrust(self, thrust, duration): + start = time.time() + self._cf.param.set_value('motorPowerSet.m1', int(thrust)) + while time.time() - start < duration: + self._localization.send_emergency_stop_watchdog() + print(time.time() - start) + time.sleep(0.1) + + def _ramp_motors(self): + self._cf.param.set_value('motor.batCompensation', 0) + self._cf.param.set_value('motorPowerSet.m1', 0) + self._cf.param.set_value('motorPowerSet.enable', 2) + + while self.is_connected: + # 0 -> 1 + self._applyThrust(1.0 * 65535, 3.0) + # 1 -> 0 + self._applyThrust(0.0 * 65535, 3.0) + + # 0 -> 0.5 + self._applyThrust(0.5 * 65535, 3.0) + # 0.5 -> 0 + self._applyThrust(0.0 * 65535, 3.0) + + break + + self._cf.param.set_value('motorPowerSet.enable', 0) + time.sleep(1) + self._cf.close_link() + + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + uri = "radio://0/42/2M/E7E7E7E7E7" + + # # calibrate the scale + # le = calibScale.CalibScale(uri) + + # while not le.is_connected: + # time.sleep(0.1) + + # a, b = le.measure() + + a, b = 0.0006567048912693716, -25.86591793553 + + # collect data + le = CollectData(uri, a, b) + + # The Crazyflie lib doesn't contain anything to keep the application alive, + # so this is where your application should do something. In our case we + # are just waiting until we are disconnected. + while le.is_connected: + time.sleep(1) diff --git a/tools/system_id/plot_motor_delay.py b/tools/system_id/plot_motor_delay.py new file mode 100644 index 0000000000..76c773a360 --- /dev/null +++ b/tools/system_id/plot_motor_delay.py @@ -0,0 +1,63 @@ +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument("file", help="csv file") +args = parser.parse_args() + +data = np.loadtxt(args.file, delimiter=',', skiprows=1) + +# default +c00 = 11.093358483549203 +c10 = -39.08104165843915 +c01 = -9.525647087583181 +c20 = 20.573302305476638 +c11 = 38.42885066644033 + +def pwm2force(pwm, v): + pwm = pwm / 65536 + v = v / 4.2 + return c00 + c10 * pwm + c01 * v + c20 * pwm * pwm + c11 * pwm * v + + +# u dot = lambda (u_des - u) +l = 16 + +u_pred = [0] +dts = np.diff(data[:,0]) / 1e3 +for i, dt in enumerate(dts): + u = u_pred[-1] + pwm_des = data[i,2] + v = data[i,3] + # convert pwm -> force + f_des = pwm2force(pwm_des, v) * 4 + + u_dot = l * (f_des - u) + u_pred.append(u + u_dot * dt) +u_pred = np.array(u_pred) + +fig, ax1 = plt.subplots() + +# ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell") +# ax2 = ax1.twinx() +# ax2.plot((data[:,0] - data[0,0])/1e3, data[:,2], 'g', label="PWM") + +# ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction") + +ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell") + +# compute f desired from pwm +f_des = [] +for i in range(0, data.shape[0]): + f_des.append(pwm2force(data[i,2], data[i,3]) * 4) +f_des = np.array(f_des) + +ax1.plot((data[:,0] - data[0,0])/1e3, f_des, 'g', label="Desired") +ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction") + + +ax1.legend() + +plt.show() \ No newline at end of file From 0760296249c351857ec6bc93f1c10ca43cd67b8b Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 13 Mar 2021 16:25:34 +0100 Subject: [PATCH 19/85] fix incorrect merge --- src/hal/src/usec_time.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/hal/src/usec_time.c b/src/hal/src/usec_time.c index 074ea930e9..6971d308ea 100644 --- a/src/hal/src/usec_time.c +++ b/src/hal/src/usec_time.c @@ -24,10 +24,9 @@ * * usec_time.c - microsecond-resolution timer and timestamps. */ - #include - #include "usec_time.h" +#include "cfassert.h" #include "nvicconf.h" #include "stm32fxxx.h" From 80d82e4313c5acfdb5b54d0e700a3699dd130bd0 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 13 Mar 2021 16:33:28 +0100 Subject: [PATCH 20/85] disable tumble check, since the CF is upside down in force stand --- src/modules/src/supervisor.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index cf0037f19a..a5fe0d754c 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -116,7 +116,8 @@ void supervisorUpdate(const sensorData_t *data) { isFlying = isFlyingCheck(); - isTumbled = isTumbledCheck(data); + // isTumbled = isTumbledCheck(data); + isTumbled = false; if (isTumbled && isFlying) { stabilizerSetEmergencyStop(); } From f0532ddb7f06c194b2211d0267b806b71efb2ace Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Mar 2021 10:42:24 +0100 Subject: [PATCH 21/85] avoid compiler error for unused isTumbledCheck function --- src/modules/src/supervisor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index a5fe0d754c..739a2993be 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -116,7 +116,7 @@ void supervisorUpdate(const sensorData_t *data) { isFlying = isFlyingCheck(); - // isTumbled = isTumbledCheck(data); + isTumbled = isTumbledCheck(data); isTumbled = false; if (isTumbled && isFlying) { stabilizerSetEmergencyStop(); From fec0e0db632c77ad75f242fdd5682c13d651565a Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Mar 2021 10:52:36 +0100 Subject: [PATCH 22/85] acs37800: fix compiler errors --- Makefile | 3 ++- src/deck/drivers/src/acs37800.c | 18 +++++++++--------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/Makefile b/Makefile index 0bab83913a..6dd5268fba 100644 --- a/Makefile +++ b/Makefile @@ -36,7 +36,7 @@ include $(CRAZYFLIE_BASE)/tools/make/platform.mk CFLAGS += -DCRAZYFLIE_FW # Branch specific changes -CFLAGS += -DDECK_FORCE=bcLoadcell +CFLAGS += -DDECK_FORCE=bcLoadcell:bcRpm:bcACS37800 ######### Stabilizer configuration ########## ## These are set by the platform (see tools/make/platforms/*.mk), can be overwritten here @@ -224,6 +224,7 @@ PROJ_OBJ += activeMarkerDeck.o # PROJ_OBJ += loadcell.o PROJ_OBJ += loadcell_nau7802.o PROJ_OBJ += rpm.o +PROJ_OBJ += acs37800.o # Uart2 Link for CRTP communication is not compatible with decks using uart2 ifeq ($(UART2_LINK), 1) diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c index 78d8250e40..aa67fd2b6c 100644 --- a/src/deck/drivers/src/acs37800.c +++ b/src/deck/drivers/src/acs37800.c @@ -38,6 +38,7 @@ #include "log.h" #include "sleepus.h" #include "i2cdev.h" +#include "system.h" // EEPROM @@ -149,30 +150,29 @@ float convertSignedFixedPoint(uint32_t inputValue, uint16_t binaryPoint, uint16_ static bool asc37800Read32(uint8_t reg, uint32_t *data32) { - i2cdevReadReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)data32); + return i2cdevReadReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)data32); } static bool asc37800Write32(uint8_t reg, uint32_t data32) { - i2cdevWriteReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)&data32); + return i2cdevWriteReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)&data32); } -static bool ascFindAndSetAddress(void) +static void ascFindAndSetAddress(void) { uint8_t startAddr; uint32_t dummy = 0; - bool isRepying; for (startAddr = 96; startAddr <= 127; startAddr++) { - isRepying = i2cdevWrite(I2C1_DEV, startAddr, 1, (uint8_t *)&dummy); + bool isReplying = i2cdevWrite(I2C1_DEV, startAddr, 1, (uint8_t *)&dummy); - if (isRepying) + if (isReplying) { // Unlock EEPROM dummy = ACS_ACCESS_CODE; i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_ACCESS_CODE, 4, (uint8_t *)&dummy); - // EERPOM: write and lock i2c address to 0x7F; + // EEPROM: write and lock i2c address to 0x7F; dummy = (0x7F << 2) | (0x01 << 9); i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy); @@ -215,9 +215,9 @@ static void asc37800Init(DeckInfo *info) bool res; res = asc37800Read32(reg, &val); - DEBUG_PRINT("%d:R:0x%.2X:%X\n", res, reg, val); + DEBUG_PRINT("%d:R:0x%.2X:%lX\n", res, reg, val); res = asc37800Read32(reg+0x10, &val); - DEBUG_PRINT("%d:R:0x%.2X:%X\n\n", res, reg+0x10, val); + DEBUG_PRINT("%d:R:0x%.2X:%lX\n\n", res, reg+0x10, val); } From b5f9a8c75a94c3a4e9a9158d3f03e1138b0bffad Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Fri, 19 Mar 2021 13:51:11 +0100 Subject: [PATCH 23/85] Fixed some factional bits in ACS37800 and other improvments. --- src/deck/drivers/src/acs37800.c | 58 +++++++++++++++++++++++++-------- src/drivers/src/i2c_drv.c | 19 +++++------ 2 files changed, 54 insertions(+), 23 deletions(-) diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c index aa67fd2b6c..e8aff3dca6 100644 --- a/src/deck/drivers/src/acs37800.c +++ b/src/deck/drivers/src/acs37800.c @@ -33,12 +33,12 @@ #include "timers.h" #include "debug.h" +#include "system.h" #include "deck.h" #include "param.h" #include "log.h" #include "sleepus.h" #include "i2cdev.h" -#include "system.h" // EEPROM @@ -71,20 +71,29 @@ #define ACS_I2C_ADDR 0x7F #define ACS_ACCESS_CODE 0x4F70656E + +/** + * The magic scale factors that are measured and does not correspond at all to the datasheet... + */ #define SCALE_V (27.23f) -#define SCALE_I (618.51f) +#define SCALE_I (17.03f) static bool isInit; static uint32_t viBatRaw; -static uint32_t vavgBatRaw; +static uint32_t viBatRMS; +static uint32_t vavgBatRMS; static float vBat; +static float vBatRMS; static float vavgBat; static float iBat; +static float iBatRMS; static float iavgBat; static uint32_t pBatRaw; static float pBat; +static uint16_t currZtrim, currZtrimOld; + static void asc37800Task(void* prm); /* @@ -163,7 +172,7 @@ static void ascFindAndSetAddress(void) uint8_t startAddr; uint32_t dummy = 0; - for (startAddr = 96; startAddr <= 127; startAddr++) + for (startAddr = 96; startAddr <= 110; startAddr++) { bool isReplying = i2cdevWrite(I2C1_DEV, startAddr, 1, (uint8_t *)&dummy); @@ -173,8 +182,12 @@ static void ascFindAndSetAddress(void) dummy = ACS_ACCESS_CODE; i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_ACCESS_CODE, 4, (uint8_t *)&dummy); // EEPROM: write and lock i2c address to 0x7F; + i2cdevReadReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy); + DEBUG_PRINT("ACS37800 A:0x%.2X R:0x%.2X:%X\n", (unsigned int)startAddr, (unsigned int)ACSREG_E_IO_SEL, (unsigned int)dummy); + dummy = (0x7F << 2) | (0x01 << 9); i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy); + vTaskDelay(M2T(10)); DEBUG_PRINT("ACS37800 found on: %d. Setting address to 0x7E. Power cycle needed!\n", startAddr); } @@ -205,9 +218,11 @@ static void asc37800Init(DeckInfo *info) asc37800Write32(ACSREG_S_IO_SEL, (1 << 24) | (32 << 14) | (0x01 << 9) | (0x7F << 2)); // Set current and voltage for averaging and keep device specific (trim) settings. asc37800Read32(ACSREG_S_TRIM, &val); + currZtrim = currZtrimOld = val & 0xFF; asc37800Write32(ACSREG_S_TRIM, (val | (0 << 23) | (1 << 22))); - // Set average to 100 samples. - asc37800Write32(ACSREG_S_OFFS_AVG, ((100 << 7) | (100 << 0))); +// asc37800Write32(ACSREG_S_TRIM, ((0 << 23) | (1 << 22) | 20)); + // Set average to 10 samples. (ASC37800 sample rate 1khz) + asc37800Write32(ACSREG_S_OFFS_AVG, ((10 << 7) | (10 << 0))); DEBUG_PRINT("---------------\n"); for (int reg = 0x0B; reg <= 0x0F; reg++) @@ -215,9 +230,9 @@ static void asc37800Init(DeckInfo *info) bool res; res = asc37800Read32(reg, &val); - DEBUG_PRINT("%d:R:0x%.2X:%lX\n", res, reg, val); + DEBUG_PRINT("%d:R:0x%.2X:%X\n", (unsigned int)res, (unsigned int)reg, (unsigned int)val); res = asc37800Read32(reg+0x10, &val); - DEBUG_PRINT("%d:R:0x%.2X:%lX\n\n", res, reg+0x10, val); + DEBUG_PRINT("%d:R:0x%.2X:%X\n\n", (unsigned int)res, (unsigned int)reg+0x10, (unsigned int)val); } @@ -237,16 +252,30 @@ static void asc37800Task(void* prm) while(1) { vTaskDelayUntil(&lastWakeTime, M2T(10)); - asc37800Read32(ACSREG_I_V_RMS, &viBatRaw); + asc37800Read32(ACSREG_I_V_CODES, &viBatRaw); + asc37800Read32(ACSREG_I_V_RMS, &viBatRMS); asc37800Read32(ACSREG_P_ACTIVE, &pBatRaw); - asc37800Read32(ACSREG_VRMS_AVGS, &vavgBatRaw); + asc37800Read32(ACSREG_VRMS_AVGS, &vavgBatRMS); - vBat = convertUnsignedFixedPoint((viBatRaw & 0xFFFF), 16, 16); + vBat = convertUnsignedFixedPoint((viBatRaw & 0xFFFF), 15, 16); iBat = convertSignedFixedPoint((viBatRaw >> 16 & 0xFFFF), 15, 16); - vavgBat = convertSignedFixedPoint((vavgBatRaw & 0xFFFF), 15, 16); - iavgBat = convertSignedFixedPoint((vavgBatRaw >> 16 & 0xFFFF), 15, 16); + vBatRMS = SCALE_V * convertUnsignedFixedPoint((viBatRMS & 0xFFFF), 16, 16); + iBatRMS = SCALE_I * convertSignedFixedPoint((viBatRMS >> 16 & 0xFFFF), 16, 16); + vavgBat = SCALE_V * convertSignedFixedPoint((vavgBatRMS & 0xFFFF), 16, 16); + iavgBat = SCALE_I * convertSignedFixedPoint((vavgBatRMS >> 16 & 0xFFFF), 16, 16); pBat = convertSignedFixedPoint((pBatRaw & 0xFFFF), 15, 16); + if (currZtrimOld != currZtrim) + { + uint32_t val; + asc37800Read32(ACSREG_S_TRIM, &val); + DEBUG_PRINT("%X ->", (unsigned int)val); + val = (val & 0xFFFFFE00) | currZtrim; + DEBUG_PRINT("%X\n ", (unsigned int)val); + asc37800Write32(ACSREG_S_TRIM, val); + currZtrimOld = currZtrim; + } + // DEBUG_PRINT("V: %.3f I: %.3f P: %.3f\n", vBat, iBat, pBat); // DEBUG_PRINT("V: %f\tI: %f\tP: %f\n", vBat, iBat, pBat); // DEBUG_PRINT("Vc: %f\tIc: %f\tV: %f\tI: %f\tP: %f\n", vavgBat, iavgBat, vBat, iBat, pBat); @@ -270,12 +299,15 @@ PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcACS37800, &isInit) PARAM_GROUP_STOP(deck) PARAM_GROUP_START(asc37800) +PARAM_ADD(PARAM_UINT16, currZtrim, &currZtrim) PARAM_GROUP_STOP(asc37800) LOG_GROUP_START(asc37800) LOG_ADD(LOG_FLOAT, v, &vBat) +LOG_ADD(LOG_FLOAT, vRMS, &vBatRMS) LOG_ADD(LOG_FLOAT, v_avg, &vavgBat) LOG_ADD(LOG_FLOAT, i, &iBat) +LOG_ADD(LOG_FLOAT, iRMS, &iBatRMS) LOG_ADD(LOG_FLOAT, i_avg, &iavgBat) LOG_ADD(LOG_FLOAT, p, &pBat) LOG_GROUP_STOP(asc37800) diff --git a/src/drivers/src/i2c_drv.c b/src/drivers/src/i2c_drv.c index 23a9c6d9e0..120a38d24c 100644 --- a/src/drivers/src/i2c_drv.c +++ b/src/drivers/src/i2c_drv.c @@ -58,7 +58,7 @@ #define I2C_DEFAULT_SENSORS_CLOCK_SPEED 400000 // Definition of eeprom and deck I2C buss -#define I2C_DEFAULT_DECK_CLOCK_SPEED 100000 +#define I2C_DEFAULT_DECK_CLOCK_SPEED 400000 // Misc constants. #define I2C_NO_BLOCK 0 @@ -610,21 +610,20 @@ static void i2cdrvClearDMA(I2cDrv* i2c) static void i2cdrvDmaIsrHandler(I2cDrv* i2c) { - if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Tranasfer complete - { - i2cdrvClearDMA(i2c); - i2cNotifyClient(i2c); - // Are there any other messages to transact? - i2cTryNextMessage(i2c); - } if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag)) // Transfer error { DMA_ClearITPendingBit(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag); //TODO: Best thing we could do? i2c->txMessage.status = i2cNack; - i2cNotifyClient(i2c); - i2cTryNextMessage(i2c); } + if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Tranasfer complete + { + i2c->txMessage.status = i2cAck; + } + i2cdrvClearDMA(i2c); + i2cNotifyClient(i2c); + // Are there any other messages to transact? + i2cTryNextMessage(i2c); } From dc251e0b590d4b8b53cb0848759f9f6e04a76b2f Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Mar 2021 14:33:18 +0100 Subject: [PATCH 24/85] new gain and plots --- src/deck/drivers/src/acs37800.c | 2 +- .../{calibScale.py => calibscale.py} | 28 ++++++++--- .../{collectData.py => collect_data.py} | 48 ++++++++++--------- ...axThrust.py => collect_data_max_thrust.py} | 14 +++--- tools/system_id/collect_data_motor_delay.py | 12 ++--- tools/system_id/plot_data.py | 35 ++++++++++++-- 6 files changed, 91 insertions(+), 48 deletions(-) rename tools/system_id/{calibScale.py => calibscale.py} (87%) rename tools/system_id/{collectData.py => collect_data.py} (85%) rename tools/system_id/{collectDataMaxThrust.py => collect_data_max_thrust.py} (97%) diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c index e8aff3dca6..5d8459c061 100644 --- a/src/deck/drivers/src/acs37800.c +++ b/src/deck/drivers/src/acs37800.c @@ -75,7 +75,7 @@ /** * The magic scale factors that are measured and does not correspond at all to the datasheet... */ -#define SCALE_V (27.23f) +#define SCALE_V (24.85f) #define SCALE_I (17.03f) diff --git a/tools/system_id/calibScale.py b/tools/system_id/calibscale.py similarity index 87% rename from tools/system_id/calibScale.py rename to tools/system_id/calibscale.py index f93a9926f8..d5bb8ea222 100644 --- a/tools/system_id/calibScale.py +++ b/tools/system_id/calibscale.py @@ -1,10 +1,10 @@ """ -Example that uses a load cell to measure thrust using different RPMs +Calibrate load cell """ +import argparse import logging import time -# from threading import Timer -# from threading import Thread +import yaml import cflib.crtp from cflib.crazyflie import Crazyflie @@ -13,8 +13,6 @@ import numpy as np import matplotlib.pyplot as plt -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) class CalibScale: def __init__(self, link_uri): @@ -121,11 +119,27 @@ def measure(self, num_measurements = 100): if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument("--output", default="calibration.yaml", help="Output file containing the calibration") + parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie") + args = parser.parse_args() + + # Only output errors from the logging framework + logging.basicConfig(level=logging.ERROR) + # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) - le = CalibScale("radio://0/42/2M/E7E7E7E7E7") + le = CalibScale(args.uri) while not le.is_connected: time.sleep(0.1) - print(le.measure()) + a,b = le.measure() + result = dict() + result['a'] = a + result['b'] = b + + with open(args.output, 'w') as yaml_file: + yaml.dump(result, yaml_file) + + diff --git a/tools/system_id/collectData.py b/tools/system_id/collect_data.py similarity index 85% rename from tools/system_id/collectData.py rename to tools/system_id/collect_data.py index f9f66333f7..e5feeebcc4 100644 --- a/tools/system_id/collectData.py +++ b/tools/system_id/collect_data.py @@ -1,9 +1,10 @@ """ -Example that uses a load cell to measure thrust using different RPMs +Ramp motors and collect data using the system-id deck """ +import argparse import logging import time -from threading import Timer +import yaml from threading import Thread import cflib.crtp @@ -11,11 +12,6 @@ from cflib.crazyflie.log import LogConfig from cflib.crazyflie.localization import Localization -import calibScale - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - class CollectData: """ @@ -54,17 +50,19 @@ def _connected(self, link_uri): self._cf.param.set_value('loadcell.b', str(self.calib_b)) self._file = open("data.csv", "w+") - self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4\n"); + self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A]\n"); # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='data', period_in_ms=10) self._lg_stab.add_variable('loadcell.weight', 'float') self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') - self._lg_stab.add_variable('pm.vbat', 'float') + self._lg_stab.add_variable('pm.vbatMV', 'uint16_t') self._lg_stab.add_variable('rpm.m1', 'uint16_t') self._lg_stab.add_variable('rpm.m2', 'uint16_t') self._lg_stab.add_variable('rpm.m3', 'uint16_t') self._lg_stab.add_variable('rpm.m4', 'uint16_t') + self._lg_stab.add_variable('asc37800.v_avg', 'float') + self._lg_stab.add_variable('asc37800.i_avg', 'float') # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we @@ -94,14 +92,16 @@ def _stab_log_error(self, logconf, msg): def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print('[%d][%s]: %s' % (timestamp, logconf.name, data)) - self._file.write("{},{},{},{},{},{},{}\n".format( + self._file.write("{},{},{},{},{},{},{},{},{}\n".format( data['loadcell.weight'], data['pwm.m1_pwm'], - data['pm.vbat'], + data['pm.vbatMV']/ 1000, data['rpm.m1'], data['rpm.m2'], data['rpm.m3'], - data['rpm.m4'])) + data['rpm.m4'], + data['asc37800.v_avg'], + data['asc37800.i_avg'])) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie @@ -157,23 +157,25 @@ def _ramp_motors(self): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - uri = "radio://0/42/2M/E7E7E7E7E7" + parser = argparse.ArgumentParser() + parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration") + parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie") + args = parser.parse_args() - # calibrate the scale - le = calibScale.CalibScale(uri) + # Only output errors from the logging framework + logging.basicConfig(level=logging.ERROR) - while not le.is_connected: - time.sleep(0.1) - - a, b = le.measure() + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) - # a,b = 4.046543357726483e-05, -29.212331441036078 + with open(args.calibration, 'r') as f: + r = yaml.safe_load(f) + a = r['a'] + b = r['b'] # collect data - le = CollectData(uri, a, b) + le = CollectData(args.uri, a, b) # The Crazyflie lib doesn't contain anything to keep the application alive, # so this is where your application should do something. In our case we diff --git a/tools/system_id/collectDataMaxThrust.py b/tools/system_id/collect_data_max_thrust.py similarity index 97% rename from tools/system_id/collectDataMaxThrust.py rename to tools/system_id/collect_data_max_thrust.py index 14841da46f..3427a580f7 100644 --- a/tools/system_id/collectDataMaxThrust.py +++ b/tools/system_id/collect_data_max_thrust.py @@ -57,7 +57,7 @@ def _connected(self, link_uri): self._cf.param.set_value('loadcell.b', str(self.calib_b)) self._file = open("data.csv", "w+") - self._file.write("thrust[g],pwm,vbat[V],maxThrust[g],maxThrustVbat[V]\n"); + self._file.write("thrust[g],pwm,vbat[V],maxThrust[g],maxThrustVbat[V],rpm\n"); # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='data', period_in_ms=10) @@ -181,14 +181,14 @@ def _ramp_motors(self): uri = "radio://0/42/2M/E7E7E7E7E7" - # calibrate the scale - le = calibScale.CalibScale(uri) + # # calibrate the scale + # le = calibScale.CalibScale(uri) - while not le.is_connected: - time.sleep(0.1) + # while not le.is_connected: + # time.sleep(0.1) - a, b = le.measure() - # a,b = 4.046543357726483e-05, -29.212331441036078 + # a, b = le.measure() + a,b = 0.0006567048912693716, -25.86591793553 # collect data le = CollectData(uri, a, b) diff --git a/tools/system_id/collect_data_motor_delay.py b/tools/system_id/collect_data_motor_delay.py index 53e49f9e3b..a4f0e6b47d 100644 --- a/tools/system_id/collect_data_motor_delay.py +++ b/tools/system_id/collect_data_motor_delay.py @@ -155,15 +155,15 @@ def _ramp_motors(self): uri = "radio://0/42/2M/E7E7E7E7E7" - # # calibrate the scale - # le = calibScale.CalibScale(uri) + # calibrate the scale + le = calibScale.CalibScale(uri) - # while not le.is_connected: - # time.sleep(0.1) + while not le.is_connected: + time.sleep(0.1) - # a, b = le.measure() + a, b = le.measure() - a, b = 0.0006567048912693716, -25.86591793553 + # a, b = 0.0006567048912693716, -25.86591793553 # collect data le = CollectData(uri, a, b) diff --git a/tools/system_id/plot_data.py b/tools/system_id/plot_data.py index eeb8add62c..5b0bd0fee9 100644 --- a/tools/system_id/plot_data.py +++ b/tools/system_id/plot_data.py @@ -13,9 +13,36 @@ def loadFile(filename): vbat = data[:,2] # V, battery voltage, rpm = np.mean(data[:,3:7],axis=1) # average over all motors - fig,ax = plt.subplots() - ax.plot(rpm, thrust) - ax.set_xlabel('RPM') - ax.set_ylabel('Thrust [g]') + # fig,ax = plt.subplots(2) + # ax[0].plot(data[:,4] - data[:,3], label='M2-M1') + # ax[0].plot(data[:,5] - data[:,3], label='M3-M1') + # ax[0].plot(data[:,6] - data[:,3], label='M4-M1') + # ax[0].set_xlabel('Time') + # ax[0].set_ylabel('Relative RPM') + # ax[0].legend() + + # ax[1].plot(rpm, thrust) + # ax[1].set_xlabel('RPM') + # ax[1].set_ylabel('Thrust [g]') + + # z = np.polyfit(rpm, thrust, 2) + # p = np.poly1d(z) + + # xp = np.linspace(np.min(rpm), np.max(rpm), 100) + + # ax[1].plot(xp, p(xp), '--') + # ax[1].set_title("thrust [g] = {} * rpm^2 + {} * rpm + {}".format(z[0], z[1], z[2])) + + # plt.show() + + fig, ax = plt.subplots() + + print(data[0, 7]/vbat[0]) + + # ax.plot(thrust, vbat, label='pm.vbat') + # ax.plot(thrust, data[:, 7]/1.0955852742562724, label='i2c.v_avg') + # ax.plot(rpm, data[:, 8], label='i2c.i_avg') + ax.plot(thrust, data[:, 7] * data[:, 8], label='i2c.p_avg') + ax.legend() plt.show() From 581fa66d7232c850bc11031fdf8eab319bf29b65 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Mar 2021 17:13:32 +0100 Subject: [PATCH 25/85] updated scripts for use with brushless version --- src/drivers/src/motors.c | 2 +- tools/system_id/collect_data.py | 4 +- tools/system_id/collect_data_max_thrust.py | 31 +++++----- tools/system_id/collect_data_motor_delay.py | 65 +++++++++++++-------- tools/system_id/plot_data.py | 32 +++++----- tools/system_id/plot_motor_delay.py | 25 ++++---- tools/system_id/system_id.py | 4 +- 7 files changed, 95 insertions(+), 68 deletions(-) diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 74d0e21ee4..4ba264d845 100644 --- a/src/drivers/src/motors.c +++ b/src/drivers/src/motors.c @@ -246,7 +246,6 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust) percentage = percentage > 1.0f ? 1.0f : percentage; ratio = percentage * UINT16_MAX; } - motor_ratios[id] = ratio; } if (motorMap[id]->drvType == BRUSHLESS) @@ -257,6 +256,7 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust) { motorMap[id]->setCompare(motorMap[id]->tim, motorsConv16ToBits(ratio)); } + motor_ratios[id] = ratio; } } diff --git a/tools/system_id/collect_data.py b/tools/system_id/collect_data.py index e5feeebcc4..daebf767cd 100644 --- a/tools/system_id/collect_data.py +++ b/tools/system_id/collect_data.py @@ -121,7 +121,7 @@ def _disconnected(self, link_uri): def _ramp_motors(self): thrust_mult = 1 - thrust_step = 100 + thrust_step = 200 time_step = 0.1 thrust = 0 pitch = 0 @@ -137,10 +137,12 @@ def _ramp_motors(self): self._cf.param.set_value('motor.batCompensation', 0) self._cf.param.set_value('motorPowerSet.m1', 0) self._cf.param.set_value('motorPowerSet.enable', 2) + self._cf.param.set_value('system.forceArm', 1) while self.is_connected: #thrust >= 0: thrust += thrust_step * thrust_mult if thrust >= 65536 or thrust < 0: + # if thrust >= 20000 or thrust < 0: thrust_mult *= -1 thrust += thrust_step * thrust_mult print(thrust) diff --git a/tools/system_id/collect_data_max_thrust.py b/tools/system_id/collect_data_max_thrust.py index 3427a580f7..f88b22261c 100644 --- a/tools/system_id/collect_data_max_thrust.py +++ b/tools/system_id/collect_data_max_thrust.py @@ -1,18 +1,18 @@ """ Example that uses a load cell to measure thrust using different RPMs """ +import argparse import logging import time -from threading import Timer from threading import Thread import numpy as np +import yaml import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.localization import Localization -import calibScale # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -140,10 +140,11 @@ def _ramp_motors(self): self._cf.param.set_value('motor.batCompensation', 0) self._cf.param.set_value('motorPowerSet.m1', 0) self._cf.param.set_value('motorPowerSet.enable', 2) + self._cf.param.set_value('system.forceArm', 1) while self.is_connected: #thrust >= 0: # randomply sample PWM - thrust = int(np.random.uniform(30000, 65535)) + thrust = int(np.random.uniform(15000, 65535)) measurements = self._measure(thrust) # print(measurements) achievedThrust = np.mean(measurements[:,0]) @@ -176,22 +177,24 @@ def _ramp_motors(self): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) - - uri = "radio://0/42/2M/E7E7E7E7E7" + parser = argparse.ArgumentParser() + parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration") + parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie") + args = parser.parse_args() - # # calibrate the scale - # le = calibScale.CalibScale(uri) + # Only output errors from the logging framework + logging.basicConfig(level=logging.ERROR) - # while not le.is_connected: - # time.sleep(0.1) + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) - # a, b = le.measure() - a,b = 0.0006567048912693716, -25.86591793553 + with open(args.calibration, 'r') as f: + r = yaml.safe_load(f) + a = r['a'] + b = r['b'] # collect data - le = CollectData(uri, a, b) + le = CollectData(args.uri, a, b) # The Crazyflie lib doesn't contain anything to keep the application alive, # so this is where your application should do something. In our case we diff --git a/tools/system_id/collect_data_motor_delay.py b/tools/system_id/collect_data_motor_delay.py index a4f0e6b47d..4ec75755d6 100644 --- a/tools/system_id/collect_data_motor_delay.py +++ b/tools/system_id/collect_data_motor_delay.py @@ -1,10 +1,11 @@ """ Example that uses a load cell to measure thrust using different RPMs """ +import argparse import logging import time -from threading import Timer from threading import Thread +import yaml import cflib.crtp from cflib.crazyflie import Crazyflie @@ -12,8 +13,6 @@ from cflib.crazyflie.localization import Localization -import calibScale - # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) @@ -53,17 +52,23 @@ def _connected(self, link_uri): print(self.calib_a, self.calib_b) self._cf.param.set_value('loadcell.a', str(self.calib_a)) self._cf.param.set_value('loadcell.b', str(self.calib_b)) - # self._cf.param.set_value('loadcell.sampleRate', 7) # fastest sample rate: 320 Hz; - self._cf.param.set_value('loadcell.sampleRate', 4) + self._cf.param.set_value('loadcell.sampleRate', 7) # fastest sample rate: 320 Hz; + # self._cf.param.set_value('loadcell.sampleRate', 4) self._file = open("data.csv", "w+") - self._file.write("time[ms],weight[g],pwm,vbat[V]\n"); + self._file.write("time[ms],weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A]\n"); # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='data', period_in_ms=5) self._lg_stab.add_variable('loadcell.weight', 'float') self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') - self._lg_stab.add_variable('pm.vbat', 'float') + self._lg_stab.add_variable('pm.vbatMV', 'uint16_t') + self._lg_stab.add_variable('rpm.m1', 'uint16_t') + self._lg_stab.add_variable('rpm.m2', 'uint16_t') + self._lg_stab.add_variable('rpm.m3', 'uint16_t') + self._lg_stab.add_variable('rpm.m4', 'uint16_t') + self._lg_stab.add_variable('asc37800.v_avg', 'float') + self._lg_stab.add_variable('asc37800.i_avg', 'float') # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we @@ -99,8 +104,17 @@ def _stab_log_error(self, logconf, msg): def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print('[%d][%s]: %s' % (timestamp, logconf.name, data)) - self._file.write("{},{},{},{}\n".format(timestamp,data['loadcell.weight'], data['pwm.m1_pwm'], data['pm.vbat'])) - # pass + self._file.write("{},{},{},{},{},{},{},{},{},{}\n".format( + timestamp, + data['loadcell.weight'], + data['pwm.m1_pwm'], + data['pm.vbatMV'] / 1000, + data['rpm.m1'], + data['rpm.m2'], + data['rpm.m3'], + data['rpm.m4'], + data['asc37800.v_avg'], + data['asc37800.i_avg'])) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie @@ -130,17 +144,21 @@ def _ramp_motors(self): self._cf.param.set_value('motor.batCompensation', 0) self._cf.param.set_value('motorPowerSet.m1', 0) self._cf.param.set_value('motorPowerSet.enable', 2) + self._cf.param.set_value('system.forceArm', 1) while self.is_connected: + # base speed + self._applyThrust(10000, 3.0) + # 0 -> 1 self._applyThrust(1.0 * 65535, 3.0) # 1 -> 0 - self._applyThrust(0.0 * 65535, 3.0) + self._applyThrust(10000, 3.0) # 0 -> 0.5 self._applyThrust(0.5 * 65535, 3.0) # 0.5 -> 0 - self._applyThrust(0.0 * 65535, 3.0) + self._applyThrust(10000, 3.0) break @@ -150,23 +168,24 @@ def _ramp_motors(self): if __name__ == '__main__': - # Initialize the low-level drivers (don't list the debug drivers) - cflib.crtp.init_drivers(enable_debug_driver=False) + parser = argparse.ArgumentParser() + parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration") + parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie") + args = parser.parse_args() - uri = "radio://0/42/2M/E7E7E7E7E7" + # Only output errors from the logging framework + logging.basicConfig(level=logging.ERROR) - # calibrate the scale - le = calibScale.CalibScale(uri) - - while not le.is_connected: - time.sleep(0.1) - - a, b = le.measure() + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) - # a, b = 0.0006567048912693716, -25.86591793553 + with open(args.calibration, 'r') as f: + r = yaml.safe_load(f) + a = r['a'] + b = r['b'] # collect data - le = CollectData(uri, a, b) + le = CollectData(args.uri, a, b) # The Crazyflie lib doesn't contain anything to keep the application alive, # so this is where your application should do something. In our case we diff --git a/tools/system_id/plot_data.py b/tools/system_id/plot_data.py index 5b0bd0fee9..d106d9c8dc 100644 --- a/tools/system_id/plot_data.py +++ b/tools/system_id/plot_data.py @@ -13,28 +13,28 @@ def loadFile(filename): vbat = data[:,2] # V, battery voltage, rpm = np.mean(data[:,3:7],axis=1) # average over all motors - # fig,ax = plt.subplots(2) + fig,ax = plt.subplots(2) - # ax[0].plot(data[:,4] - data[:,3], label='M2-M1') - # ax[0].plot(data[:,5] - data[:,3], label='M3-M1') - # ax[0].plot(data[:,6] - data[:,3], label='M4-M1') - # ax[0].set_xlabel('Time') - # ax[0].set_ylabel('Relative RPM') - # ax[0].legend() + ax[0].plot(data[:,4] - data[:,3], label='M2-M1') + ax[0].plot(data[:,5] - data[:,3], label='M3-M1') + ax[0].plot(data[:,6] - data[:,3], label='M4-M1') + ax[0].set_xlabel('Time') + ax[0].set_ylabel('Relative RPM') + ax[0].legend() - # ax[1].plot(rpm, thrust) - # ax[1].set_xlabel('RPM') - # ax[1].set_ylabel('Thrust [g]') + ax[1].plot(rpm, thrust) + ax[1].set_xlabel('RPM') + ax[1].set_ylabel('Thrust [g]') - # z = np.polyfit(rpm, thrust, 2) - # p = np.poly1d(z) + z = np.polyfit(rpm, thrust, 2) + p = np.poly1d(z) - # xp = np.linspace(np.min(rpm), np.max(rpm), 100) + xp = np.linspace(np.min(rpm), np.max(rpm), 100) - # ax[1].plot(xp, p(xp), '--') - # ax[1].set_title("thrust [g] = {} * rpm^2 + {} * rpm + {}".format(z[0], z[1], z[2])) + ax[1].plot(xp, p(xp), '--') + ax[1].set_title("thrust [g] = {} * rpm^2 + {} * rpm + {}".format(z[0], z[1], z[2])) - # plt.show() + plt.show() fig, ax = plt.subplots() diff --git a/tools/system_id/plot_motor_delay.py b/tools/system_id/plot_motor_delay.py index 76c773a360..0f997df759 100644 --- a/tools/system_id/plot_motor_delay.py +++ b/tools/system_id/plot_motor_delay.py @@ -40,24 +40,25 @@ def pwm2force(pwm, v): fig, ax1 = plt.subplots() -# ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell") -# ax2 = ax1.twinx() -# ax2.plot((data[:,0] - data[0,0])/1e3, data[:,2], 'g', label="PWM") +ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell") +ax2 = ax1.twinx() +ax2.plot((data[:,0] - data[0,0])/1e3, data[:,2], 'g', label="PWM") # ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction") -ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell") +# ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell") -# compute f desired from pwm -f_des = [] -for i in range(0, data.shape[0]): - f_des.append(pwm2force(data[i,2], data[i,3]) * 4) -f_des = np.array(f_des) +# # compute f desired from pwm +# f_des = [] +# for i in range(0, data.shape[0]): +# f_des.append(pwm2force(data[i,2], data[i,3]) * 4) +# f_des = np.array(f_des) -ax1.plot((data[:,0] - data[0,0])/1e3, f_des, 'g', label="Desired") -ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction") +# ax1.plot((data[:,0] - data[0,0])/1e3, f_des, 'g', label="Desired") +# ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction") +# ax1.plot((data[:, 0] - data[0, 0])/1e3, data[:, 2], 'r', label="PWM") ax1.legend() -plt.show() \ No newline at end of file +plt.show() diff --git a/tools/system_id/system_id.py b/tools/system_id/system_id.py index 8a00263e54..a7d7e3247e 100644 --- a/tools/system_id/system_id.py +++ b/tools/system_id/system_id.py @@ -109,4 +109,6 @@ def system_id(filenames, validations=[]): print("e01: " + str(reg.coef_[1])) print("score = " + str(reg.score(Input, maxThrust))) -system_id(["cf3_1.csv", "cf3_2.csv"], validations=["cf3_1.csv", "cf3_2.csv"]) + +system_id(["data_max_thrust_brushless01.csv", "data_max_thrust_brushless03.csv"], + validations=["data_max_thrust_brushless01.csv"]) From ad50f36b93918771ca98ba44a8903d99575a6548 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Sat, 20 Mar 2021 19:38:13 +0100 Subject: [PATCH 26/85] Fixed ASC37800 scaling. --- src/deck/drivers/src/acs37800.c | 35 +++++++++++++++------------------ 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c index 5d8459c061..e9e12fb3f4 100644 --- a/src/deck/drivers/src/acs37800.c +++ b/src/deck/drivers/src/acs37800.c @@ -72,13 +72,6 @@ #define ACS_ACCESS_CODE 0x4F70656E -/** - * The magic scale factors that are measured and does not correspond at all to the datasheet... - */ -#define SCALE_V (24.85f) -#define SCALE_I (17.03f) - - static bool isInit; static uint32_t viBatRaw; static uint32_t viBatRMS; @@ -90,7 +83,9 @@ static float iBat; static float iBatRMS; static float iavgBat; static uint32_t pBatRaw; +static uint32_t pavgBatRaw; static float pBat; +static float pavgBat; static uint16_t currZtrim, currZtrimOld; @@ -216,11 +211,10 @@ static void asc37800Init(DeckInfo *info) // Enable bypass in shadow reg and set N to 32. asc37800Write32(ACSREG_S_IO_SEL, (1 << 24) | (32 << 14) | (0x01 << 9) | (0x7F << 2)); - // Set current and voltage for averaging and keep device specific (trim) settings. + // Set current and power for averaging and keep device specific (trim) settings. asc37800Read32(ACSREG_S_TRIM, &val); currZtrim = currZtrimOld = val & 0xFF; - asc37800Write32(ACSREG_S_TRIM, (val | (0 << 23) | (1 << 22))); -// asc37800Write32(ACSREG_S_TRIM, ((0 << 23) | (1 << 22) | 20)); + asc37800Write32(ACSREG_S_TRIM, (val | (1 << 23) | (1 << 22))); // Set average to 10 samples. (ASC37800 sample rate 1khz) asc37800Write32(ACSREG_S_OFFS_AVG, ((10 << 7) | (10 << 0))); @@ -254,16 +248,18 @@ static void asc37800Task(void* prm) asc37800Read32(ACSREG_I_V_CODES, &viBatRaw); asc37800Read32(ACSREG_I_V_RMS, &viBatRMS); - asc37800Read32(ACSREG_P_ACTIVE, &pBatRaw); asc37800Read32(ACSREG_VRMS_AVGS, &vavgBatRMS); - - vBat = convertUnsignedFixedPoint((viBatRaw & 0xFFFF), 15, 16); - iBat = convertSignedFixedPoint((viBatRaw >> 16 & 0xFFFF), 15, 16); - vBatRMS = SCALE_V * convertUnsignedFixedPoint((viBatRMS & 0xFFFF), 16, 16); - iBatRMS = SCALE_I * convertSignedFixedPoint((viBatRMS >> 16 & 0xFFFF), 16, 16); - vavgBat = SCALE_V * convertSignedFixedPoint((vavgBatRMS & 0xFFFF), 16, 16); - iavgBat = SCALE_I * convertSignedFixedPoint((vavgBatRMS >> 16 & 0xFFFF), 16, 16); - pBat = convertSignedFixedPoint((pBatRaw & 0xFFFF), 15, 16); + asc37800Read32(ACSREG_P_INSTANT, &pBatRaw); + asc37800Read32(ACSREG_P_ACT_AVGS, &pavgBatRaw); + + vBat = (int16_t)(viBatRaw & 0xFFFF) / 27500.0 * 0.250 * 92; + iBat = (int16_t)(viBatRaw >> 16 & 0xFFFF) / 27500.0 * 30.0; + vBatRMS = (uint16_t)(viBatRMS & 0xFFFF) / 55000.0 * 0.250 * 92; + iBatRMS = (uint16_t)(viBatRMS >> 16 & 0xFFFF) / 55000.0 * 30.0; + vavgBat = (uint16_t)(vavgBatRMS & 0xFFFF) / 55000.0 * 0.250 * 92; + iavgBat = (uint16_t)(vavgBatRMS >> 16 & 0xFFFF) / 55000.0 * 30.0; + pBat = (int16_t)(pBatRaw & 0xFFFF) / 3.08 * 92 / 1000.0; + pavgBat = (int16_t)(pavgBatRaw & 0xFFFF) / 3.08 * 92 / 1000.0; if (currZtrimOld != currZtrim) { @@ -310,4 +306,5 @@ LOG_ADD(LOG_FLOAT, i, &iBat) LOG_ADD(LOG_FLOAT, iRMS, &iBatRMS) LOG_ADD(LOG_FLOAT, i_avg, &iavgBat) LOG_ADD(LOG_FLOAT, p, &pBat) +LOG_ADD(LOG_FLOAT, p_avg, &pavgBat) LOG_GROUP_STOP(asc37800) From 713a16e3284334114aaef0a29ba93ce5317269c9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 23 Mar 2021 11:42:50 +0100 Subject: [PATCH 27/85] update readme --- tools/system_id/README.md | 50 +++++++++++++++++++++++++++++++++------ 1 file changed, 43 insertions(+), 7 deletions(-) diff --git a/tools/system_id/README.md b/tools/system_id/README.md index 4cbad6541a..0d22724dd1 100644 --- a/tools/system_id/README.md +++ b/tools/system_id/README.md @@ -1,11 +1,17 @@ -# System ID for PWM <-> Thrust +# System ID including PWM, RPM, Thrust, Voltage, Current, and Power +This folder contains scripts to measure propellers and motors using the systemId deck. ## Setup -* 100g Mini Load Cell, https://www.sparkfun.com/products/14727 -* Load Cell Amplifier HX711, https://learn.sparkfun.com/tutorials/load-cell-amplifier-hx711-breakout-hookup-guide -* Prototype deck, https://www.bitcraze.io/prototyping-deck/ +* Load cells: + * 100g: https://www.sparkfun.com/products/14727 + * 500g: https://www.sparkfun.com/products/14728 + * 1kg: https://www.adafruit.com/product/4540 +* SystemID board with the following ICs + * NAU7802 (thrust) + * ACS37800 (power) + * QRD1114 (RPM) * Calibration weights * M2/M3 Nylon screws * CF Mount (3D printed) @@ -14,7 +20,37 @@ Mount CF upside down (to avoid downwash) ## Measurement +### Common + 1. Mount CF -2. Flash firmware from dev-systemId branch -3. Run `python3 collectData.py` (includes prompts to calibrate using calibration weights and writes data.csv) -4. Evaluate using `python3 system_id.py` +2. Switch to crazyflie-lib-python with branch `feature-emergency-stop` in order to be able to CTRL+C a script safely. +3. Flash firmware from dev-systemId branch +4. Run `python3 calibscale.py --uri ` and follow the prompts to calibrate the load cell. This will create an output file `calibration.yaml` with the calibration data. The other scripts will read this file (other name can be specified as command line argument). + +### Ramping Motors + +This test will ramp the motors and write collected data to `data.csv`. + +``` +python3 collect_data.py --uri +``` + +The results can be visualized with `plot_data.py`. + +### Find (PWM,VBat)->Force, (desired Force,VBat)->PWM, (PWM, VBat)-> MaxForce + +This test will randomly select sample a PWM value and average the resulting force and measured vbat over a short period of time. Then, it will output the largest possible PWM value to estimate the maximum force that the Crazyflie can create at the current state-of-charge. The mapping functions can be automatically computed. + +``` +python3 collect_data_max_thrust.py --uri +python3 system_id.py +``` + +### Motor Delay + +This test uses a higher sampling rate and visualizes the delayed motor response given a step input. + +``` +python3 collect_data_motor_delay.py --uri +python3 plot_motor_delay.py +``` From a4059762b95f63d299700fec9571fec1057c4595 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 23 Mar 2021 13:46:24 +0100 Subject: [PATCH 28/85] switch pwm to 16-bit --- src/drivers/src/motors.c | 10 +++++----- tools/system_id/collect_data.py | 2 +- tools/system_id/collect_data_max_thrust.py | 2 +- tools/system_id/collect_data_motor_delay.py | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 4ba264d845..5329e8457c 100644 --- a/src/drivers/src/motors.c +++ b/src/drivers/src/motors.c @@ -48,7 +48,7 @@ static uint16_t motorsBLConv16ToBits(uint16_t bits); static uint16_t motorsConvBitsTo16(uint16_t bits); static uint16_t motorsConv16ToBits(uint16_t bits); -uint32_t motor_ratios[] = {0, 0, 0, 0}; +uint16_t motor_ratios[] = {0, 0, 0, 0}; void motorsPlayTone(uint16_t frequency, uint16_t duration_msec); void motorsPlayMelody(uint16_t *notes); @@ -331,10 +331,10 @@ void motorsPlayMelody(uint16_t *notes) } while (duration != 0); } LOG_GROUP_START(pwm) -LOG_ADD(LOG_UINT32, m1_pwm, &motor_ratios[0]) -LOG_ADD(LOG_UINT32, m2_pwm, &motor_ratios[1]) -LOG_ADD(LOG_UINT32, m3_pwm, &motor_ratios[2]) -LOG_ADD(LOG_UINT32, m4_pwm, &motor_ratios[3]) +LOG_ADD(LOG_UINT16, m1_pwm, &motor_ratios[0]) +LOG_ADD(LOG_UINT16, m2_pwm, &motor_ratios[1]) +LOG_ADD(LOG_UINT16, m3_pwm, &motor_ratios[2]) +LOG_ADD(LOG_UINT16, m4_pwm, &motor_ratios[3]) LOG_GROUP_STOP(pwm) PARAM_GROUP_START(motor) diff --git a/tools/system_id/collect_data.py b/tools/system_id/collect_data.py index daebf767cd..309dd38155 100644 --- a/tools/system_id/collect_data.py +++ b/tools/system_id/collect_data.py @@ -55,7 +55,7 @@ def _connected(self, link_uri): # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='data', period_in_ms=10) self._lg_stab.add_variable('loadcell.weight', 'float') - self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') + self._lg_stab.add_variable('pwm.m1_pwm', 'uint16_t') self._lg_stab.add_variable('pm.vbatMV', 'uint16_t') self._lg_stab.add_variable('rpm.m1', 'uint16_t') self._lg_stab.add_variable('rpm.m2', 'uint16_t') diff --git a/tools/system_id/collect_data_max_thrust.py b/tools/system_id/collect_data_max_thrust.py index f88b22261c..2c11cbe3ec 100644 --- a/tools/system_id/collect_data_max_thrust.py +++ b/tools/system_id/collect_data_max_thrust.py @@ -62,7 +62,7 @@ def _connected(self, link_uri): # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='data', period_in_ms=10) self._lg_stab.add_variable('loadcell.weight', 'float') - self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') + self._lg_stab.add_variable('pwm.m1_pwm', 'uint16_t') self._lg_stab.add_variable('pm.vbat', 'float') self._lg_stab.add_variable('rpm.m1', 'uint16_t') self._lg_stab.add_variable('rpm.m2', 'uint16_t') diff --git a/tools/system_id/collect_data_motor_delay.py b/tools/system_id/collect_data_motor_delay.py index 4ec75755d6..3ba6964dd9 100644 --- a/tools/system_id/collect_data_motor_delay.py +++ b/tools/system_id/collect_data_motor_delay.py @@ -61,7 +61,7 @@ def _connected(self, link_uri): # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='data', period_in_ms=5) self._lg_stab.add_variable('loadcell.weight', 'float') - self._lg_stab.add_variable('pwm.m1_pwm', 'uint32_t') + self._lg_stab.add_variable('pwm.m1_pwm', 'uint16_t') self._lg_stab.add_variable('pm.vbatMV', 'uint16_t') self._lg_stab.add_variable('rpm.m1', 'uint16_t') self._lg_stab.add_variable('rpm.m2', 'uint16_t') From 01b7cd9640e49a33410d09dcb36a5c6cdc0bb62d Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Wed, 24 Mar 2021 08:47:01 +0100 Subject: [PATCH 29/85] asc73800: Added milli-volt/amp/watt log variables --- src/deck/drivers/src/acs37800.c | 57 +++++++++++++++++++++++++++------ tools/system_id/collect_data.py | 16 +++++---- 2 files changed, 56 insertions(+), 17 deletions(-) diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c index e9e12fb3f4..0cbd46af17 100644 --- a/src/deck/drivers/src/acs37800.c +++ b/src/deck/drivers/src/acs37800.c @@ -41,6 +41,14 @@ #include "i2cdev.h" +#define I_RANGE 30.0f // Sensor amp range +#define V_DIVIDER 92 // (91k + 1k) / 1k +#define ADC_RANGE 0.250f +#define CODES_HALF_RANGE 27500.0f +#define CODES_FULL_RANGE 55000.0f +#define MILLW_TO_WATT 1000.0f +#define LSB_PER_MILLWATT 3.08f + // EEPROM #define ACSREG_E_TRIM 0x0B #define ACSREG_E_OFFS_AVG 0x0C @@ -78,7 +86,7 @@ static uint32_t viBatRMS; static uint32_t vavgBatRMS; static float vBat; static float vBatRMS; -static float vavgBat; +//static float vavgBat; static float iBat; static float iBatRMS; static float iavgBat; @@ -86,8 +94,14 @@ static uint32_t pBatRaw; static uint32_t pavgBatRaw; static float pBat; static float pavgBat; +// "milli" representation to save logging bits +static int16_t vBatMV; +static int16_t iBatMA; +static int16_t pBatMW; + static uint16_t currZtrim, currZtrimOld; +static uint8_t writeTrim; static void asc37800Task(void* prm); @@ -252,15 +266,20 @@ static void asc37800Task(void* prm) asc37800Read32(ACSREG_P_INSTANT, &pBatRaw); asc37800Read32(ACSREG_P_ACT_AVGS, &pavgBatRaw); - vBat = (int16_t)(viBatRaw & 0xFFFF) / 27500.0 * 0.250 * 92; - iBat = (int16_t)(viBatRaw >> 16 & 0xFFFF) / 27500.0 * 30.0; - vBatRMS = (uint16_t)(viBatRMS & 0xFFFF) / 55000.0 * 0.250 * 92; - iBatRMS = (uint16_t)(viBatRMS >> 16 & 0xFFFF) / 55000.0 * 30.0; - vavgBat = (uint16_t)(vavgBatRMS & 0xFFFF) / 55000.0 * 0.250 * 92; - iavgBat = (uint16_t)(vavgBatRMS >> 16 & 0xFFFF) / 55000.0 * 30.0; - pBat = (int16_t)(pBatRaw & 0xFFFF) / 3.08 * 92 / 1000.0; - pavgBat = (int16_t)(pavgBatRaw & 0xFFFF) / 3.08 * 92 / 1000.0; + vBat = (int16_t)(viBatRaw & 0xFFFF) / CODES_HALF_RANGE * ADC_RANGE * V_DIVIDER; + iBat = (int16_t)(viBatRaw >> 16 & 0xFFFF) / CODES_HALF_RANGE * I_RANGE; + vBatRMS = (uint16_t)(viBatRMS & 0xFFFF) / CODES_FULL_RANGE * ADC_RANGE * V_DIVIDER; + iBatRMS = (uint16_t)(viBatRMS >> 16 & 0xFFFF) / CODES_FULL_RANGE * I_RANGE; +// vavgBat = (uint16_t)(vavgBatRMS & 0xFFFF) / CODES_FULL_RANGE * ADC_RANGE * V_DIVIDER; + iavgBat = (uint16_t)(vavgBatRMS >> 16 & 0xFFFF) / CODES_FULL_RANGE * I_RANGE; + pBat = (int16_t)(pBatRaw & 0xFFFF) / LSB_PER_MILLWATT * V_DIVIDER / MILLW_TO_WATT; + pavgBat = (int16_t)(pavgBatRaw & 0xFFFF) / LSB_PER_MILLWATT * V_DIVIDER / MILLW_TO_WATT; + + vBatMV = (int16_t)(vBatRMS * 1000); + iBatMA = (int16_t)(iavgBat * 1000); + pBatMW = (int16_t)(pavgBat * 1000); + // Code so tuning can be done through cfclient parameters if (currZtrimOld != currZtrim) { uint32_t val; @@ -272,6 +291,21 @@ static void asc37800Task(void* prm) currZtrimOld = currZtrim; } + if (writeTrim != 0) + { + uint32_t val; + writeTrim = 0; + // Enable EEPROM Writing + asc37800Write32(ACSREG_ACCESS_CODE, ACS_ACCESS_CODE); + asc37800Read32(ACSREG_S_TRIM, &val); + DEBUG_PRINT("EEPROM write: %X", (unsigned int)val); + // Write trim reg to EEPROM + asc37800Write32(ACSREG_E_TRIM, val); + vTaskDelay(M2T(10)); + // Disable access + asc37800Write32(ACSREG_ACCESS_CODE, 0); + } + // DEBUG_PRINT("V: %.3f I: %.3f P: %.3f\n", vBat, iBat, pBat); // DEBUG_PRINT("V: %f\tI: %f\tP: %f\n", vBat, iBat, pBat); // DEBUG_PRINT("Vc: %f\tIc: %f\tV: %f\tI: %f\tP: %f\n", vavgBat, iavgBat, vBat, iBat, pBat); @@ -296,15 +330,18 @@ PARAM_GROUP_STOP(deck) PARAM_GROUP_START(asc37800) PARAM_ADD(PARAM_UINT16, currZtrim, &currZtrim) +PARAM_ADD(PARAM_UINT8, writeTrim, &writeTrim) PARAM_GROUP_STOP(asc37800) LOG_GROUP_START(asc37800) LOG_ADD(LOG_FLOAT, v, &vBat) LOG_ADD(LOG_FLOAT, vRMS, &vBatRMS) -LOG_ADD(LOG_FLOAT, v_avg, &vavgBat) +LOG_ADD(LOG_INT16, v_mV, &vBatMV) LOG_ADD(LOG_FLOAT, i, &iBat) LOG_ADD(LOG_FLOAT, iRMS, &iBatRMS) LOG_ADD(LOG_FLOAT, i_avg, &iavgBat) +LOG_ADD(LOG_INT16, i_mA, &iBatMA) LOG_ADD(LOG_FLOAT, p, &pBat) LOG_ADD(LOG_FLOAT, p_avg, &pavgBat) +LOG_ADD(LOG_INT16, p_mW, &pBatMW) LOG_GROUP_STOP(asc37800) diff --git a/tools/system_id/collect_data.py b/tools/system_id/collect_data.py index daebf767cd..7e016a845f 100644 --- a/tools/system_id/collect_data.py +++ b/tools/system_id/collect_data.py @@ -50,7 +50,7 @@ def _connected(self, link_uri): self._cf.param.set_value('loadcell.b', str(self.calib_b)) self._file = open("data.csv", "w+") - self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A]\n"); + self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A],p[W]\n"); # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='data', period_in_ms=10) @@ -61,8 +61,9 @@ def _connected(self, link_uri): self._lg_stab.add_variable('rpm.m2', 'uint16_t') self._lg_stab.add_variable('rpm.m3', 'uint16_t') self._lg_stab.add_variable('rpm.m4', 'uint16_t') - self._lg_stab.add_variable('asc37800.v_avg', 'float') - self._lg_stab.add_variable('asc37800.i_avg', 'float') + self._lg_stab.add_variable('asc37800.v_mV', 'int16_t') + self._lg_stab.add_variable('asc37800.i_mA', 'int16_t') + self._lg_stab.add_variable('asc37800.p_mW', 'int16_t') # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we @@ -92,7 +93,7 @@ def _stab_log_error(self, logconf, msg): def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print('[%d][%s]: %s' % (timestamp, logconf.name, data)) - self._file.write("{},{},{},{},{},{},{},{},{}\n".format( + self._file.write("{},{},{},{},{},{},{},{},{},{}\n".format( data['loadcell.weight'], data['pwm.m1_pwm'], data['pm.vbatMV']/ 1000, @@ -100,8 +101,9 @@ def _stab_log_data(self, timestamp, data, logconf): data['rpm.m2'], data['rpm.m3'], data['rpm.m4'], - data['asc37800.v_avg'], - data['asc37800.i_avg'])) + data['asc37800.v_mV']/ 1000, + data['asc37800.i_mA']/ 1000, + data['asc37800.p_mW']/ 1000)) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie @@ -121,7 +123,7 @@ def _disconnected(self, link_uri): def _ramp_motors(self): thrust_mult = 1 - thrust_step = 200 + thrust_step = 500 time_step = 0.1 thrust = 0 pitch = 0 From 0fe12aec5454b35e0849f2c8fe259ba07ddaf0e6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 14 Sep 2021 17:07:32 +0200 Subject: [PATCH 30/85] fix build errors after merge --- src/deck/drivers/src/acs37800.c | 2 +- src/deck/drivers/src/rpm.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c index 0cbd46af17..19ebd0ecc4 100644 --- a/src/deck/drivers/src/acs37800.c +++ b/src/deck/drivers/src/acs37800.c @@ -317,7 +317,7 @@ static const DeckDriver asc37800_deck = { .pid = 0x00, .name = "bcACS37800", - .usedGpio = DECK_USING_SCL | DECK_USING_SDA, + .usedGpio = DECK_USING_I2C, .init = asc37800Init, }; diff --git a/src/deck/drivers/src/rpm.c b/src/deck/drivers/src/rpm.c index 4879d7eff5..4c9eb19f44 100644 --- a/src/deck/drivers/src/rpm.c +++ b/src/deck/drivers/src/rpm.c @@ -311,7 +311,7 @@ static const DeckDriver rpm_deck = { .pid = 0x00, .name = "bcRpm", - .usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3 | DECK_USING_TX2 | DECK_USING_RX2, + .usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3 | DECK_USING_PA2 | DECK_USING_PA3, .init = rpmInit, }; From 4b99fcc0cd73e26ba2b6b26deb313b810ff8b9e6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 21 Jan 2022 11:57:56 +0100 Subject: [PATCH 31/85] add code to send state information to GAP8 --- Makefile | 4 +++ src/modules/src/cvmrs.c | 56 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 src/modules/src/cvmrs.c diff --git a/Makefile b/Makefile index 990e7af226..509d068fbf 100644 --- a/Makefile +++ b/Makefile @@ -27,6 +27,7 @@ PLATFORM ?= cf2 LPS_TDMA_ENABLE ?= 0 LPS_TDOA_ENABLE ?= 0 LPS_TDOA3_ENABLE ?= 0 +APP ?= 1 # Platform configuration handling @@ -269,6 +270,9 @@ PROJ_OBJ += SEGGER_RTT.o SEGGER_RTT_printf.o CFLAGS += -DDEBUG_PRINT_ON_SEGGER_RTT endif +# App Layer +PROJ_OBJ += cvmrs.o + # Libs PROJ_OBJ += libarm_math.a diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c new file mode 100644 index 0000000000..62f0db8cbf --- /dev/null +++ b/src/modules/src/cvmrs.c @@ -0,0 +1,56 @@ +#include "FreeRTOS.h" +#include "task.h" +#include "system.h" +#include "log.h" +#include "aideck.h" +#include "usec_time.h" + +#define TASK_FREQ 100 + +typedef struct +{ + uint8_t cmd; + uint64_t timestamp; // usec timestamp from STM32 + int16_t x; // compressed [mm] + int16_t y; // compressed [mm] + int16_t z; // compressed + uint32_t quat; // compressed, see quatcompress.h +} __attribute__((packed)) StatePacket_t; +// static StatePacket_t cf_state; + +static CPXPacket_t cpx_packet; + +void appMain() +{ + uint32_t lastWakeTime; + + //Wait for the system to be fully started to start stabilization loop + systemWaitStart(); + + lastWakeTime = xTaskGetTickCount(); + + logVarId_t x_id = logGetVarId("stateEstimateZ", "x"); + logVarId_t y_id = logGetVarId("stateEstimateZ", "y"); + logVarId_t z_id = logGetVarId("stateEstimateZ", "z"); + logVarId_t quat_id = logGetVarId("stateEstimateZ", "quat"); + + cpx_packet.route.destination = GAP8; + cpx_packet.route.function = APP; + cpx_packet.route.source = STM32; + StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; + state_packet->cmd = 0; + + while (1) + { + vTaskDelayUntil(&lastWakeTime, F2T(TASK_FREQ)); + + // Sending current state information to GAP8 + state_packet->timestamp = usecTimestamp(); + state_packet->x = logGetInt(x_id); + state_packet->y = logGetInt(y_id); + state_packet->z = logGetInt(z_id); + state_packet->quat = logGetInt(quat_id); + + cpxSendPacketBlocking(&cpx_packet, sizeof(StatePacket_t)); + } +} \ No newline at end of file From e22f980d955118fa37aa741581b7bb109e7dfe8f Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 26 Jan 2022 18:22:23 +0100 Subject: [PATCH 32/85] add neural-swarm2 controllers --- Makefile | 2 +- src/deck/drivers/src/zranger2.c | 3 +- src/drivers/interface/motors.h | 6 + src/drivers/src/motors.c | 95 +++- src/modules/interface/controller.h | 3 + src/modules/interface/controller_lee.h | 37 ++ .../interface/controller_mellingerSI.h | 38 ++ src/modules/interface/controller_sjc.h | 16 + src/modules/interface/math3d.h | 34 ++ src/modules/interface/stabilizer_types.h | 25 +- src/modules/src/controller.c | 6 + src/modules/src/controller_lee.c | 315 ++++++++++++ src/modules/src/controller_mellinger.c | 26 +- src/modules/src/controller_mellingerSI.c | 313 ++++++++++++ src/modules/src/controller_sjc.c | 450 ++++++++++++++++++ src/modules/src/crtp_commander_generic.c | 7 + src/modules/src/crtp_commander_high_level.c | 14 +- src/modules/src/planner.c | 6 +- src/modules/src/power_distribution_stock.c | 214 ++++++++- src/modules/src/stabilizer.c | 1 + 20 files changed, 1586 insertions(+), 25 deletions(-) create mode 100644 src/modules/interface/controller_lee.h create mode 100644 src/modules/interface/controller_mellingerSI.h create mode 100644 src/modules/interface/controller_sjc.h create mode 100644 src/modules/src/controller_lee.c create mode 100644 src/modules/src/controller_mellingerSI.c create mode 100644 src/modules/src/controller_sjc.c diff --git a/Makefile b/Makefile index 990e7af226..ba352a4a49 100644 --- a/Makefile +++ b/Makefile @@ -172,7 +172,7 @@ PROJ_OBJ += crtp_commander_generic.o crtp_localization_service.o peer_localizati PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o position_controller_indi.o PROJ_OBJ += estimator.o estimator_complementary.o -PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o controller_indi.o +PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o controller_indi.o controller_sjc.o controller_mellingerSI.o controller_lee.o PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o PROJ_OBJ += collision_avoidance.o health.o diff --git a/src/deck/drivers/src/zranger2.c b/src/deck/drivers/src/zranger2.c index d7ea5f4884..230e450805 100644 --- a/src/deck/drivers/src/zranger2.c +++ b/src/deck/drivers/src/zranger2.c @@ -55,6 +55,7 @@ static float expCoeff; static uint16_t range_last = 0; +static bool enableEKF = false; static bool isInit; NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t dev; @@ -137,7 +138,7 @@ void zRanger2Task(void* arg) // check if range is feasible and push into the estimator // the sensor should not be able to measure >5 [m], and outliers typically // occur as >8 [m] measurements - if (range_last < RANGE_OUTLIER_LIMIT) { + if (enableEKF && range_last < RANGE_OUTLIER_LIMIT) { float distance = (float)range_last * 0.001f; // Scale from [mm] to [m] float stdDev = expStdA * (1.0f + expf( expCoeff * (distance - expPointA))); rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount()); diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h index e161740699..f68b8ef25b 100644 --- a/src/drivers/interface/motors.h +++ b/src/drivers/interface/motors.h @@ -259,6 +259,12 @@ bool motorsTest(void); */ void motorsSetRatio(uint32_t id, uint16_t ratio); +// computes maximum thrust in grams given the current battery state +float motorsGetMaxThrust(); + +// set thrust for motor (in grams) +void motorsSetThrust(uint32_t id, float thrust); + /** * Get the PWM ratio of the motor 'id'. Return -1 if wrong ID. */ diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 87122d270c..827c68bf72 100644 --- a/src/drivers/src/motors.c +++ b/src/drivers/src/motors.c @@ -41,6 +41,22 @@ //Logging includes #include "log.h" +#include "param.h" +#include "math3d.h" + +// Data for CF14 +static float d00 = 0.5543364748044269; +static float d10 = 0.11442589787133063; +static float d01 = -0.5067031467944692; +static float d20 = -0.002283966554392003; +static float d11 = -0.03255320005438393; + +static float e00 = -10.291152501242268; +static float e10 = 3.926415845326646; +static float e01 = 26.077196474667165; + +static float maxThrust; +static bool new_thrust_comp = false; static uint16_t motorsBLConvBitsTo16(uint16_t bits); static uint16_t motorsBLConv16ToBits(uint16_t bits); @@ -149,6 +165,7 @@ static uint16_t motorsConv16ToBits(uint16_t bits) // // And to get the PWM as a percentage we would need to divide the // Voltage needed with the Supply voltage. + static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust) { float supply_voltage = pmGetBatteryVoltage(); @@ -160,6 +177,24 @@ static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust) * under 2V. That would suggest a damaged battery. This protects against * rushing the motors on bugs and invalid voltage levels. */ + if (new_thrust_comp) { + if (ithrust > 0) { + // desired thrust in grams + float maxThrust = motorsGetMaxThrust(); + float maxNewton = maxThrust / 1000.0f * 9.81f; + float thrustNewton = ((float)ithrust / 65535.0f) * maxNewton; + float thrustGram = thrustNewton / 9.81f * 1000.0f; + // normalized voltage + float v = supply_voltage / 4.2f; + // normalized pwm: + float pwm = d00 + d10 * thrustGram + d01 * v + d20 * thrustGram * thrustGram + d11 * thrustGram * v; + + return pwm * UINT16_MAX; + } else { + return 0; + } + } + if (supply_voltage < 2.0f) { return ithrust; @@ -303,7 +338,7 @@ bool motorsTest(void) return isInit; } -// Ithrust is thrust mapped for 65536 <==> 60 grams +// Ithrust is thrust mapped for 65536 <==> 15 grams (per rotor) void motorsSetRatio(uint32_t id, uint16_t ithrust) { if (isInit) { @@ -313,7 +348,7 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust) ratio = ithrust; -#ifdef ENABLE_THRUST_BAT_COMPENSATED + #ifdef ENABLE_THRUST_BAT_COMPENSATED if (motorMap[id]->drvType == BRUSHED) { // To make sure we provide the correct PWM given current supply voltage @@ -322,7 +357,7 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust) ratio = motorsCompensateBatteryVoltage(ithrust); motor_ratios[id] = ratio; } -#endif + #endif if (motorMap[id]->drvType == BRUSHLESS) { motorMap[id]->setCompare(motorMap[id]->tim, motorsBLConv16ToBits(ratio)); @@ -334,6 +369,41 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust) } } +// computes maximum thrust in grams given the current battery state +float motorsGetMaxThrust() +{ + // normalized voltage + float v = pmGetBatteryVoltage() / 4.2f; + // normalized pwm + float pwm = (motor_ratios[0] + motor_ratios[1] + motor_ratios[2] + motor_ratios[3]) / 4.0f / UINT16_MAX; + + maxThrust = clamp(e00 + e10 * pwm + e01 * v, 8, 50); + + return maxThrust; +} + +// set thrust for motor (in grams) +void motorsSetThrust(uint32_t id, float thrustGram) +{ + if (motorMap[id]->drvType == BRUSHED) + { + if (thrustGram > 0) { + // normalized voltage + float v = pmGetBatteryVoltage() / 4.2f; + // normalized pwm: + float pwm = d00 + d10 * thrustGram + d01 * v + d20 * thrustGram * thrustGram + d11 * thrustGram * v; + + motor_ratios[id] = pwm * UINT16_MAX; + } else { + motor_ratios[id] = 0; + } + + motorMap[id]->setCompare(motorMap[id]->tim, motorsConv16ToBits(motor_ratios[id])); + } else { + ASSERT(false); + } +} + int motorsGetRatio(uint32_t id) { int ratio; @@ -445,4 +515,23 @@ LOG_ADD(LOG_UINT32, m3_pwm, &motor_ratios[2]) * @brief Current motor 4 PWM output */ LOG_ADD(LOG_UINT32, m4_pwm, &motor_ratios[3]) + +LOG_ADD(LOG_FLOAT, maxThrust, &maxThrust) LOG_GROUP_STOP(pwm) + +PARAM_GROUP_START(pwm) + + PARAM_ADD(PARAM_FLOAT, d00, &d00) + PARAM_ADD(PARAM_FLOAT, d10, &d10) + PARAM_ADD(PARAM_FLOAT, d01, &d01) + PARAM_ADD(PARAM_FLOAT, d20, &d20) + PARAM_ADD(PARAM_FLOAT, d11, &d11) + + PARAM_ADD(PARAM_FLOAT, e00, &e00) + PARAM_ADD(PARAM_FLOAT, e10, &e10) + PARAM_ADD(PARAM_FLOAT, e01, &e01) + + PARAM_ADD(PARAM_UINT8, new_thrust_comp, &new_thrust_comp) + + +PARAM_GROUP_STOP(pwm) diff --git a/src/modules/interface/controller.h b/src/modules/interface/controller.h index a5042c00b8..3a8b57ac8b 100644 --- a/src/modules/interface/controller.h +++ b/src/modules/interface/controller.h @@ -33,6 +33,9 @@ typedef enum { ControllerTypePID, ControllerTypeMellinger, ControllerTypeINDI, + ControllerTypeSJC, + ControllerTypeMellingerSI, + ControllerTypeLee, ControllerType_COUNT, } ControllerType; diff --git a/src/modules/interface/controller_lee.h b/src/modules/interface/controller_lee.h new file mode 100644 index 0000000000..8d4b64588d --- /dev/null +++ b/src/modules/interface/controller_lee.h @@ -0,0 +1,37 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2016 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#ifndef __CONTROLLER_LEE_H__ +#define __CONTROLLER_LEE_H__ + +#include "stabilizer_types.h" + +void controllerLeeInit(void); +bool controllerLeeTest(void); +void controllerLee(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); + +#endif //__CONTROLLER_LEE_H__ diff --git a/src/modules/interface/controller_mellingerSI.h b/src/modules/interface/controller_mellingerSI.h new file mode 100644 index 0000000000..d53fcb619f --- /dev/null +++ b/src/modules/interface/controller_mellingerSI.h @@ -0,0 +1,38 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2016 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * controller_mellingerSI.h - Mellinger Controller Interface + */ +#ifndef __CONTROLLER_MELLINGER_SI_H__ +#define __CONTROLLER_MELLINGER_SI_H__ + +#include "stabilizer_types.h" + +void controllerMellingerSIInit(void); +bool controllerMellingerSITest(void); +void controllerMellingerSI(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); + +#endif //__CONTROLLER_MELLINGER_SI_H__ diff --git a/src/modules/interface/controller_sjc.h b/src/modules/interface/controller_sjc.h new file mode 100644 index 0000000000..7451597f9c --- /dev/null +++ b/src/modules/interface/controller_sjc.h @@ -0,0 +1,16 @@ +/** + * controller_sjc.h - Controller Interface for nonlinear controller + */ +#ifndef __CONTROLLER_SJC_H__ +#define __CONTROLLER_SJC_H__ + +#include "stabilizer_types.h" + +void controllerSJCInit(void); +bool controllerSJCTest(void); +void controllerSJC(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); + +#endif //__CONTROLLER_SJC_H__ diff --git a/src/modules/interface/math3d.h b/src/modules/interface/math3d.h index 673d5d6124..80d9ed6faf 100644 --- a/src/modules/interface/math3d.h +++ b/src/modules/interface/math3d.h @@ -45,6 +45,35 @@ SOFTWARE. static inline float fsqr(float x) { return x * x; } static inline float radians(float degrees) { return (M_PI_F / 180.0f) * degrees; } static inline float degrees(float radians) { return (180.0f / M_PI_F) * radians; } + +// Normalize radians to be in range [-pi,pi] +// See https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi +static inline float normalize_radians(float radians) +{ + // Copy the sign of the value in radians to the value of pi. + float signed_pi = copysignf(M_PI_F, radians); + // Set the value of difference to the appropriate signed value between pi and -pi. + radians = fmodf(radians + signed_pi, 2 * M_PI_F) - signed_pi; + return radians; +} + +// modulo operation that uses the floored definition (as in Python), rather than +// the truncated definition used for the % operator in C +// See https://en.wikipedia.org/wiki/Modulo_operation +static inline float fmodf_floored(float x, float n) +{ + return x - floorf(x / n) * n; +} + +// compute shortest signed angle between two given angles (in range [-pi, pi]) +// See https://stackoverflow.com/questions/1878907/how-can-i-find-the-difference-between-two-angles +static inline float shortest_signed_angle_radians(float start, float goal) +{ + float diff = goal - start; + float signed_diff = fmodf_floored(diff + M_PI_F, 2 * M_PI_F) - M_PI_F; + return signed_diff; +} + static inline float clamp(float value, float min, float max) { if (value < min) return min; if (value > max) return max; @@ -264,6 +293,11 @@ static inline struct vec vadd4(struct vec a, struct vec b, struct vec c, struct // TODO: make sure it compiles to optimal code return vadd(vadd(a, b), vadd(c, d)); } +// add 5 vectors. +static inline struct vec vadd5(struct vec a, struct vec b, struct vec c, struct vec d, struct vec e) { + // TODO: make sure it compiles to optimal code + return vadd(vadd(vadd(vadd(a, b), c), d), e); +} // subtract b and c from a. static inline struct vec vsub2(struct vec a, struct vec b, struct vec c) { return vadd3(a, vneg(b), vneg(c)); diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 4717f069e7..dd90e5dce9 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -167,11 +167,28 @@ typedef struct state_s { acc_t acc; // Gs (but acc.z without considering gravity) } state_t; +typedef enum control_mode_e { + controlModeLegacy = 0, // legacy mode with int16_t roll, pitch, yaw and float thrust + controlModeForceTorque = 1, + controlModeForce = 2, +} control_mode_t; + typedef struct control_s { - int16_t roll; - int16_t pitch; - int16_t yaw; - float thrust; + union { + struct { + // legacy part + int16_t roll; + int16_t pitch; + int16_t yaw; + float thrust; + }; + struct { + float thrustSI; // N + float torque[3]; // Nm + }; + float normalizedForces[4]; // 0 ... 1 + }; + control_mode_t controlMode; } control_t; typedef enum mode_e { diff --git a/src/modules/src/controller.c b/src/modules/src/controller.c index 820f2b7c72..9af41d121a 100644 --- a/src/modules/src/controller.c +++ b/src/modules/src/controller.c @@ -6,6 +6,9 @@ #include "controller_pid.h" #include "controller_mellinger.h" #include "controller_indi.h" +#include "controller_sjc.h" +#include "controller_mellingerSI.h" +#include "controller_lee.h" #define DEFAULT_CONTROLLER ControllerTypePID static ControllerType currentController = ControllerTypeAny; @@ -24,6 +27,9 @@ static ControllerFcns controllerFunctions[] = { {.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"}, {.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"}, {.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"}, + {.init = controllerSJCInit, .test = controllerSJCTest, .update = controllerSJC, .name = "SJC"}, + {.init = controllerMellingerSIInit, .test = controllerMellingerSITest, .update = controllerMellingerSI, .name = "MellingerSI"}, + {.init = controllerLeeInit, .test = controllerLeeTest, .update = controllerLee, .name = "Lee"}, }; diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c new file mode 100644 index 0000000000..ae9b496d9c --- /dev/null +++ b/src/modules/src/controller_lee.c @@ -0,0 +1,315 @@ +/* +The MIT License (MIT) + +Copyright (c) 2019 Wolfgang Hoenig + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +This controller is based on the following publication: + +Taeyoung Lee, Melvin Leok, and N. Harris McClamroch +Geometric Tracking Control of a Quadrotor UAV on SE(3) +CDC 2010 + +* Difference to Mellinger: + * Different angular velocity error + * Higher-order terms in attitude controller + +TODO: + * Switch position controller + * consider Omega_d dot (currently assumes this is zero) + +*/ + +#include + +#include "param.h" +#include "log.h" +#include "math3d.h" +#include "controller_lee.h" + +#include "usec_time.h" + +#include "FreeRTOS.h" +#include "task.h" + +#define GRAVITY_MAGNITUDE (9.81f) + +static float g_vehicleMass = 0.033; // TODO: should be CF global for other modules + +// Inertia matrix (diagonal matrix), see +// System Identification of the Crazyflie 2.0 Nano Quadrocopter +// BA theses, Julian Foerster, ETHZ +// https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 +static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 + +// Position PID +static struct vec Kpos_P = {6, 6, 6}; // Kp in paper +static float Kpos_P_limit = 100; +static struct vec Kpos_D = {4, 4, 4}; // Kv in paper +static float Kpos_D_limit = 100; +static struct vec Kpos_I = {0, 0, 0}; // not in paper +static float Kpos_I_limit = 2; +static struct vec i_error_pos; + +// Attitude PID +static struct vec KR = {10, 10, 10}; +static struct vec Komega = {0.0005, 0.0005, 0.001}; + +// Logging variables +static struct vec rpy; +static struct vec rpy_des; +static struct vec qr; +static struct vec omega; +static struct vec omega_r; +static struct vec u; + +static uint32_t ticks; + +static inline struct vec vclampscl(struct vec value, float min, float max) { + return mkvec( + clamp(value.x, min, max), + clamp(value.y, min, max), + clamp(value.z, min, max)); +} + +void controllerLeeReset(void) +{ + i_error_pos = vzero(); +} + +void controllerLeeInit(void) +{ + controllerLeeReset(); +} + +bool controllerLeeTest(void) +{ + return true; +} + +void controllerLee(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + + + if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { + return; + } + + uint64_t startTime = usecTimestamp(); + + float dt = (float)(1.0f/ATTITUDE_RATE); + + // Address inconsistency in firmware where we need to compute our own desired yaw angle + // Rate-controlled YAW is moving YAW angle setpoint + float desiredYaw = 0; //rad + if (setpoint->mode.yaw == modeVelocity) { + desiredYaw = radians(state->attitude.yaw + setpoint->attitudeRate.yaw * dt); + } else if (setpoint->mode.yaw == modeAbs) { + desiredYaw = radians(setpoint->attitude.yaw); + } else if (setpoint->mode.quat == modeAbs) { + struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); + rpy_des = quat2rpy(setpoint_quat); + desiredYaw = rpy_des.z; + } + + // qr: Desired/reference angles in rad + // struct vec qr; + + // Position controller + if ( setpoint->mode.x == modeAbs + || setpoint->mode.y == modeAbs + || setpoint->mode.z == modeAbs) { + struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); + struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); + struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); + struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + + // errors + struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); + struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); + i_error_pos = vclampscl(vadd(i_error_pos, vscl(dt, pos_e)), -Kpos_I_limit, Kpos_I_limit); + + struct vec F_d = vscl(g_vehicleMass, vadd4( + acc_d, + veltmul(Kpos_D, vel_e), + veltmul(Kpos_P, pos_e), + veltmul(Kpos_I, i_error_pos))); + + control->thrustSI = vmag(F_d); + // Reset the accumulated error while on the ground + if (control->thrustSI < 0.01f) { + controllerLeeReset(); + } + + // Use current yaw instead of desired yaw for roll/pitch + float yaw = radians(state->attitude.yaw); + qr = mkvec( + asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI), + atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z), + desiredYaw); + } else { + if (setpoint->mode.z == modeDisable) { + if (setpoint->thrust < 1000) { + control->controlMode = controlModeForceTorque; + control->thrustSI = 0; + control->torque[0] = 0; + control->torque[1] = 0; + control->torque[2] = 0; + controllerLeeReset(); + return; + } + } + // On CF2, thrust is mapped 65536 <==> 4 * 12 grams + const float max_thrust = 70.0f / 1000.0f * 9.81f; // N + control->thrustSI = setpoint->thrust / UINT16_MAX * max_thrust; + + qr = mkvec( + radians(setpoint->attitude.roll), + -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + desiredYaw); + } + + // Attitude controller + + // current rotation [R] + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + struct mat33 R = quat2rotmat(q); + + rpy = quat2rpy(q); + + // desired rotation [Rdes] + struct quat q_des = rpy2quat(qr); + struct mat33 R_des = quat2rotmat(q_des); + + rpy_des = quat2rpy(q_des); + + // rotation error + struct mat33 eRM = msub(mmul(mtranspose(R_des), R), mmul(mtranspose(R), R_des)); + + struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0])); + + // angular velocity + omega = mkvec( + radians(sensors->gyro.x), + radians(sensors->gyro.y), + radians(sensors->gyro.z)); + + struct vec omega_des = mkvec( + radians(setpoint->attitudeRate.roll), + radians(setpoint->attitudeRate.pitch), + radians(setpoint->attitudeRate.yaw)); + + omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); + + struct vec omega_error = vsub(omega, omega_r); + + // compute moments + // M = -kR eR - kw ew + w x Jw - J(w x wr) + u = vadd4( + vneg(veltmul(KR, eR)), + vneg(veltmul(Komega, omega_error)), + vcross(omega, veltmul(J, omega)), + vneg( veltmul(J, vcross(omega, omega_r))) + ); + + // if (enableNN > 1) { + // u = vsub(u, tau_a); + // } + + control->controlMode = controlModeForceTorque; + control->torque[0] = u.x; + control->torque[1] = u.y; + control->torque[2] = u.z; + + ticks = usecTimestamp() - startTime; +} + +PARAM_GROUP_START(ctrlLee) +// Attitude P +PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x) +PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y) +PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z) +// Attitude D +PARAM_ADD(PARAM_FLOAT, Kw_x, &Komega.x) +PARAM_ADD(PARAM_FLOAT, Kw_y, &Komega.y) +PARAM_ADD(PARAM_FLOAT, Kw_z, &Komega.z) + +// J +PARAM_ADD(PARAM_FLOAT, J_x, &J.x) +PARAM_ADD(PARAM_FLOAT, J_y, &J.y) +PARAM_ADD(PARAM_FLOAT, J_z, &J.z) + +// Position P +PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z) +PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit) +// Position D +PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z) +PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit) +// Position I +PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) +PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) +PARAM_GROUP_STOP(ctrlLee) + + +LOG_GROUP_START(ctrlLee) +LOG_ADD(LOG_FLOAT, torquex, &u.x) +LOG_ADD(LOG_FLOAT, torquey, &u.y) +LOG_ADD(LOG_FLOAT, torquez, &u.z) + +// current angles +LOG_ADD(LOG_FLOAT, rpyx, &rpy.x) +LOG_ADD(LOG_FLOAT, rpyy, &rpy.y) +LOG_ADD(LOG_FLOAT, rpyz, &rpy.z) + +// desired angles +LOG_ADD(LOG_FLOAT, rpydx, &rpy_des.x) +LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) +LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) + +// errors +LOG_ADD(LOG_FLOAT, i_error_posx, &i_error_pos.x) +LOG_ADD(LOG_FLOAT, i_error_posy, &i_error_pos.y) +LOG_ADD(LOG_FLOAT, i_error_posz, &i_error_pos.z) + +// omega +LOG_ADD(LOG_FLOAT, omegax, &omega.x) +LOG_ADD(LOG_FLOAT, omegay, &omega.y) +LOG_ADD(LOG_FLOAT, omegaz, &omega.z) + +// omega_r +LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x) +LOG_ADD(LOG_FLOAT, omegary, &omega_r.y) +LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) + +LOG_ADD(LOG_UINT32, ticks, &ticks) + +LOG_GROUP_STOP(ctrlLee) diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c index 4a2dd25252..5b72eeacbb 100644 --- a/src/modules/src/controller_mellinger.c +++ b/src/modules/src/controller_mellinger.c @@ -101,6 +101,10 @@ static float r_pitch; static float r_yaw; static float accelz; +static struct vec r_error; +static struct vec v_error; +static struct vec eR, ew; + void controllerMellingerReset(void) { i_error_x = 0; @@ -126,15 +130,15 @@ void controllerMellinger(control_t *control, setpoint_t *setpoint, const state_t *state, const uint32_t tick) { - struct vec r_error; - struct vec v_error; + // struct vec r_error; + // struct vec v_error; struct vec target_thrust; struct vec z_axis; float current_thrust; struct vec x_axis_desired; struct vec y_axis_desired; struct vec x_c_des; - struct vec eR, ew, M; + struct vec /*eR, ew,*/ M; float dt; float desiredYaw = 0; //deg @@ -358,4 +362,20 @@ LOG_ADD(LOG_FLOAT, zdz, &z_axis_desired.z) LOG_ADD(LOG_FLOAT, i_err_x, &i_error_x) LOG_ADD(LOG_FLOAT, i_err_y, &i_error_y) LOG_ADD(LOG_FLOAT, i_err_z, &i_error_z) + +LOG_ADD(LOG_FLOAT, ep_x, &r_error.x) +LOG_ADD(LOG_FLOAT, ep_y, &r_error.y) +LOG_ADD(LOG_FLOAT, ep_z, &r_error.z) + +LOG_ADD(LOG_FLOAT, ev_x, &v_error.x) +LOG_ADD(LOG_FLOAT, ev_y, &v_error.y) +LOG_ADD(LOG_FLOAT, ev_z, &v_error.z) + +LOG_ADD(LOG_FLOAT, eR_x, &eR.x) +LOG_ADD(LOG_FLOAT, eR_y, &eR.y) +LOG_ADD(LOG_FLOAT, eR_z, &eR.z) + +LOG_ADD(LOG_FLOAT, ew_x, &ew.x) +LOG_ADD(LOG_FLOAT, ew_y, &ew.y) +LOG_ADD(LOG_FLOAT, ew_z, &ew.z) LOG_GROUP_STOP(ctrlMel) diff --git a/src/modules/src/controller_mellingerSI.c b/src/modules/src/controller_mellingerSI.c new file mode 100644 index 0000000000..09081fba4c --- /dev/null +++ b/src/modules/src/controller_mellingerSI.c @@ -0,0 +1,313 @@ +/* +The MIT License (MIT) + +Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +This controller is based on the following publication: + +Daniel Mellinger, Vijay Kumar: +Minimum snap trajectory generation and control for quadrotors. +IEEE International Conference on Robotics and Automation (ICRA), 2011. + +We added the following: + * Integral terms (compensates for: battery voltage drop over time, unbalanced center of mass due to asymmmetries, and uneven wear on propellers and motors) + * Support to use this controller as an attitude-only controller for manual flight + + * This version uses SI units and math3d + + +TODO + * switch position controller + * remove integral part from attitude controller + * Tune yaw + +*/ + +#include + +#include "param.h" +#include "log.h" +#include "math3d.h" +#include "controller_mellingerSI.h" +#include "usec_time.h" +// #include "debug.h" +#include "power_distribution.h" + +#define GRAVITY_MAGNITUDE (9.81f) + +static float g_vehicleMass = 0.033; // TODO: should be CF global for other modules + +// Position PID +static struct vec Kpos_P = {6, 6, 6}; // Kp in paper +static float Kpos_P_limit = 100; +static struct vec Kpos_D = {4, 4, 4}; // Kv in paper +static float Kpos_D_limit = 100; +static struct vec Kpos_I = {0, 0, 0}; // not in paper +static float Kpos_I_limit = 2; +static struct vec i_error_pos; + +// Attitude PID +static struct vec KR = {10, 10, 10}; +static struct vec Komega = {0.0005, 0.0005, 0.001}; + +// Attitude I on omega +static struct vec Katt_I = {0.00025, 0.00025, 0.0005}; +static float Katt_I_limit = 2; +static struct vec i_error_att; + +// Logging variables +static struct vec rpy; +static struct vec rpy_des; +static struct vec qr; +static struct vec omega; +static struct vec omega_des; +static struct vec u; + +static uint32_t ticks; + +static inline struct vec vclampscl(struct vec value, float min, float max) { + return mkvec( + clamp(value.x, min, max), + clamp(value.y, min, max), + clamp(value.z, min, max)); +} + +void controllerMellingerSIReset(void) +{ + i_error_pos = vzero(); + i_error_att = vzero(); +} + +void controllerMellingerSIInit(void) +{ + controllerMellingerSIReset(); +} + +bool controllerMellingerSITest(void) +{ + return true; +} + +void controllerMellingerSI(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { + return; + } + + uint64_t startTime = usecTimestamp(); + + float dt = (float)(1.0f/ATTITUDE_RATE); + + // Address inconsistency in firmware where we need to compute our own desired yaw angle + // Rate-controlled YAW is moving YAW angle setpoint + float desiredYaw = 0; //rad + if (setpoint->mode.yaw == modeVelocity) { + desiredYaw = radians(state->attitude.yaw + setpoint->attitudeRate.yaw * dt); + } else if (setpoint->mode.yaw == modeAbs) { + desiredYaw = radians(setpoint->attitude.yaw); + } else if (setpoint->mode.quat == modeAbs) { + struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); + rpy_des = quat2rpy(setpoint_quat); + desiredYaw = rpy_des.z; + } + + // qr: Desired/reference angles in rad + // struct vec qr; + + // Position controller + if ( setpoint->mode.x == modeAbs + || setpoint->mode.y == modeAbs + || setpoint->mode.z == modeAbs) { + struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); + struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); + struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); + struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + + // errors + struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); + struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); + i_error_pos = vclampscl(vadd(i_error_pos, vscl(dt, pos_e)), -Kpos_I_limit, Kpos_I_limit); + + struct vec F_d = vscl(g_vehicleMass, vadd4( + acc_d, + veltmul(Kpos_D, vel_e), + veltmul(Kpos_P, pos_e), + veltmul(Kpos_I, i_error_pos))); + + control->thrustSI = vmag(F_d); + // Reset the accumulated error while on the ground + if (control->thrustSI < 0.01f) { + controllerMellingerSIReset(); + } + + // Use current yaw instead of desired yaw for roll/pitch + float yaw = radians(state->attitude.yaw); + qr = mkvec( + asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI), + atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z), + desiredYaw); + } else { + if (setpoint->mode.z == modeDisable) { + if (setpoint->thrust < 1000) { + control->controlMode = controlModeForceTorque; + control->thrustSI = 0; + control->torque[0] = 0; + control->torque[1] = 0; + control->torque[2] = 0; + controllerMellingerSIReset(); + return; + } + } + // On CF2, thrust is mapped 65536 <==> 4 * 12 grams + const float max_thrust = 4 * 12.0 / 1000.0 * 9.81; // N + control->thrustSI = setpoint->thrust / 65536.0f * max_thrust; + + qr = mkvec( + radians(setpoint->attitude.roll), + -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + radians(desiredYaw)); + } + + // Attitude controller + + // current rotation [R] + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + struct mat33 R = quat2rotmat(q); + + rpy = quat2rpy(q); + + // desired rotation [Rdes] + struct quat q_des = rpy2quat(qr); + struct mat33 R_des = quat2rotmat(q_des); + + rpy_des = quat2rpy(q_des); + + // rotation error + struct mat33 eRM = msub(mmul(mtranspose(R_des), R), mmul(mtranspose(R), R_des)); + + struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0])); + + // angular velocity + omega = mkvec( + radians(sensors->gyro.x), + radians(sensors->gyro.y), + radians(sensors->gyro.z)); + + omega_des = mkvec( + radians(setpoint->attitudeRate.roll), + radians(setpoint->attitudeRate.pitch), + radians(setpoint->attitudeRate.yaw)); + + // Integral part on omega + struct vec omega_error = vsub(omega, omega_des); + i_error_att = vclampscl(vadd(i_error_att, vscl(dt, omega_error)), -Katt_I_limit, Katt_I_limit); + + // compute moments + u = vadd3( + vneg(veltmul(KR, eR)), + vneg(veltmul(Komega, omega_error)), + vneg(veltmul(Katt_I, i_error_att))); + + control->controlMode = controlModeForceTorque; + control->torque[0] = u.x; + control->torque[1] = u.y; + control->torque[2] = u.z; + + ticks = usecTimestamp() - startTime; +} + +PARAM_GROUP_START(ctrlMelSI) +// Attitude P +PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x) +PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y) +PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z) +// Attitude D +PARAM_ADD(PARAM_FLOAT, Kw_x, &Komega.x) +PARAM_ADD(PARAM_FLOAT, Kw_y, &Komega.y) +PARAM_ADD(PARAM_FLOAT, Kw_z, &Komega.z) +// Attitude I +PARAM_ADD(PARAM_FLOAT, Katt_Ix, &Katt_I.x) +PARAM_ADD(PARAM_FLOAT, Katt_Iy, &Katt_I.y) +PARAM_ADD(PARAM_FLOAT, Katt_Iz, &Katt_I.z) +PARAM_ADD(PARAM_FLOAT, Katt_I_limit, &Katt_I_limit) + +// Position P +PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z) +PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit) +// Position D +PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z) +PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit) +// Position I +PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) +PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) + +PARAM_GROUP_STOP(ctrlMelSI) + + +LOG_GROUP_START(ctrlMelSI) +LOG_ADD(LOG_FLOAT, torquex, &u.x) +LOG_ADD(LOG_FLOAT, torquey, &u.y) +LOG_ADD(LOG_FLOAT, torquez, &u.z) + +// current angles +LOG_ADD(LOG_FLOAT, rpyx, &rpy.x) +LOG_ADD(LOG_FLOAT, rpyy, &rpy.y) +LOG_ADD(LOG_FLOAT, rpyz, &rpy.z) + +// desired angles +LOG_ADD(LOG_FLOAT, rpydx, &rpy_des.x) +LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) +LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) + +// errors +LOG_ADD(LOG_FLOAT, i_error_attx, &i_error_att.x) +LOG_ADD(LOG_FLOAT, i_error_atty, &i_error_att.y) +LOG_ADD(LOG_FLOAT, i_error_attz, &i_error_att.z) + +LOG_ADD(LOG_FLOAT, i_error_posx, &i_error_pos.x) +LOG_ADD(LOG_FLOAT, i_error_posy, &i_error_pos.y) +LOG_ADD(LOG_FLOAT, i_error_posz, &i_error_pos.z) + +// omega +LOG_ADD(LOG_FLOAT, omegax, &omega.x) +LOG_ADD(LOG_FLOAT, omegay, &omega.y) +LOG_ADD(LOG_FLOAT, omegaz, &omega.z) + +// omega_des +LOG_ADD(LOG_FLOAT, omegadx, &omega_des.x) +LOG_ADD(LOG_FLOAT, omegady, &omega_des.y) +LOG_ADD(LOG_FLOAT, omegadz, &omega_des.z) + +LOG_ADD(LOG_UINT32, ticks, &ticks) + +LOG_GROUP_STOP(ctrlMelSI) diff --git a/src/modules/src/controller_sjc.c b/src/modules/src/controller_sjc.c new file mode 100644 index 0000000000..31d1da589a --- /dev/null +++ b/src/modules/src/controller_sjc.c @@ -0,0 +1,450 @@ +/* +The MIT License (MIT) + +Copyright (c) 2019 Wolfgang Hoenig + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +This controller is based on the following publication (section 7): + +Daniel Morgan, Giri P Subramanian, Soon-Jo Chung, Fred Y Hadaegh +Swarm assignment and trajectory optimization using variable-swarm, distributed auction assignment and sequential convex programming +IJRR 2016 + +Notes: + * There is a typo in the paper, in eq 71: It should be -K (omega - omega_r) (i.e., no dot) + * We make the following changes: + 1. The first term of the control law (J omega_r_dot) is treated as a damping term on omega_r + to capture higher-order effects such as motor delays. Thus, we allow to tune this J (the J + in the second part is the real inertia matrix) + 2. We use the current yaw angle instead of the desired yaw angle to compute desired roll/pitch angles + 3. We add a D-term on omega_r - omega to account for motor delays + * This implementation is inspired by the implementation of the controller by + Giri P Subramanian for CF 1.0 + * Runtime attitude controller: 6us + * Note that a quaternion-based version is too complex for on-board execution if an analytical model + of Z^-1 and Z_dot^-1 is to be used +*/ + +#include + +#include "param.h" +#include "log.h" +#include "math3d.h" +#include "controller_sjc.h" +#include "usec_time.h" +// #include "debug.h" + +#define GRAVITY_MAGNITUDE (9.81f) + +static uint32_t ticks; + +static uint8_t trigReset; + +static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules + +// Attitude P on omega +static struct vec K = {0.0005, 0.0005, 0.001}; +// static float K_limit = 8.7; // ~500 deg/s + +// Attitude P on angle +static struct vec lambda = {10, 10, 10}; +// static float lambda_limit = M_PI / 2.0; // 90 deg + +// Attitude I on omega +static struct vec Katt_I = {0.00025, 0.00025, 0.0005}; +static float Katt_I_limit = 2; +static struct vec i_error_att; + +// D on omega_dot +static struct vec omega_error_prev; +static struct vec omega_error_dot; +static struct vec Katt_Dw = {0, 0, 0}; + +// Position gains +static struct vec Kpos_P = {12, 12, 12}; +static float Kpos_P_limit = 100; +static struct vec Kpos_D = {8, 8, 8}; +static float Kpos_D_limit = 100; +static struct vec Kpos_I = {8, 8, 8}; +static float Kpos_I_limit = 2; +static struct vec i_error_pos; + +static float Kpos_I_decay = 1.0f; + +static struct vec Kpos_A = {0, 0, 0}; +static float Kpos_A_limit = 100; + +// delay compensation idea #1 +static float T_d_last = 0; +static float T_d_lambda = 0.0; // 0 means disabled + +// Inertia matrix (diagonal matrix), see +// System Identification of the Crazyflie 2.0 Nano Quadrocopter +// BA theses, Julian Foerster, ETHZ +// https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 +static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 + +// We treat the first part of the control law (J omega_r_dot) as damping on omega_r +// by allowing to change J +static struct vec Jtune = {48e-6, 48e-6, 29e-6}; + +// logging variables +static struct vec u; +static struct vec qr; +static struct vec q; +static struct vec omega; +static struct vec omega_r; + +static struct vec qrp; +static struct vec qr_dot; + +static inline struct vec vclampscl(struct vec value, float min, float max) { + return mkvec( + clamp(value.x, min, max), + clamp(value.y, min, max), + clamp(value.z, min, max)); +} + +// subtract d, b and c from a. +static inline struct vec vsub3(struct vec a, struct vec b, struct vec c, struct vec d) { + return vadd4(a, vneg(b), vneg(c), vneg(d)); +} + +static inline struct vec vsub4(struct vec a, struct vec b, struct vec c, struct vec d, struct vec e) { + return vadd(vadd4(a, vneg(b), vneg(c), vneg(d)), vneg(e)); +} + +void controllerSJCReset(void) +{ + i_error_pos = vzero(); + i_error_att = vzero(); + omega_error_prev = vzero(); + omega_error_dot = vzero(); +} + +void controllerSJCInit(void) +{ + controllerSJCReset(); +} + +bool controllerSJCTest(void) +{ + return true; +} + +void controllerSJC(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { + return; + } + + if (trigReset == 1) { + controllerSJCReset(); + } else if (trigReset == 2) { + i_error_pos.z = 0; + } + trigReset = 0; + + float dt = (float)(1.0f/ATTITUDE_RATE); + + // Address inconsistency in firmware where we need to compute our own desired yaw angle + // Rate-controlled YAW is moving YAW angle setpoint + float desiredYaw = 0; //rad + if (setpoint->mode.yaw == modeVelocity) { + desiredYaw = radians(state->attitude.yaw + setpoint->attitudeRate.yaw * dt); + } else if (setpoint->mode.yaw == modeAbs) { + desiredYaw = radians(setpoint->attitude.yaw); + } else if (setpoint->mode.quat == modeAbs) { + struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); + struct vec rpy = quat2rpy(setpoint_quat); + desiredYaw = rpy.z; + } + + // qr: Desired/reference angles in rad + // struct vec qr; + + // Position controller + if ( setpoint->mode.x == modeAbs + || setpoint->mode.y == modeAbs + || setpoint->mode.z == modeAbs) { + + if (RATE_DO_EXECUTE(POSITION_RATE, tick)) { + struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); + struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); + struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); + struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + struct vec stateAcc = vscl(9.81, mkvec(state->acc.x, state->acc.y, state->acc.z + 1)); // Gs -> m/s^2 + + // errors + struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); + struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); + // This is for delay compensation (idea #2): feedback term on acceleration + struct vec acc_e = vclampscl(vsub(acc_d, stateAcc), -Kpos_A_limit, Kpos_A_limit); + i_error_pos = vclampscl(vadd(vscl(Kpos_I_decay, i_error_pos), vscl(dt, pos_e)), -Kpos_I_limit, Kpos_I_limit); + + struct vec F_d = vscl(g_vehicleMass, vadd5( + acc_d, + veltmul(Kpos_A, acc_e), + veltmul(Kpos_D, vel_e), + veltmul(Kpos_P, pos_e), + veltmul(Kpos_I, i_error_pos))); + + control->thrustSI = vmag(F_d); + + // This is for delay compensation (idea #1) + // replace by T_d by T = T_d + T_d dot / lambda + float T_d_dot = (control->thrustSI - T_d_last) * POSITION_RATE; + if (T_d_lambda > 0) { + control->thrustSI = control->thrustSI + T_d_dot / T_d_lambda; + } + T_d_last = control->thrustSI; + + // Reset the accumulated error while on the ground + if (control->thrustSI < 0.01f) { + controllerSJCReset(); + } + + // Use current yaw instead of desired yaw for roll/pitch + float yaw = radians(state->attitude.yaw); + qr = mkvec( + asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI), + atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z), + desiredYaw); + } + } else { + if (setpoint->mode.z == modeDisable) { + if (setpoint->thrust < 1000) { + control->controlMode = controlModeForceTorque; + control->thrustSI = 0; + control->torque[0] = 0; + control->torque[1] = 0; + control->torque[2] = 0; + controllerSJCReset(); + return; + } + } + // On CF2, thrust is mapped 65536 <==> 4 * 12 grams + const float max_thrust = 4 * 12.0 / 1000.0 * 9.81; // N + control->thrustSI = setpoint->thrust / 65536.0f * max_thrust; + + qr = mkvec( + radians(setpoint->attitude.roll), + -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + radians(desiredYaw)); + } + + // Attitude controller + // DEBUG_PRINT("q %d %d %d %d\n", (int)(qr.x * 1000), (int)(qr.y * 1000), (int)(qr.z * 1000), (int)(control->thrustSI * 1000)); + + // q: vector of three-dimensional attitude representation + // here: Euler angles in rad + q = mkvec( + radians(state->attitude.roll), + radians(-state->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + radians(state->attitude.yaw)); + + // omega: angular velocity in rad/s + omega = mkvec( + radians(sensors->gyro.x), + radians(sensors->gyro.y), + radians(sensors->gyro.z)); + + // q_dot = Z(q)*omega + // For euler angles Z(q) is defined in Giri P Subramanian MS thesis, equation 8 + // [1 sin(r)tan(p) cos(r)tan(p)] + // Z(q) = [0 cos(r) -sin(r) ] + // [0 sin(r)sec(p) cos(r)sec(p)] + // TODO: add check for division by zero? + struct vec q_dot = mkvec( + omega.x + sinf(q.x)*tanf(q.y)*omega.y + cosf(q.x)*tanf(q.y)*omega.z, + cosf(q.x) *omega.y - sinf(q.x) *omega.z, + sinf(q.x)/cosf(q.y)*omega.y + cosf(q.x)/cosf(q.y)*omega.z); + + // qr_dot desired/reference angular velocity in rad/s + qr_dot = mkvec( + radians(setpoint->attitudeRate.roll), + radians(setpoint->attitudeRate.pitch), + radians(setpoint->attitudeRate.yaw)); + + // qr_ddot desired/reference angular acceleration in rad/s^2 + struct vec qr_ddot = vzero(); + + // omega_r: desired/reference angular velocity in rad/s + // omega_r = Z(q)^-1 * (qr_dot + lambda (qr - q)) = Z(q)^-1 * qrp + // For euler angles Z(q)^-1 is: + // [1 0 -sin(p) ] + // Z(q)^-1 = [0 cos(r) cos(p)sin(r)] + // [0 -sin(r) cos(p)cos(r)] + qrp = vadd(qr_dot, veltmul(lambda, vsub(qr, q))); + omega_r = mkvec( + qrp.x + -sinf(q.y) *qrp.z, + cosf(q.x)*qrp.y + cosf(q.y)*sinf(q.x)*qrp.z, + - sinf(q.x)*qrp.y + cosf(q.y)*cosf(q.x)*qrp.z); + + // omegar_dot = Z(q)^-1_dot*qr_dot + Z(q)^-1*qr_ddot + // + Z(q)^-1_dot*lambda(qr - q) + Z(q)^-1*lambda*(qr_dot - q_dot) + // = Z(q)^-1_dot (qr_dot + lambda (qr - q)) + // + Z(q)^-1 (qr_ddot + lambda (qr_dot - q_dot)) + // = Z(q)^-1_dot qrp + Z(q)^-1 qrp_dot + // For euler angles Z(q)^-1_dot is: + // [0 0 -cos(p)p_dot ] + // Z(q)^-1_dot = [0 -sin(r)r_dot -sin(p)sin(r)p_dot+cos(p)cos(r)r_dot] + // [0 -cos(r)r_dot -cos(r)sin(p)p_dot-cos(p)sin(r)r_dot] + struct vec qrp_dot = vadd(qr_ddot, veltmul(lambda, vsub(qr_dot, q_dot))); + struct vec omega_r_dot = mkvec( + -cosf(q.y) *q_dot.y *qrp.z + qrp_dot.x -sinf(q.y) *qrp_dot.z, + -sinf(q.x)*q_dot.x*qrp.y + (-sinf(q.y)*sinf(q.x)*q_dot.y+cosf(q.y)*cosf(q.x)*q_dot.x)*qrp.z + cosf(q.x)*qrp_dot.y + cosf(q.y)*sinf(q.x)*qrp_dot.z, + -cosf(q.x)*q_dot.x*qrp.y + (-cosf(q.x)*sinf(q.y)*q_dot.y-cosf(q.y)*sinf(q.x)*q_dot.x)*qrp.z + -sinf(q.x)*qrp_dot.y + cosf(q.y)*cosf(q.x)*qrp_dot.z); + + // Integral part on omega (as in paper) + struct vec omega_error = vsub(omega, omega_r); + i_error_att = vclampscl(vadd(i_error_att, vscl(dt, omega_error)), -Katt_I_limit, Katt_I_limit); + + // D part on omega error (to deal with motor delays) + omega_error_dot = vdiv(vsub(omega_error, omega_error_prev), dt); + // filter estimate + omega_error_prev = omega_error; + + // // Integral part on angle + // struct vec q_error = vsub(qr, q); + // i_error_att = vclampscl(vadd(i_error_att, vscl(dt, q_error)), -Katt_I_limit, Katt_I_limit); + + // compute moments (note there is a typo on the paper in equation 71) + // u = J omega_r_dot - (J omega) x omega_r - K(omega - omega_r) - Katt_I \integral(w-w_r, dt) + u = vsub4( + veltmul(Jtune, omega_r_dot), + vcross(veltmul(J, omega), omega_r), + veltmul(K, omega_error), + veltmul(Katt_I, i_error_att), + veltmul(Katt_Dw, omega_error_dot)); + + control->controlMode = controlModeForceTorque; + control->torque[0] = u.x; + control->torque[1] = u.y; + control->torque[2] = u.z; +} + +PARAM_GROUP_START(ctrlSJC) +// Attitude P +PARAM_ADD(PARAM_FLOAT, Katt_Px, &lambda.x) +PARAM_ADD(PARAM_FLOAT, Katt_Py, &lambda.y) +PARAM_ADD(PARAM_FLOAT, Katt_Pz, &lambda.z) +// Attitude D +PARAM_ADD(PARAM_FLOAT, Katt_Dx, &K.x) +PARAM_ADD(PARAM_FLOAT, Katt_Dy, &K.y) +PARAM_ADD(PARAM_FLOAT, Katt_Dz, &K.z) +// Attitude I +PARAM_ADD(PARAM_FLOAT, Katt_Ix, &Katt_I.x) +PARAM_ADD(PARAM_FLOAT, Katt_Iy, &Katt_I.y) +PARAM_ADD(PARAM_FLOAT, Katt_Iz, &Katt_I.z) +PARAM_ADD(PARAM_FLOAT, Katt_I_limit, &Katt_I_limit) +// Attitude Dw +PARAM_ADD(PARAM_FLOAT, Katt_Dwx, &Katt_Dw.x) +PARAM_ADD(PARAM_FLOAT, Katt_Dwy, &Katt_Dw.y) +PARAM_ADD(PARAM_FLOAT, Katt_Dwz, &Katt_Dw.z) +// Position P +PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z) +PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit) +// Position D +PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z) +PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit) + +// Position A +PARAM_ADD(PARAM_FLOAT, Kpos_Ax, &Kpos_A.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Ay, &Kpos_A.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Az, &Kpos_A.z) +PARAM_ADD(PARAM_FLOAT, Kpos_A_limit, &Kpos_A_limit) + +// Position I +PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) +PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) +PARAM_ADD(PARAM_FLOAT, Kpos_I_decay, &Kpos_I_decay) + +// Jtune +PARAM_ADD(PARAM_FLOAT, Jtune_x, &Jtune.x) +PARAM_ADD(PARAM_FLOAT, Jtune_y, &Jtune.y) +PARAM_ADD(PARAM_FLOAT, Jtune_z, &Jtune.z) + +PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) + +PARAM_ADD(PARAM_UINT8, trigReset, &trigReset) + +// delay compensation (0 == disabled) +PARAM_ADD(PARAM_UINT8, T_d_lambda, &T_d_lambda) + +PARAM_GROUP_STOP(ctrlSJC) + +LOG_GROUP_START(ctrlSJC) +LOG_ADD(LOG_FLOAT, torquex, &u.x) +LOG_ADD(LOG_FLOAT, torquey, &u.y) +LOG_ADD(LOG_FLOAT, torquez, &u.z) +// current angles +LOG_ADD(LOG_FLOAT, qx, &q.x) +LOG_ADD(LOG_FLOAT, qy, &q.y) +LOG_ADD(LOG_FLOAT, qz, &q.z) +// desired angles +LOG_ADD(LOG_FLOAT, qrx, &qr.x) +LOG_ADD(LOG_FLOAT, qry, &qr.y) +LOG_ADD(LOG_FLOAT, qrz, &qr.z) + +// errors +LOG_ADD(LOG_FLOAT, i_error_attx, &i_error_att.x) +LOG_ADD(LOG_FLOAT, i_error_atty, &i_error_att.y) +LOG_ADD(LOG_FLOAT, i_error_attz, &i_error_att.z) + +LOG_ADD(LOG_FLOAT, i_error_posx, &i_error_pos.x) +LOG_ADD(LOG_FLOAT, i_error_posy, &i_error_pos.y) +LOG_ADD(LOG_FLOAT, i_error_posz, &i_error_pos.z) + +// omega +LOG_ADD(LOG_FLOAT, omegax, &omega.x) +LOG_ADD(LOG_FLOAT, omegay, &omega.y) +LOG_ADD(LOG_FLOAT, omegaz, &omega.z) + +// omega dot hat +LOG_ADD(LOG_FLOAT, domegax, &omega_error_dot.x) +LOG_ADD(LOG_FLOAT, domegay, &omega_error_dot.y) +LOG_ADD(LOG_FLOAT, domegaz, &omega_error_dot.z) + +// omega_r +LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x) +LOG_ADD(LOG_FLOAT, omegary, &omega_r.y) +LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) + +LOG_ADD(LOG_FLOAT, qrpx, &qrp.x) +LOG_ADD(LOG_FLOAT, qrpz, &qrp.z) + +LOG_ADD(LOG_FLOAT, qrdotz, &qr_dot.z) + +LOG_ADD(LOG_UINT32, ticks, &ticks) + +LOG_GROUP_STOP(ctrlSJC) \ No newline at end of file diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c index fd8745653d..79fef47579 100644 --- a/src/modules/src/crtp_commander_generic.c +++ b/src/modules/src/crtp_commander_generic.c @@ -35,6 +35,11 @@ #include "num.h" #include "quatcompress.h" #include "FreeRTOS.h" +#include "eventtrigger.h" + + +EVENTTRIGGER(cmdFullState) + /* The generic commander format contains a packet type and data that has to be * decoded into a setpoint_t structure. The aim is to make it future-proof @@ -338,6 +343,8 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat setpoint->mode.roll = modeDisable; setpoint->mode.pitch = modeDisable; setpoint->mode.yaw = modeDisable; + + eventTrigger(&eventTrigger_cmdFullState); } /* positionDecoder diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 9b12cdd043..6f9347fbeb 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -104,6 +104,8 @@ static StaticSemaphore_t lockTrajBuffer; static float defaultTakeoffVelocity = 0.5f; static float defaultLandingVelocity = 0.5f; +static float yawrate = 0.0f; + // Trajectory memory handling from the memory module static uint32_t handleMemGetSize(void) { return crtpCommanderHighLevelTrajectoryMemSize(); } static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer); @@ -328,7 +330,14 @@ void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat // store the last setpoint pos = ev.pos; vel = ev.vel; - yaw = ev.yaw; + + if (yawrate != 0) { + yaw += yawrate * 0.001f; + setpoint->attitude.yaw = degrees(yaw); + setpoint->attitudeRate.yaw = degrees(yawrate); + } else { + yaw = ev.yaw; + } } } @@ -807,4 +816,7 @@ PARAM_ADD_CORE(PARAM_FLOAT, vtoff, &defaultTakeoffVelocity) */ PARAM_ADD_CORE(PARAM_FLOAT, vland, &defaultLandingVelocity) + +PARAM_ADD(PARAM_FLOAT, yawrate, &yawrate) + PARAM_GROUP_STOP(hlCommander) diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index 8da0d40bc6..c0c01f434d 100644 --- a/src/modules/src/planner.c +++ b/src/modules/src/planner.c @@ -168,9 +168,13 @@ int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool r hover_yaw += curr_eval->yaw; } + // compute the shortest possible rotation towards 0 + hover_yaw = normalize_radians(hover_yaw); + float goal_yaw = hover_yaw + shortest_signed_angle_radians(hover_yaw, 0); + piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration, curr_eval->pos, curr_eval->yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc, - hover_pos, hover_yaw, vzero(), 0, vzero()); + hover_pos, hover_yaw, vzero(), goal_yaw, vzero()); p->reversed = false; p->state = TRAJECTORY_STATE_FLYING; diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c index edcdc8a755..882a62e8d8 100644 --- a/src/modules/src/power_distribution_stock.c +++ b/src/modules/src/power_distribution_stock.c @@ -31,11 +31,20 @@ #include "log.h" #include "param.h" #include "num.h" +#include "math3d.h" #include "platform.h" #include "motors.h" #include "debug.h" static bool motorSetEnable = false; +static uint8_t saturationStatus = 0; + +enum saturationBits +{ + SaturationOffsetThrust = 1, + SaturationRollPitch = 2, + SaturationYaw = 4, +}; static struct { uint32_t m1; @@ -57,6 +66,19 @@ static struct { static uint32_t idleThrust = DEFAULT_IDLE_THRUST; +float motorForce[4]; + +// static float g_thrustpart; +static float g_rollpart; +static float g_pitchpart; +static float g_yawpart; + +static float thrust; +static struct vec torque; + +static float thrust_to_torque = 0.006f; +static float arm_length = 0.046f; // m + void powerDistributionInit(void) { motorsInit(platformConfigGetMotorMapping()); @@ -81,7 +103,7 @@ void powerStop() motorsSetRatio(MOTOR_M4, 0); } -void powerDistribution(const control_t *control) +static void powerDistributionLegacy(const control_t *control) { #ifdef QUAD_FORMATION_X int16_t r = control->roll / 2.0f; @@ -100,16 +122,6 @@ void powerDistribution(const control_t *control) motorPower.m4 = limitThrust(control->thrust + control->roll - control->yaw); #endif - - if (motorSetEnable) - { - motorsSetRatio(MOTOR_M1, motorPowerSet.m1); - motorsSetRatio(MOTOR_M2, motorPowerSet.m2); - motorsSetRatio(MOTOR_M3, motorPowerSet.m3); - motorsSetRatio(MOTOR_M4, motorPowerSet.m4); - } - else - { if (motorPower.m1 < idleThrust) { motorPower.m1 = idleThrust; } @@ -127,6 +139,180 @@ void powerDistribution(const control_t *control) motorsSetRatio(MOTOR_M2, motorPower.m2); motorsSetRatio(MOTOR_M3, motorPower.m3); motorsSetRatio(MOTOR_M4, motorPower.m4); +} + +static void powerDistributionForceTorque(const control_t *control) +{ + // On CF2, thrust is mapped 65536 <==> 60 grams + thrust = control->thrustSI; + torque = mkvec(control->torque[0], control->torque[1], control->torque[2]); + + // torque.x = clamp(torque.x, -0.002, 0.002); + // torque.y = clamp(torque.y, -0.002, 0.002); + // torque.z = clamp(torque.z, -0.0005, 0.0005); + + // see https://github.com/jpreiss/libquadrotor/blob/master/src/quad_control.c + const float thrustpart = 0.25f * control->thrustSI; // N (per rotor) + const float yawpart = -0.25f * torque.z / thrust_to_torque; + + float const arm = 0.707106781f * arm_length; + const float rollpart = 0.25f / arm * torque.x; + const float pitchpart = 0.25f / arm * torque.y; + + // Thrust for each motor in N + saturationStatus = 0; + + // Simple thrust mixing +#if 1 + motorForce[0] = thrustpart - rollpart - pitchpart + yawpart; + motorForce[1] = thrustpart - rollpart + pitchpart - yawpart; + motorForce[2] = thrustpart + rollpart + pitchpart + yawpart; + motorForce[3] = thrustpart + rollpart - pitchpart - yawpart; + + // g_thrustpart = thrustpart; + g_rollpart = rollpart; + g_pitchpart = pitchpart; + g_yawpart = yawpart; +#else + // Thrust mixing similar to PX4 (see https://px4.github.io/Firmware-Doxygen/d7/d2a/mixer__multirotor_8cpp_source.html) + // 1. Mix thrust, roll, pitch without yaw + motorForce[0] = thrustpart - rollpart - pitchpart; + motorForce[1] = thrustpart - rollpart + pitchpart; + motorForce[2] = thrustpart + rollpart + pitchpart; + motorForce[3] = thrustpart + rollpart - pitchpart; + + float minForce = motorForce[0]; + float maxForce = motorForce[0]; + for (int i = 1; i < 4; ++i) { + minForce = fminf(minForce, motorForce[i]); + maxForce = fmaxf(maxForce, motorForce[i]); + } + + float deltaForce = maxForce - minForce; + float thrustOffset = 0; + float rollPitchScale = 1.0f; + + // 2. Check if we can shift thrust to avoid saturation + if (deltaForce <= max_thrust) { + if (minForce < 0) { + thrustOffset = -minForce; + } else if (maxForce > max_thrust) { + thrustOffset = -(maxForce - max_thrust); + } + } else { + // shifting thrust is not sufficient => scale roll and pitch as well + rollPitchScale = max_thrust / deltaForce; + thrustOffset = max_thrust - ((maxForce - thrustpart) * rollPitchScale + thrustpart); + } + + // 3. Mix thrust (with offset), roll/pitch (with scale) to identify margin for yaw + motorForce[0] = thrustpart+thrustOffset - rollpart*rollPitchScale - pitchpart*rollPitchScale; + motorForce[1] = thrustpart+thrustOffset - rollpart*rollPitchScale + pitchpart*rollPitchScale; + motorForce[2] = thrustpart+thrustOffset + rollpart*rollPitchScale + pitchpart*rollPitchScale; + motorForce[3] = thrustpart+thrustOffset + rollpart*rollPitchScale - pitchpart*rollPitchScale; + + float maxYawPart = max_thrust - motorForce[0]; // positive yaw can be at most + float minYawPart = 0 - motorForce[0]; // negative yaw can be at most + maxYawPart = fmaxf(maxYawPart, motorForce[1]); + maxYawPart = fmaxf(maxYawPart, max_thrust - motorForce[2]); + maxYawPart = fmaxf(maxYawPart, motorForce[3]); + minYawPart = fminf(minYawPart, motorForce[1] - max_thrust); + minYawPart = fminf(minYawPart, 0 - motorForce[2]); + minYawPart = fminf(minYawPart, motorForce[3] - max_thrust); + + float clamped_yawpart = clamp(yawpart, minYawPart, maxYawPart); + if (thrustOffset != 0) { + saturationStatus |= SaturationOffsetThrust; + } + if (rollPitchScale != 1.0f) { + saturationStatus |= SaturationRollPitch; + } + if (yawpart != clamped_yawpart) { + saturationStatus |= SaturationYaw; + } + + // 4. Final thrust mixing (unlike PX4, we do not allow to reduce thrust to get yaw response) + motorForce[0] = thrustpart+thrustOffset - rollpart*rollPitchScale - pitchpart*rollPitchScale + clamped_yawpart; + motorForce[1] = thrustpart+thrustOffset - rollpart*rollPitchScale + pitchpart*rollPitchScale - clamped_yawpart; + motorForce[2] = thrustpart+thrustOffset + rollpart*rollPitchScale + pitchpart*rollPitchScale + clamped_yawpart; + motorForce[3] = thrustpart+thrustOffset + rollpart*rollPitchScale - pitchpart*rollPitchScale - clamped_yawpart; +#endif + // for CF2, motorratio directly maps to thrust (not rpm etc.) + // Thus, we only need to scale the values here + + const float maxThrustInGram = motorsGetMaxThrust(); // g + +#if 0 + const float maxThrust = maxThrustInGram * 9.81f / 1000.0f; // N + + // yaw-torque saturation + // a) mix without yaw + motorForce[0] = thrustpart - rollpart - pitchpart; + motorForce[1] = thrustpart - rollpart + pitchpart; + motorForce[2] = thrustpart + rollpart + pitchpart; + motorForce[3] = thrustpart + rollpart - pitchpart; + + // b) find maxYawPart + float maxYawPart = maxThrust - motorForce[0]; // positive yaw can be at most + float minYawPart = 0 - motorForce[0]; // negative yaw can be at most + maxYawPart = fmaxf(maxYawPart, motorForce[1]); + maxYawPart = fmaxf(maxYawPart, maxThrust - motorForce[2]); + maxYawPart = fmaxf(maxYawPart, motorForce[3]); + minYawPart = fminf(minYawPart, motorForce[1] - maxThrust); + minYawPart = fminf(minYawPart, 0 - motorForce[2]); + minYawPart = fminf(minYawPart, motorForce[3] - maxThrust); + + float clamped_yawpart = 0; + if (minYawPart < maxYawPart) { + clamped_yawpart = clamp(yawpart, minYawPart, maxYawPart); + } + + // c) re-mix + motorForce[0] = thrustpart - rollpart - pitchpart + clamped_yawpart; + motorForce[1] = thrustpart - rollpart + pitchpart - clamped_yawpart; + motorForce[2] = thrustpart + rollpart + pitchpart + clamped_yawpart; + motorForce[3] = thrustpart + rollpart - pitchpart - clamped_yawpart; + + // collective-thrust saturation: skip for now +#endif + for (int i = 0; i < 4; ++i) { + float forceInGrams = clamp(motorForce[i] / 9.81f * 1000.0f, 0, maxThrustInGram); + motorsSetThrust(i, forceInGrams); + } +} + +static void powerDistributionForce(const control_t *control) +{ + const float maxThrustInGram = motorsGetMaxThrust(); // g + for (int i = 0; i < 4; ++i) { + float forceInGrams = control->normalizedForces[i] * maxThrustInGram; + motorsSetThrust(i, forceInGrams); + } +} + +void powerDistribution(const control_t *control) +{ + if (motorSetEnable) + { + motorsSetRatio(MOTOR_M1, motorPowerSet.m1); + motorsSetRatio(MOTOR_M2, motorPowerSet.m2); + motorsSetRatio(MOTOR_M3, motorPowerSet.m3); + motorsSetRatio(MOTOR_M4, motorPowerSet.m4); + } else { + + switch (control->controlMode) + { + case controlModeLegacy: + powerDistributionLegacy(control); + break; + case controlModeForceTorque: + powerDistributionForceTorque(control); + break; + case controlModeForce: + powerDistributionForce(control); + break; + } + } } @@ -197,3 +383,9 @@ LOG_ADD_CORE(LOG_UINT32, m3, &motorPower.m3) */ LOG_ADD_CORE(LOG_UINT32, m4, &motorPower.m4) LOG_GROUP_STOP(motor) + + +PARAM_GROUP_START(sysId) +PARAM_ADD(PARAM_FLOAT, thrust_to_torque, &thrust_to_torque) +PARAM_ADD(PARAM_FLOAT, arm_length, &arm_length) +PARAM_GROUP_STOP(sysId) \ No newline at end of file diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index a5faf53e5f..0fb58ef808 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -249,6 +249,7 @@ static void stabilizerTask(void* param) } // allow to update controller dynamically if (getControllerType() != controllerType) { + control.controlMode = controlModeLegacy; controllerInit(controllerType); controllerType = getControllerType(); } From f7f81c9bd9a88eec3078aa2ae5ddf2fadd478229 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 28 Jan 2022 14:37:34 +0100 Subject: [PATCH 33/85] add controller prototype --- Makefile | 20 +++++++++---------- bindings/cffirmware.i | 45 +++++++++++++++++++++++++++++++++++++++++++ bindings/setup.py | 3 ++- 3 files changed, 57 insertions(+), 11 deletions(-) diff --git a/Makefile b/Makefile index ba352a4a49..aa648745f5 100644 --- a/Makefile +++ b/Makefile @@ -382,15 +382,15 @@ define n endef -# Make sure that the submodules are up to date. -# Check if there are any files in the vendor directories, if not warn the user. -ifeq ($(wildcard $(CRAZYFLIE_BASE)/vendor/*/*),) - $(error $n \ - The submodules does not seem to be present, consider fetching them by:$n \ - $$ git submodule init$n \ - $$ git submodule update$n \ - ) -endif +# # Make sure that the submodules are up to date. +# # Check if there are any files in the vendor directories, if not warn the user. +# ifeq ($(wildcard $(CRAZYFLIE_BASE)/vendor/*/*),) +# $(error $n \ +# The submodules does not seem to be present, consider fetching them by:$n \ +# $$ git submodule init$n \ +# $$ git submodule update$n \ +# ) +# endif #################### Targets ############################### @@ -503,7 +503,7 @@ bindings_python: bindings/setup.py bin/cffirmware_wrap.c $(MOD_SRC)/*.c $(PYTHON) bindings/setup.py build_ext --inplace bin/cffirmware_wrap.c cffirmware.py: bindings/cffirmware.i $(MOD_INC)/*.h - swig -python -I$(MOD_INC) -o bin/cffirmware_wrap.c bindings/cffirmware.i + swig -python -I$(MOD_INC) -I$(CRAZYFLIE_BASE)/src/hal/interface -o bin/cffirmware_wrap.c bindings/cffirmware.i mv bin/cffirmware.py cffirmware.py test_python: bindings_python diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i index 6016717eb8..0979a274f7 100755 --- a/bindings/cffirmware.i +++ b/bindings/cffirmware.i @@ -1,5 +1,46 @@ %module cffirmware +%include "stdint.i" +%include "cpointer.i" +%include "carrays.i" +// %typemap(in) float[ANY] (float temp[$1_dim0]) { +// int i; +// if (!PySequence_Check($input)) { +// PyErr_SetString(PyExc_ValueError, "Expected a sequence"); +// SWIG_fail; +// } +// if (PySequence_Length($input) != $1_dim0) { +// PyErr_SetString(PyExc_ValueError, "Size mismatch. Expected $1_dim0 elements"); +// SWIG_fail; +// } +// for (i = 0; i < $1_dim0; i++) { +// PyObject *o = PySequence_GetItem($input, i); +// if (PyNumber_Check(o)) { +// temp[i] = (float) PyFloat_AsDouble(o); +// } else { +// PyErr_SetString(PyExc_ValueError, "Sequence elements must be numbers"); +// SWIG_fail; +// } +// } +// $1 = temp; +// } + +%typemap(memberin) float [ANY] { + int i; + for (i = 0; i < $1_dim0; i++) { + $1[i] = $input[i]; + } +} + +%typemap(out) float [ANY] { + int i; + $result = PyList_New($1_dim0); + for (i = 0; i < $1_dim0; i++) { + PyObject *o = PyFloat_FromDouble((double) $1[i]); + PyList_SetItem($result,i,o); + } +} + // ignore GNU specific compiler attributes #define __attribute__(x) @@ -9,14 +50,18 @@ #include "pptraj.h" #include "planner.h" #include "stabilizer_types.h" +#include "imu_types.h" #include "collision_avoidance.h" +#include "controller_sjc.h" %} %include "math3d.h" %include "pptraj.h" %include "planner.h" %include "stabilizer_types.h" +%include "imu_types.h" %include "collision_avoidance.h" +%include "controller_sjc.h" %inline %{ struct poly4d* piecewise_get(struct piecewise_traj *pp, int i) diff --git a/bindings/setup.py b/bindings/setup.py index b150c16fb7..3b48741776 100644 --- a/bindings/setup.py +++ b/bindings/setup.py @@ -15,7 +15,8 @@ "pptraj.c", "pptraj_compressed.c", "planner.c", - "collision_avoidance.c" + "collision_avoidance.c", + "controller_sjc.c", ] fw_sources = [os.path.join(fw_dir, "src/modules/src", mod) for mod in modules] From 9f8ddd21a9a9068ef76cf9d0bca92c61d68f83a8 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 21 Jan 2022 11:57:56 +0100 Subject: [PATCH 34/85] add code to send state information to GAP8 --- Makefile | 4 +++ src/modules/src/cvmrs.c | 56 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 src/modules/src/cvmrs.c diff --git a/Makefile b/Makefile index 532f7fd785..d8374fbc09 100644 --- a/Makefile +++ b/Makefile @@ -26,6 +26,7 @@ PLATFORM ?= cf2 LPS_TDMA_ENABLE ?= 0 LPS_TDOA_ENABLE ?= 0 LPS_TDOA3_ENABLE ?= 0 +APP ?= 1 # Cload is handled in a special way on windows in WSL to use the Windows python interpreter ifdef WSL_DISTRO_NAME @@ -274,6 +275,9 @@ PROJ_OBJ += SEGGER_RTT.o SEGGER_RTT_printf.o CFLAGS += -DDEBUG_PRINT_ON_SEGGER_RTT endif +# App Layer +PROJ_OBJ += cvmrs.o + # Libs PROJ_OBJ += libarm_math.a diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c new file mode 100644 index 0000000000..62f0db8cbf --- /dev/null +++ b/src/modules/src/cvmrs.c @@ -0,0 +1,56 @@ +#include "FreeRTOS.h" +#include "task.h" +#include "system.h" +#include "log.h" +#include "aideck.h" +#include "usec_time.h" + +#define TASK_FREQ 100 + +typedef struct +{ + uint8_t cmd; + uint64_t timestamp; // usec timestamp from STM32 + int16_t x; // compressed [mm] + int16_t y; // compressed [mm] + int16_t z; // compressed + uint32_t quat; // compressed, see quatcompress.h +} __attribute__((packed)) StatePacket_t; +// static StatePacket_t cf_state; + +static CPXPacket_t cpx_packet; + +void appMain() +{ + uint32_t lastWakeTime; + + //Wait for the system to be fully started to start stabilization loop + systemWaitStart(); + + lastWakeTime = xTaskGetTickCount(); + + logVarId_t x_id = logGetVarId("stateEstimateZ", "x"); + logVarId_t y_id = logGetVarId("stateEstimateZ", "y"); + logVarId_t z_id = logGetVarId("stateEstimateZ", "z"); + logVarId_t quat_id = logGetVarId("stateEstimateZ", "quat"); + + cpx_packet.route.destination = GAP8; + cpx_packet.route.function = APP; + cpx_packet.route.source = STM32; + StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; + state_packet->cmd = 0; + + while (1) + { + vTaskDelayUntil(&lastWakeTime, F2T(TASK_FREQ)); + + // Sending current state information to GAP8 + state_packet->timestamp = usecTimestamp(); + state_packet->x = logGetInt(x_id); + state_packet->y = logGetInt(y_id); + state_packet->z = logGetInt(z_id); + state_packet->quat = logGetInt(quat_id); + + cpxSendPacketBlocking(&cpx_packet, sizeof(StatePacket_t)); + } +} \ No newline at end of file From 834c0dce0a69df9e561a4fdee203472029403b65 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 1 Feb 2022 20:00:10 +0100 Subject: [PATCH 35/85] small improvements --- src/deck/drivers/src/aideck.c | 27 +++++++++++++++++++++++++++ src/modules/src/cvmrs.c | 6 ++++-- 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/deck/drivers/src/aideck.c b/src/deck/drivers/src/aideck.c index f5063acd3b..dd8a1e71e1 100644 --- a/src/deck/drivers/src/aideck.c +++ b/src/deck/drivers/src/aideck.c @@ -56,6 +56,7 @@ #include "aideck.h" static bool isInit = false; +static bool reset = false; static uint8_t byte; #define UART_TRANSPORT_HEADER_SIZE (2) @@ -195,7 +196,24 @@ static void Gap8Task(void *param) // Read out the byte the Gap8 sends and immediately send it to the console. while (1) { + // If the client triggers a reset via parameter update + if (reset) { + + // Pull reset for GAP8/ESP32 + pinMode(DECK_GPIO_IO4, OUTPUT); + digitalWrite(DECK_GPIO_IO4, LOW); + + vTaskDelay(M2T(100)); + + // Release reset for GAP8/ESP32 + digitalWrite(DECK_GPIO_IO4, HIGH); + pinMode(DECK_GPIO_IO4, INPUT_PULLUP); + + paramSetInt(paramGetVarId("aiDeck", "reset"), 0); + } + uart1GetDataWithDefaultTimeout(&byte); + consolePutchar(byte); } } @@ -330,4 +348,13 @@ PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcAIDeck, &isInit) PARAM_GROUP_STOP(deck) +PARAM_GROUP_START(aiDeck) +/** + * @brief Reset the AI deck (GAP8 and NINA) + */ +PARAM_ADD_CORE(PARAM_UINT8, reset, &reset) + +PARAM_GROUP_STOP(aiDeck) + + DECK_DRIVER(aideck_deck); diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 62f0db8cbf..56870a88d7 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -5,7 +5,7 @@ #include "aideck.h" #include "usec_time.h" -#define TASK_FREQ 100 +#define TASK_FREQ 10 typedef struct { @@ -27,7 +27,6 @@ void appMain() //Wait for the system to be fully started to start stabilization loop systemWaitStart(); - lastWakeTime = xTaskGetTickCount(); logVarId_t x_id = logGetVarId("stateEstimateZ", "x"); logVarId_t y_id = logGetVarId("stateEstimateZ", "y"); @@ -40,6 +39,9 @@ void appMain() StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; state_packet->cmd = 0; + vTaskDelay(10000); + + lastWakeTime = xTaskGetTickCount(); while (1) { vTaskDelayUntil(&lastWakeTime, F2T(TASK_FREQ)); From 2a61a869bec8d99416493c850fc31300c9925c56 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 2 Mar 2022 14:07:37 +0100 Subject: [PATCH 36/85] PID controller: improve handling of changing yaw angles --- src/modules/src/attitude_pid_controller.c | 11 +++++++---- src/modules/src/controller_pid.c | 6 +++++- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c index b88001e842..cf20e03f18 100644 --- a/src/modules/src/attitude_pid_controller.c +++ b/src/modules/src/attitude_pid_controller.c @@ -112,6 +112,11 @@ void attitudeControllerCorrectRatePID( yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true)); } +static float my_mod(float a, float n) +{ + return a - floorf(a/n) * n; +} + void attitudeControllerCorrectAttitudePID( float eulerRollActual, float eulerPitchActual, float eulerYawActual, float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, @@ -127,10 +132,8 @@ void attitudeControllerCorrectAttitudePID( // Update PID for yaw axis float yawError; yawError = eulerYawDesired - eulerYawActual; - if (yawError > 180.0f) - yawError -= 360.0f; - else if (yawError < -180.0f) - yawError += 360.0f; + // see https://stackoverflow.com/questions/1878907/how-can-i-find-the-difference-between-two-angles + yawError = my_mod(yawError + 180, 360) - 180; pidSetError(&pidYaw, yawError); *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false); } diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c index 7e66dba109..f1e359aad0 100644 --- a/src/modules/src/controller_pid.c +++ b/src/modules/src/controller_pid.c @@ -79,8 +79,12 @@ void controllerPid(control_t *control, setpoint_t *setpoint, attitudeDesired.yaw = state->attitude.yaw - YAW_MAX_DELTA; } #endif - } else { + } else if (setpoint->mode.yaw == modeAbs) { attitudeDesired.yaw = setpoint->attitude.yaw; + } else if (setpoint->mode.quat == modeAbs) { + struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); + struct vec rpy = quat2rpy(setpoint_quat); + attitudeDesired.yaw = degrees(rpy.z); } attitudeDesired.yaw = capAngle(attitudeDesired.yaw); From 753e405202a5762ebab0995550485526d3e6e43d Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 29 Mar 2022 15:35:05 +0200 Subject: [PATCH 37/85] fix build error --- src/modules/src/Kbuild | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/src/Kbuild b/src/modules/src/Kbuild index dc5cb158f5..9813404302 100644 --- a/src/modules/src/Kbuild +++ b/src/modules/src/Kbuild @@ -8,9 +8,12 @@ obj-y += commander.o obj-y += comm.o obj-y += console.o obj-y += controller_indi.o +obj-y += controller_lee.o obj-y += controller_mellinger.o +obj-y += controller_mellingerSI.o obj-y += controller.o obj-y += controller_pid.o +obj-y += controller_sjc.o obj-y += crtp_commander_generic.o obj-y += crtp_commander_high_level.o obj-y += crtp_commander.o From 39018891f653325d234dd0b0696aee6d30c90c15 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 31 Mar 2022 15:29:10 +0200 Subject: [PATCH 38/85] Merge origin/master -> origin/cvmrs --- .github/workflows/CI.yml | 80 +- .github/workflows/release.yml | 74 + .gitignore | 12 + CONTRIBUTING.md | 44 - Kconfig | 184 ++ Makefile | 515 +--- README.md | 7 + app_api/Kbuild | 1 + app_api/Kconfig | 5 + app_api/Makefile | 27 +- app_api/app-config | 4 + app_api/src/Kbuild | 1 + app_api/src/app_main.c | 2 + configs/all.config | 12 + configs/app_api.conf | 1 + configs/bigquad.conf | 4 + configs/bolt_defconfig | 5 + configs/bosch.conf | 1 + configs/cf2_defconfig | 4 + configs/defconfig | 1 + configs/loco_tdma.conf | 5 + configs/loco_tdoa2.conf | 2 + configs/loco_tdoa3.conf | 2 + configs/loco_tdoa3_all.conf | 4 + configs/tag_defconfig | 8 + docs/_data/menu.yml | 3 + docs/building-and-flashing/build.md | 94 +- docs/development/create_platform.md | 227 ++ docs/development/howto.md | 19 +- docs/development/kbuild.md | 59 + docs/development/oot.md | 54 +- docs/development/openocd_gdb_debugging.md | 4 +- docs/development/starting_development.md | 2 +- docs/functional-areas/crtp/crtp_console.md | 23 +- docs/functional-areas/crtp/crtp_link.md | 37 + docs/functional-areas/crtp/crtp_platform.md | 117 + docs/functional-areas/crtp/index.md | 154 +- docs/functional-areas/lighthouse/index.md | 3 + .../lighthouse/multi_base_stations.md | 52 + .../lighthouse/positioning_methods.md | 2 +- .../memory-subsystem/MEM_TYPE_DECK_MEM.md | 129 +- .../sensor-to-control/commanders_setpoints.md | 2 +- .../configure_estimator_controller.md | 4 +- .../sensor-to-control/controllers.md | 2 +- docs/images/kbuild1.png | Bin 0 -> 69727 bytes docs/images/kbuild2.png | Bin 0 -> 128933 bytes docs/index.md | 2 +- docs/userguides/app_layer.md | 52 +- docs/userguides/deck.md | 12 +- docs/userguides/platform.md | 59 + examples/app_appchannel_test/Kbuild | 1 + examples/app_appchannel_test/Makefile | 27 +- examples/app_appchannel_test/app-config | 3 + examples/app_appchannel_test/src/Kbuild | 1 + examples/app_hello_world-cpp/Kbuild | 1 + examples/app_hello_world-cpp/Makefile | 28 + examples/app_hello_world-cpp/README.md | 17 + examples/app_hello_world-cpp/app-config | 3 + examples/app_hello_world-cpp/src/Kbuild | 1 + .../app_hello_world-cpp/src/hello_world.cpp | 70 + examples/app_hello_world/Kbuild | 1 + examples/app_hello_world/Makefile | 32 +- examples/app_hello_world/app-config | 3 + .../app_hello_world/other/headerInOtherDir.h | 3 + examples/app_hello_world/src/Kbuild | 1 + examples/app_hello_world/src/hello_world.c | 6 +- examples/app_internal_param_log/Kbuild | 1 + examples/app_internal_param_log/Makefile | 27 +- examples/app_internal_param_log/app-config | 3 + examples/app_internal_param_log/src/Kbuild | 1 + examples/app_peer_to_peer/Kbuild | 1 + examples/app_peer_to_peer/Makefile | 27 +- examples/app_peer_to_peer/app-config | 3 + examples/app_peer_to_peer/src/Kbuild | 1 + examples/demos/app_push_demo/Kbuild | 1 + examples/demos/app_push_demo/Makefile | 28 +- examples/demos/app_push_demo/app-config | 3 + examples/demos/app_push_demo/src/Kbuild | 1 + examples/demos/app_wall_following_demo/Kbuild | 1 + .../demos/app_wall_following_demo/Makefile | 29 +- .../demos/app_wall_following_demo/app-config | 3 + .../demos/app_wall_following_demo/src/Kbuild | 2 + examples/demos/swarm_demo/Kbuild | 1 + examples/demos/swarm_demo/Makefile | 37 +- examples/demos/swarm_demo/app-config | 3 + include/config/cc/optimize/for/performance.h | 0 include/config/cross/compile.h | 0 include/config/enable/foo/bar.h | 0 include/config/estimator/name.h | 0 include/config/log/enable.h | 0 include/config/log/error.h | 0 include/config/log/timestamp.h | 0 module.json | 3 + scripts/Kbuild.include | 391 +++ scripts/Makefile | 2 + scripts/Makefile.build | 335 +++ scripts/Makefile.clean | 91 + scripts/Makefile.extrawarn | 71 + scripts/Makefile.host | 128 + scripts/Makefile.lib | 296 ++ scripts/basic/.gitignore | 2 + scripts/basic/Makefile | 16 + scripts/basic/bin2c.c | 35 + scripts/basic/fixdep.c | 479 +++ scripts/checkincludes.pl | 89 + scripts/gcc-goto.sh | 21 + scripts/headerdep.pl | 192 ++ scripts/kconfig/.gitignore | 22 + scripts/kconfig/Makefile | 294 ++ scripts/kconfig/POTFILES.in | 12 + scripts/kconfig/check.sh | 13 + scripts/kconfig/conf.c | 723 +++++ scripts/kconfig/confdata.c | 1249 ++++++++ scripts/kconfig/expr.c | 1206 ++++++++ scripts/kconfig/expr.h | 238 ++ scripts/kconfig/gconf.c | 1521 ++++++++++ scripts/kconfig/gconf.glade | 661 +++++ scripts/kconfig/images.c | 326 +++ scripts/kconfig/kxgettext.c | 235 ++ scripts/kconfig/list.h | 131 + scripts/kconfig/lkc.h | 186 ++ scripts/kconfig/lkc_proto.h | 52 + scripts/kconfig/lxdialog/.gitignore | 4 + scripts/kconfig/lxdialog/BIG.FAT.WARNING | 4 + scripts/kconfig/lxdialog/check-lxdialog.sh | 91 + scripts/kconfig/lxdialog/checklist.c | 332 +++ scripts/kconfig/lxdialog/dialog.h | 257 ++ scripts/kconfig/lxdialog/inputbox.c | 301 ++ scripts/kconfig/lxdialog/menubox.c | 437 +++ scripts/kconfig/lxdialog/textbox.c | 408 +++ scripts/kconfig/lxdialog/util.c | 713 +++++ scripts/kconfig/lxdialog/yesno.c | 114 + scripts/kconfig/mconf.c | 1047 +++++++ scripts/kconfig/menu.c | 697 +++++ scripts/kconfig/merge_config.sh | 170 ++ scripts/kconfig/nconf.c | 1561 ++++++++++ scripts/kconfig/nconf.gui.c | 656 +++++ scripts/kconfig/nconf.h | 96 + scripts/kconfig/qconf.cc | 1870 ++++++++++++ scripts/kconfig/qconf.h | 330 +++ scripts/kconfig/streamline_config.pl | 681 +++++ scripts/kconfig/symbol.c | 1392 +++++++++ scripts/kconfig/util.c | 147 + scripts/kconfig/zconf.gperf | 49 + scripts/kconfig/zconf.hash.c_shipped | 293 ++ scripts/kconfig/zconf.l | 374 +++ scripts/kconfig/zconf.lex.c_shipped | 2473 ++++++++++++++++ scripts/kconfig/zconf.tab.c_shipped | 2580 +++++++++++++++++ scripts/kconfig/zconf.y | 742 +++++ scripts/mkmakefile | 52 + src/Kbuild | 8 + src/config/FreeRTOSConfig.h | 4 +- src/config/config.h | 8 +- src/deck/Kbuild | 3 + src/deck/api/Kbuild | 5 + src/deck/api/deck_spi3.c | 3 +- src/deck/core/Kbuild | 5 + src/deck/core/deck.c | 2 +- src/deck/core/deck_drivers.c | 2 +- src/deck/core/deck_info.c | 25 +- src/deck/core/deck_memory.c | 39 +- src/deck/core/deck_test.c | 8 +- src/deck/drivers/interface/aideck-router.h | 37 + src/deck/drivers/interface/aideck.h | 65 +- src/deck/drivers/interface/lpsTdma.h | 12 +- src/deck/drivers/interface/lpsTdoa2Tag.h | 8 +- src/deck/drivers/src/Kbuild | 20 + src/deck/drivers/src/Kconfig | 320 ++ src/deck/drivers/src/aideck-router.c | 133 + src/deck/drivers/src/aideck.c | 172 +- src/deck/drivers/src/bigquad.c | 13 +- src/deck/drivers/src/ledring12.c | 242 +- src/deck/drivers/src/locodeck.c | 15 +- src/deck/drivers/src/lpsTdoa2Tag.c | 8 +- src/deck/drivers/src/lpsTdoa3Tag.c | 11 +- src/deck/drivers/src/lpsTwrTag.c | 2 +- src/deck/drivers/src/test/Kbuild | 3 + .../test/{exptestBolt.c => exptestBolt11.c} | 6 +- src/deck/drivers/src/usddeck.c | 4 +- src/drivers/Kbuild | 3 + src/drivers/bosch/src/Kbuild | 10 + src/drivers/esp32/src/Kbuild | 2 + src/drivers/interface/motors.h | 10 +- src/drivers/src/Kbuild | 27 + src/drivers/src/Kconfig | 0 src/drivers/src/i2c_drv.c | 6 +- src/drivers/src/motors.c | 3 +- .../src/{motors_def_cf2.c => motors_def.c} | 143 +- src/drivers/src/ws2812_cf2.c | 7 +- src/hal/Kbuild | 1 + src/hal/interface/pm.h | 47 +- src/hal/src/Kbuild | 24 + src/hal/src/Kconfig | 12 + src/hal/src/ledseq.c | 9 +- src/hal/src/ow_syslink.c | 35 - src/hal/src/pm_stm32f4.c | 42 +- src/hal/src/sensors.c | 16 +- src/hal/src/sensors_bmi088_bmp388.c | 2 +- src/hal/src/sensors_mpu9250_lps25h.c | 21 - src/hal/src/usb.c | 6 + src/init/Kbuild | 2 + src/init/main.c | 4 + ...rtup_stm32f40xx.s => startup_stm32f40xx.S} | 0 .../CM3/startup/arm/startup_stm32f10x_cl.s | 364 --- .../CM3/startup/arm/startup_stm32f10x_hd.s | 370 --- .../CM3/startup/arm/startup_stm32f10x_ld.s | 293 -- .../CM3/startup/arm/startup_stm32f10x_md.s | 303 -- .../CM3/startup/gcc/startup_stm32f10x_cl.s | 464 --- .../CM3/startup/gcc/startup_stm32f10x_hd.s | 483 --- .../CM3/startup/gcc/startup_stm32f10x_ld.s | 339 --- .../CM3/startup/gcc/startup_stm32f10x_md.s | 371 --- .../CM3/startup/iar/startup_stm32f10x_cl.s | 498 ---- .../CM3/startup/iar/startup_stm32f10x_hd.s | 510 ---- .../CM3/startup/iar/startup_stm32f10x_ld.s | 357 --- .../CM3/startup/iar/startup_stm32f10x_md.s | 382 --- src/lib/Kbuild | 46 + src/modules/Kbuild | 1 + src/modules/interface/bootloader.h | 49 + src/modules/interface/estimator.h | 7 +- src/modules/interface/param.h | 2 - src/modules/interface/pptraj.h | 2 +- src/modules/interface/queuemonitor.h | 7 +- src/modules/src/Kbuild | 64 + src/modules/src/Kconfig | 125 + src/modules/src/app_handler.c | 14 +- src/modules/src/attitude_pid_controller.c | 91 +- src/modules/src/bootloader.c | 84 + src/modules/src/controller.c | 14 +- src/modules/src/controller_mellinger.c | 38 +- src/modules/src/crtp_commander_generic.c | 10 +- src/modules/src/crtp_localization_service.c | 29 +- src/modules/src/cvmrs.c | 9 +- src/modules/src/estimator.c | 16 +- src/modules/src/estimator_kalman.c | 16 +- src/modules/src/health.c | 7 +- src/modules/src/kalman_core/Kbuild | 12 + src/modules/src/kalman_core/kalman_core.c | 2 +- src/modules/src/lighthouse/Kbuild | 5 + src/modules/src/lighthouse/lighthouse_core.c | 29 +- .../src/lighthouse/lighthouse_position_est.c | 3 +- src/modules/src/param_logic.c | 5 +- src/modules/src/position_controller_pid.c | 94 +- src/modules/src/position_estimator_altitude.c | 12 +- src/modules/src/power_distribution_stock.c | 9 +- src/modules/src/queuemonitor.c | 4 +- src/modules/src/sensfusion6.c | 18 +- src/modules/src/stabilizer.c | 5 +- src/modules/src/system.c | 24 +- src/platform/Kbuild | 1 + src/platform/{ => interface}/platform.h | 18 +- src/platform/interface/platform_defaults.h | 40 + .../interface/platform_defaults_bolt.h | 43 + .../interface/platform_defaults_cf2.h | 44 + .../interface/platform_defaults_tag.h | 43 + src/platform/src/Kbuild | 6 + src/platform/{ => src}/platform.c | 1 - src/platform/src/platform_bolt.c | 82 + src/platform/{ => src}/platform_cf2.c | 20 +- src/platform/{ => src}/platform_stm32f4.c | 0 src/platform/{ => src}/platform_tag.c | 0 src/platform/{ => src}/platform_utils.c | 0 src/utils/Kbuild | 1 + src/utils/interface/debug.h | 5 +- src/utils/src/Kbuild | 43 + src/utils/src/kve/kve_storage.c | 2 +- src/utils/src/lighthouse/pulse_processor_v2.c | 23 +- src/utils/src/malloc.c | 22 + test/deck/core/test_deck_memory.c | 406 +-- test/utils/src/kve/test_kve.c | 215 ++ .../src/lighthouse/test_pulse_processor_v2.c | 127 +- tools/build/check_elf | 2 +- tools/build/make_app | 23 + tools/gen-dox/Doxyfile | 4 +- tools/kbuild/Makefile.kbuild | 827 ++++++ tools/make/config.mk.example | 133 - tools/make/oot.mk | 29 + tools/make/platform.mk | 35 - tools/make/platforms/cf2.mk | 19 - tools/make/platforms/tag.mk | 19 - tools/make/targets.mk | 18 +- tools/make/usb-bootloader.py | 21 + tools/test/gcc.yml | 4 +- vendor/Kbuild | 285 ++ 283 files changed, 32588 insertions(+), 6554 deletions(-) create mode 100644 .github/workflows/release.yml delete mode 100644 CONTRIBUTING.md create mode 100644 Kconfig create mode 100644 app_api/Kbuild create mode 100644 app_api/Kconfig create mode 100644 app_api/app-config create mode 100644 app_api/src/Kbuild create mode 100644 configs/all.config create mode 100644 configs/app_api.conf create mode 100644 configs/bigquad.conf create mode 100644 configs/bolt_defconfig create mode 100644 configs/bosch.conf create mode 100644 configs/cf2_defconfig create mode 100644 configs/defconfig create mode 100644 configs/loco_tdma.conf create mode 100644 configs/loco_tdoa2.conf create mode 100644 configs/loco_tdoa3.conf create mode 100644 configs/loco_tdoa3_all.conf create mode 100644 configs/tag_defconfig create mode 100644 docs/development/create_platform.md create mode 100644 docs/development/kbuild.md create mode 100644 docs/functional-areas/crtp/crtp_link.md create mode 100644 docs/functional-areas/crtp/crtp_platform.md create mode 100644 docs/functional-areas/lighthouse/multi_base_stations.md create mode 100644 docs/images/kbuild1.png create mode 100644 docs/images/kbuild2.png create mode 100644 docs/userguides/platform.md create mode 100644 examples/app_appchannel_test/Kbuild create mode 100644 examples/app_appchannel_test/app-config create mode 100644 examples/app_appchannel_test/src/Kbuild create mode 100644 examples/app_hello_world-cpp/Kbuild create mode 100644 examples/app_hello_world-cpp/Makefile create mode 100644 examples/app_hello_world-cpp/README.md create mode 100644 examples/app_hello_world-cpp/app-config create mode 100644 examples/app_hello_world-cpp/src/Kbuild create mode 100644 examples/app_hello_world-cpp/src/hello_world.cpp create mode 100644 examples/app_hello_world/Kbuild create mode 100644 examples/app_hello_world/app-config create mode 100644 examples/app_hello_world/other/headerInOtherDir.h create mode 100644 examples/app_hello_world/src/Kbuild create mode 100644 examples/app_internal_param_log/Kbuild create mode 100644 examples/app_internal_param_log/app-config create mode 100644 examples/app_internal_param_log/src/Kbuild create mode 100644 examples/app_peer_to_peer/Kbuild create mode 100644 examples/app_peer_to_peer/app-config create mode 100644 examples/app_peer_to_peer/src/Kbuild create mode 100644 examples/demos/app_push_demo/Kbuild create mode 100644 examples/demos/app_push_demo/app-config create mode 100644 examples/demos/app_push_demo/src/Kbuild create mode 100644 examples/demos/app_wall_following_demo/Kbuild create mode 100644 examples/demos/app_wall_following_demo/app-config create mode 100644 examples/demos/app_wall_following_demo/src/Kbuild create mode 100644 examples/demos/swarm_demo/Kbuild create mode 100644 examples/demos/swarm_demo/app-config create mode 100644 include/config/cc/optimize/for/performance.h create mode 100644 include/config/cross/compile.h create mode 100644 include/config/enable/foo/bar.h create mode 100644 include/config/estimator/name.h create mode 100644 include/config/log/enable.h create mode 100644 include/config/log/error.h create mode 100644 include/config/log/timestamp.h create mode 100644 scripts/Kbuild.include create mode 100644 scripts/Makefile create mode 100644 scripts/Makefile.build create mode 100644 scripts/Makefile.clean create mode 100644 scripts/Makefile.extrawarn create mode 100644 scripts/Makefile.host create mode 100644 scripts/Makefile.lib create mode 100644 scripts/basic/.gitignore create mode 100644 scripts/basic/Makefile create mode 100644 scripts/basic/bin2c.c create mode 100644 scripts/basic/fixdep.c create mode 100755 scripts/checkincludes.pl create mode 100755 scripts/gcc-goto.sh create mode 100755 scripts/headerdep.pl create mode 100644 scripts/kconfig/.gitignore create mode 100644 scripts/kconfig/Makefile create mode 100644 scripts/kconfig/POTFILES.in create mode 100755 scripts/kconfig/check.sh create mode 100644 scripts/kconfig/conf.c create mode 100644 scripts/kconfig/confdata.c create mode 100644 scripts/kconfig/expr.c create mode 100644 scripts/kconfig/expr.h create mode 100644 scripts/kconfig/gconf.c create mode 100644 scripts/kconfig/gconf.glade create mode 100644 scripts/kconfig/images.c create mode 100644 scripts/kconfig/kxgettext.c create mode 100644 scripts/kconfig/list.h create mode 100644 scripts/kconfig/lkc.h create mode 100644 scripts/kconfig/lkc_proto.h create mode 100644 scripts/kconfig/lxdialog/.gitignore create mode 100644 scripts/kconfig/lxdialog/BIG.FAT.WARNING create mode 100755 scripts/kconfig/lxdialog/check-lxdialog.sh create mode 100644 scripts/kconfig/lxdialog/checklist.c create mode 100644 scripts/kconfig/lxdialog/dialog.h create mode 100644 scripts/kconfig/lxdialog/inputbox.c create mode 100644 scripts/kconfig/lxdialog/menubox.c create mode 100644 scripts/kconfig/lxdialog/textbox.c create mode 100644 scripts/kconfig/lxdialog/util.c create mode 100644 scripts/kconfig/lxdialog/yesno.c create mode 100644 scripts/kconfig/mconf.c create mode 100644 scripts/kconfig/menu.c create mode 100755 scripts/kconfig/merge_config.sh create mode 100644 scripts/kconfig/nconf.c create mode 100644 scripts/kconfig/nconf.gui.c create mode 100644 scripts/kconfig/nconf.h create mode 100644 scripts/kconfig/qconf.cc create mode 100644 scripts/kconfig/qconf.h create mode 100755 scripts/kconfig/streamline_config.pl create mode 100644 scripts/kconfig/symbol.c create mode 100644 scripts/kconfig/util.c create mode 100644 scripts/kconfig/zconf.gperf create mode 100644 scripts/kconfig/zconf.hash.c_shipped create mode 100644 scripts/kconfig/zconf.l create mode 100644 scripts/kconfig/zconf.lex.c_shipped create mode 100644 scripts/kconfig/zconf.tab.c_shipped create mode 100644 scripts/kconfig/zconf.y create mode 100755 scripts/mkmakefile create mode 100644 src/Kbuild create mode 100644 src/deck/Kbuild create mode 100644 src/deck/api/Kbuild create mode 100644 src/deck/core/Kbuild create mode 100644 src/deck/drivers/interface/aideck-router.h create mode 100644 src/deck/drivers/src/Kbuild create mode 100644 src/deck/drivers/src/Kconfig create mode 100644 src/deck/drivers/src/aideck-router.c create mode 100644 src/deck/drivers/src/test/Kbuild rename src/deck/drivers/src/test/{exptestBolt.c => exptestBolt11.c} (98%) create mode 100644 src/drivers/Kbuild create mode 100644 src/drivers/bosch/src/Kbuild create mode 100644 src/drivers/esp32/src/Kbuild create mode 100644 src/drivers/src/Kbuild create mode 100644 src/drivers/src/Kconfig rename src/drivers/src/{motors_def_cf2.c => motors_def.c} (81%) create mode 100644 src/hal/Kbuild create mode 100644 src/hal/src/Kbuild create mode 100644 src/hal/src/Kconfig create mode 100644 src/init/Kbuild rename src/init/{startup_stm32f40xx.s => startup_stm32f40xx.S} (100%) delete mode 100644 src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_cl.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_hd.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_ld.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_md.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_cl.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_hd.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_ld.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_md.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_cl.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_hd.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_ld.s delete mode 100644 src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_md.s create mode 100644 src/lib/Kbuild create mode 100644 src/modules/Kbuild create mode 100644 src/modules/interface/bootloader.h create mode 100644 src/modules/src/Kbuild create mode 100644 src/modules/src/Kconfig create mode 100644 src/modules/src/bootloader.c create mode 100644 src/modules/src/kalman_core/Kbuild create mode 100644 src/modules/src/lighthouse/Kbuild create mode 100644 src/platform/Kbuild rename src/platform/{ => interface}/platform.h (86%) create mode 100644 src/platform/interface/platform_defaults.h create mode 100644 src/platform/interface/platform_defaults_bolt.h create mode 100644 src/platform/interface/platform_defaults_cf2.h create mode 100644 src/platform/interface/platform_defaults_tag.h create mode 100644 src/platform/src/Kbuild rename src/platform/{ => src}/platform.c (99%) create mode 100644 src/platform/src/platform_bolt.c rename src/platform/{ => src}/platform_cf2.c (78%) rename src/platform/{ => src}/platform_stm32f4.c (100%) rename src/platform/{ => src}/platform_tag.c (100%) rename src/platform/{ => src}/platform_utils.c (100%) create mode 100644 src/utils/Kbuild create mode 100644 src/utils/src/Kbuild create mode 100644 src/utils/src/malloc.c create mode 100644 test/utils/src/kve/test_kve.c create mode 100755 tools/build/make_app create mode 100644 tools/kbuild/Makefile.kbuild delete mode 100644 tools/make/config.mk.example create mode 100644 tools/make/oot.mk delete mode 100644 tools/make/platform.mk delete mode 100644 tools/make/platforms/cf2.mk delete mode 100644 tools/make/platforms/tag.mk create mode 100644 tools/make/usb-bootloader.py create mode 100644 vendor/Kbuild diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 1f9c3271ee..ff78851ded 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -21,15 +21,15 @@ jobs: submodules: true - name: build - run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=cf2 UNIT_TEST_STYLE=min + run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "make defconfig && ./tools/build/build PLATFORM=cf2 UNIT_TEST_STYLE=min" - name: Upload Build Artifact uses: actions/upload-artifact@v2.1.4 with: name: cf2-${{ github.sha }} path: cf2.bin - - tag: + + bolt: runs-on: ubuntu-latest steps: @@ -39,17 +39,16 @@ jobs: submodules: true - name: build - run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=tag UNIT_TEST_STYLE=min + run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "make bolt_defconfig && ./tools/build/build UNIT_TEST_STYLE=min" - name: Upload Build Artifact uses: actions/upload-artifact@v2.1.4 with: - name: tag-${{ github.sha }} - path: tag.bin - - all-flags: + name: bolt-${{ github.sha }} + path: bolt.bin + + tag: runs-on: ubuntu-latest - needs: cf2 steps: - name: Checkout Repo @@ -57,8 +56,14 @@ jobs: with: submodules: true - - name: All features - run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=cf2 DEBUG=1 "EXTRA_CFLAGS=-DCALIBRATED_LED_MORSE -DIMU_TAKE_ACCEL_BIAS -DIMU_MPU6500_DLPF_256HZ -DMADWICK_QUATERNION_IMU -DDEBUG_QUEUE_MONITOR -DENABLE_BQ_DECK -DSITAW_ENABLED -DOW_WRITE_TEST -DOW_READ_TEST -DDEBUG_PRINT_ON_UART -DENABLE_UART1 -DENABLE_UART2" UNIT_TEST_STYLE=min + - name: build + run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "make tag_defconfig && ./tools/build/build PLATFORM=tag UNIT_TEST_STYLE=min" + + - name: Upload Build Artifact + uses: actions/upload-artifact@v2.1.4 + with: + name: tag-${{ github.sha }} + path: tag.bin features: runs-on: ubuntu-latest @@ -69,21 +74,24 @@ jobs: matrix: features: # Build cf2 with bosch sensors - - SENSORS=bosch + - bosch.conf # Build cf2 with TDMA - - LPS_TDMA_ENABLE=1 "EXTRA_CFLAGS=-DTDMA_NSLOTS_BITS=1 -DTDMA_SLOT=0" + - loco_tdma.conf # Build cf2 with TDOA2 positioning mode - - LPS_TDOA_ENABLE=1 + - loco_tdoa2.conf + #- LPS_TDOA_ENABLE=1 # Build cf2 with TDOA3 positioning mode - - LPS_TDOA3_ENABLE=1 + - loco_tdoa3.conf + #- LPS_TDOA3_ENABLE=1 # Build cf2 with TDOA3 and all config options - - LPS_TDOA3_ENABLE=1 "EXTRA_CFLAGS=-DLPS_2D_POSITION_HEIGHT=1.2 -DLPS_LONGER_RANGE -DLPS_FULL_TX_POWER" + - loco_tdoa3_all.conf # Build Bigquad deck with all config options - - > - "EXTRA_CFLAGS=-DENABLE_BQ_DECK -DBQ_DECK_ENABLE_OSD -DBQ_DECK_ENABLE_PM" + - bigquad.conf + # Build API test app layer app + - app_api.conf env: - FEATURE: ${{ matrix.features }} - + CONF: ${{ matrix.features }} + steps: - name: Checkout Repo uses: actions/checkout@v2 @@ -91,7 +99,7 @@ jobs: submodules: true - name: build - run: docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=cf2 ${{ env.FEATURE }} UNIT_TEST_STYLE=min + run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "./scripts/kconfig/merge_config.sh configs/${CONF} configs/defconfig && ./tools/build/build UNIT_TEST_STYLE=min" apps: runs-on: ubuntu-latest @@ -101,14 +109,38 @@ jobs: fail-fast: false matrix: example: - - app_api - examples/app_hello_world + - examples/app_hello_world-cpp - examples/app_peer_to_peer - examples/demos/app_push_demo - examples/demos/swarm_demo env: EXAMPLE: ${{ matrix.example }} - + + steps: + - name: Checkout Repo + uses: actions/checkout@v2 + with: + submodules: true + + - name: build + run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "./tools/build/make_app ${EXAMPLE}" + + kbuild-targets: + runs-on: ubuntu-latest + needs: cf2 + + strategy: + fail-fast: false + matrix: + target: + - allyesconfig + - allnoconfig + - randconfig + + env: + TARGET: ${{ matrix.target }} + steps: - name: Checkout Repo uses: actions/checkout@v2 @@ -116,4 +148,4 @@ jobs: submodules: true - name: build - run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "cd ${EXAMPLE}; make -j$(nproc)" + run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "KCONFIG_ALLCONFIG=configs/all.config make ${TARGET} && ./tools/build/build UNIT_TEST_STYLE=min" diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 0000000000..2031b52e77 --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,74 @@ +# Release jobs + +name: Release + +on: + workflow_dispatch: + +jobs: + release: + name: Create Release on Github + runs-on: ubuntu-latest + steps: + - name: Create Release + id: create_release + uses: actions/create-release@v1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + tag_name: ${{ github.ref }} + release_name: ${{ github.ref }} + draft: true + prerelease: true + - name: Release URL on file + run: echo "${{ steps.create_release.outputs.upload_url }}" > release_url.txt + - name: Save Release URL File For Uploading Files + uses: actions/upload-artifact@v1 + with: + name: release_url + path: release_url.txt + + upload: + name: Upload Binaries to release + needs: [release] + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + platforms: [cf2, tag, bolt] + + steps: + - name: Checkout Repo + uses: actions/checkout@v2 + with: + submodules: true + + - name: Build + run: docker run --rm -v ${PWD}:/module bitcraze/builder bash -c "make ${{ matrix.platforms }}_defconfig && ./tools/build/build PLATFORM=${{ matrix.platforms }} UNIT_TEST_STYLE=min" + + - name: Load Release URL File from release job + uses: actions/download-artifact@v1 + with: + name: release_url + + - name: Get Release File Name & Upload URL + id: get_release_info + run: | + value=`cat release_url/release_url.txt` + echo ::set-output name=upload_url::$value + + - name: Get the version + id: get_release_version + env: + GITHUB_REF : ${{ github.ref }} + run: echo ::set-output name=release_version::${GITHUB_REF/refs\/tags\//} + + - name: Upload ${{ matrix.platforms }} bin + uses: actions/upload-release-asset@v1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + upload_url: ${{ steps.get_release_info.outputs.upload_url }} + asset_path: ${{ matrix.platforms }}.bin + asset_name: ${{ matrix.platforms }}-${{ steps.get_release_version.outputs.release_version }}.bin + asset_content_type: application/octet-stream diff --git a/.gitignore b/.gitignore index 1632c47336..7d52179c0f 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,7 @@ cffirmware.py /cf2.* /tag.* +/bolt.* *.gch current_platform.mk @@ -34,3 +35,14 @@ docs/.jekyll-cache docs/api/logs.md_raw docs/api/params.md_raw docs/api/log_param_doc.json +docs/api + +build/ +# To avoid that /tools/build is ignored +!/tools/* + +**/*.o +**/*.cmd +**/*.a +*.elf +src/utils/src/version_gen.c diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md deleted file mode 100644 index 1c54c91061..0000000000 --- a/CONTRIBUTING.md +++ /dev/null @@ -1,44 +0,0 @@ -Contributing -============ - -👍🎉 Thanks a lot for considering contributing 🎉👍 - -We welcome and encourage contribution. There is many way to contribute: you can -write bug report, contribute code or documentation. -You can also go to the [bitcraze forum](https://forum.bitcraze.io) and help others. - -## Reporting issues - -When reporting issues the more information you can supply the better. - - - **Information about the environment:** - - What version of the firmware are you running - - If relevant, what are you using to control the Crazyflie. ie. What lib or client, and what version. - - **How to reproduce the issue:** Step-by-step guide on how the issue can be reproduced (or at least how you reproduce it). - Include everything you think might be useful, the more information the better. - -## Improvements request and proposal - -We and the community are continuously working to improve the firmware. -Feel free to make an issue to request a new functionality. - -## Contributing code/Pull-Request - -We welcome code contribution, this can be done by starting a pull-request. - -If the change is big, typically if the change span to more than one file, consider starting an issue first to discuss the improvement. -This will makes it much easier to make the change fit well into the firmware. - -There is some basic requirement for us to merge a pull request: - - Describe the change - - Refer to any issues it effects - - Separate one pull request per functionality: if you start writing "and" in the feature description consider if it could be separated in two pull requests. - - The pull request must pass the automated test (see test section bellow) - -In your code: - - 2 spaces indentation - - Make sure the coding style of your code follows the style of the file. - -### Run test - -To run the tests please have a look at the [unit test documentation](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/development/unit_testing/). diff --git a/Kconfig b/Kconfig new file mode 100644 index 0000000000..a3d9b12cc7 --- /dev/null +++ b/Kconfig @@ -0,0 +1,184 @@ +mainmenu "Crazyflie Platform Configuration" + +menu "Build and debug options" + +config CROSS_COMPILE + string "Cross-compiler tool prefix" + default "arm-none-eabi-" + help + Same as running 'make CROSS_COMPILE=prefix-' but stored for + default make runs in this application build directory. You don't + need to set this unless you want the configured application build + directory to select the cross-compiler automatically. + +config DEBUG + bool "Enable debug build" + default n + help + Enable debug symbols and extra output from firmware. + +config DEBUG_QUEUE_MONITOR + bool "Enable debug queue monitoring" + depends on DEBUG + default n + help + Enable the queue monitoring functionality. + +config DEBUG_ENABLE_LED_MORSE + bool "Enable blinking morse sequence with LEDs" + default n + help + If enabled makes it possible to define LED sequences in form of the DOT, + DASH, GAP, LETTER_GAP and WORD_GAP parts of Morse code. + +config DEBUG_PRINT_ON_UART1 + bool "Switch DEBUG_PRINT to UART1" + default n + help + Switch output of DEBUG_PRINT from radio to UART1 with 115200 baudrate. + Note! This will interfere with several expansion decks, see the expansion + deck documentation on https://www.bitcraze.io/ for more information about + pin allocation. + +config DEBUG_DECK_IGNORE_OW + bool "Do not enumerate OW based expansion decks" + default n + help + Do not enumerate decks discovered by OneWire memory. This might be useful + if one deck memory is very wrong. One could use the Crazyflie as a + passive onewire memory writer. + +endmenu + +menu "Platform configuration" + +choice + prompt "Platform to build" + default CONFIG_PLATFORM_CF2 + +config PLATFORM_CF2 + bool "Build for CF2" + select SENSORS_BMI088_BMP388 + select SENSORS_MPU9250_LPS25H + +config PLATFORM_BOLT + bool "Build for Bolt" + select SENSORS_BMI088_BMP388 + select SENSORS_BMI088_SPI + +config PLATFORM_TAG + bool "Build for the roadrunner" + select SENSORS_BMI088_BMP388 + +endchoice + +endmenu + +menu "IMU configuration" + +choice + prompt "IMU algorithm" + default IMU_MAHONY_QUATERNION + +config IMU_MADGWICK_QUATERNION + bool "Madgwick's AHRS and IMU algorithms" + help + Use Madgwick's IMU and AHRS algorithms. + See: http://www.x-io.co.uk/open-source-ahrs-with-x-imu + +config IMU_MAHONY_QUATERNION + bool "Mahony IMU algorithm" + help + Use Mahony's algorithm from the paper: + Nonlinear Complementary Filters on the Special Orthogonal Group + See: https://ieeexplore.ieee.org/document/4608934 + +endchoice + +endmenu + +menu "Sensor configuration" + +config SENSORS_MPU9250_LPS25H + bool "Support for mpu9250 and lps25h sensors" + default n + help + Include support for the Bosch bmi088 inertial and bmp388 + barometric sensors + +config SENSORS_BMI088_BMP388 + bool "Support for bmi088 and bmp388 sensors" + default n + select SENSORS_BMI088_I2C + help + Include support for the Bosch bmi088 inertial and bmp388 + barometric sensors + +config SENSORS_BOSCH + bool "Include Bosch sensors" + default n + +config SENSORS_IGNORE_BAROMETER_FAIL + bool "Ignore failure from barometer" + depends on SENSORS_BMI088_BMP388 + default n + +config SENSORS_BMI088_SPI + bool "Support for SPI communincation with the bmi088 sensor" + depends on SENSORS_BMI088_BMP388 + default n + help + Include support using SPI with the Bosch bmi088 inertial sensor + +config SENSORS_BMI088_I2C + bool "Support for I2C communincation with the bmi088 sensor" + depends on SENSORS_BMI088_BMP388 + default n + help + Include support using I2C with the Bosch bmi088 inertial sensor + +endmenu + +source src/hal/src/Kconfig + +menu "App layer configuration" + +source "app_api/Kconfig" + +config APP_ENABLE + bool "Enable app entry point" + default n + help + When enabled the firmware will call a function void appMain() from a + task after the startup sequence has completed. This function should not + return. + +config APP_STACKSIZE + int "Set the task stack size" + depends on APP_ENABLE + default 300 + help + Size is in 32bit word (4 Bytes). The default is 300 (1.2KBytes) + +config APP_PRIORITY + int "Set the task priority" + depends on APP_ENABLE + default 0 + help + Set the task priority between 0 and 5. Default is 0 (same as IDLE). + +endmenu + +menu "Expansion deck configuration" + +config DECK_FORCE + string "Force load specified custom deck driver" + default "none" + help + A colon seperated list of custom drivers to force load or "none". + +source src/deck/drivers/src/Kconfig + +endmenu + +source "src/modules/src/Kconfig" diff --git a/Makefile b/Makefile index d8374fbc09..590b11f774 100644 --- a/Makefile +++ b/Makefile @@ -1,449 +1,137 @@ -# CrazyFlie's Makefile -# Copyright (c) 2011-2021 Bitcraze AB -# This Makefile compiles all the object file to ./bin/ and the resulting firmware -# image in ./cfX.elf and ./cfX.bin -CRAZYFLIE_BASE ?= ./ - -# Put your personal build config in tools/make/config.mk and DO NOT COMMIT IT! -# Make a copy of tools/make/config.mk.example to get you started --include tools/make/config.mk - -CFLAGS += $(EXTRA_CFLAGS) - -######### JTAG and environment configuration ########## OPENOCD ?= openocd -OPENOCD_INTERFACE ?= interface/stlink.cfg +OPENOCD_INTERFACE ?= interface/stlink-v2.cfg +OPENOCD_TARGET ?= target/stm32f4x.cfg OPENOCD_CMDS ?= -CROSS_COMPILE ?= arm-none-eabi- + +CPU = stm32f4 +LOAD_ADDRESS_stm32f4 = 0x8000000 +LOAD_ADDRESS_CLOAD_stm32f4 = 0x8004000 + PYTHON ?= python3 -DFU_UTIL ?= dfu-util -CLOAD ?= 1 -DEBUG ?= 0 -CLOAD_CMDS ?= -CLOAD_ARGS ?= -PLATFORM ?= cf2 -LPS_TDMA_ENABLE ?= 0 -LPS_TDOA_ENABLE ?= 0 -LPS_TDOA3_ENABLE ?= 0 -APP ?= 1 # Cload is handled in a special way on windows in WSL to use the Windows python interpreter ifdef WSL_DISTRO_NAME CLOAD_SCRIPT ?= python.exe -m cfloader else -CLOAD_SCRIPT ?= python3 -m cfloader +CLOAD_SCRIPT ?= $(PYTHON) -m cfloader endif -# Platform configuration handling --include current_platform.mk -include $(CRAZYFLIE_BASE)/tools/make/platform.mk - -CFLAGS += -DCRAZYFLIE_FW - -#OpenOCD conf -RTOS_DEBUG ?= 0 - -LIB = $(CRAZYFLIE_BASE)/src/lib -FREERTOS = $(CRAZYFLIE_BASE)/vendor/FreeRTOS -CFLAGS += -DBLOBS_LOC='"$(CRAZYFLIE_BASE)/blobs/"' +DFU_UTIL ?= dfu-util -# Communication Link -UART2_LINK ?= 0 +CLOAD_CMDS ?= +CLOAD_ARGS ?= +ARCH := stm32f4 +SRCARCH := stm32f4 -############### CPU-specific build configuration ################ +ARCH_CFLAGS += -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16 -g3 +ARCH_CFLAGS += -fno-math-errno -DARM_MATH_CM4 -D__FPU_PRESENT=1 -mfp16-format=ieee +ARCH_CFLAGS += -Wno-array-bounds -Wno-stringop-overread +ARCH_CFLAGS += -Wno-stringop-overflow +ARCH_CFLAGS += -DSTM32F4XX -DSTM32F40_41xxx -DHSE_VALUE=8000000 -DUSE_STDPERIPH_DRIVER -ifeq ($(CPU), stm32f4) +FREERTOS = $(srctree)/vendor/FreeRTOS PORT = $(FREERTOS)/portable/GCC/ARM_CM4F -LINKER_DIR = $(CRAZYFLIE_BASE)/tools/make/F405/linker -ST_OBJ_DIR = $(CRAZYFLIE_BASE)/tools/make/F405 - -OPENOCD_TARGET ?= target/stm32f4x.cfg - - -# St Lib -VPATH += $(LIB)/CMSIS/STM32F4xx/Source/ -VPATH += $(LIB)/STM32_USB_Device_Library/Core/src -VPATH += $(LIB)/STM32_USB_OTG_Driver/src -VPATH += $(CRAZYFLIE_BASE)/src/deck/api $(CRAZYFLIE_BASE)/src/deck/core $(CRAZYFLIE_BASE)/src/deck/drivers/src $(CRAZYFLIE_BASE)/src/deck/drivers/src/test -VPATH += $(CRAZYFLIE_BASE)/src/utils/src/tdoa $(CRAZYFLIE_BASE)/src/utils/src/lighthouse -CRT0 = startup_stm32f40xx.o system_stm32f4xx.o - -# Add ST lib object files --include $(ST_OBJ_DIR)/st_obj.mk - -# USB obj -ST_OBJ += usb_core.o usb_dcd_int.o usb_dcd.o -# USB Device obj -ST_OBJ += usbd_ioreq.o usbd_req.o usbd_core.o - +LIB = $(srctree)/src/lib PROCESSOR = -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16 -CFLAGS += -fno-math-errno -DARM_MATH_CM4 -D__FPU_PRESENT=1 -mfp16-format=ieee - -#Flags required by the ST library -CFLAGS += -DSTM32F4XX -DSTM32F40_41xxx -DHSE_VALUE=8000000 -DUSE_STDPERIPH_DRIVER - -LOAD_ADDRESS_stm32f4 = 0x8000000 -LOAD_ADDRESS_CLOAD_stm32f4 = 0x8004000 -MEM_SIZE_FLASH_K = 1008 -MEM_SIZE_RAM_K = 128 -MEM_SIZE_CCM_K = 64 -endif - -################ Build configuration ################## - -# libdw dw1000 driver -VPATH += $(CRAZYFLIE_BASE)/vendor/libdw1000/src - -# vl53l1 driver -VPATH += $(LIB)/vl53l1/core/src - -# FreeRTOS -VPATH += $(PORT) -PORT_OBJ = port.o -VPATH += $(FREERTOS)/portable/MemMang -MEMMANG_OBJ ?= heap_4.o - -VPATH += $(FREERTOS) -FREERTOS_OBJ = list.o tasks.o queue.o timers.o event_groups.o $(MEMMANG_OBJ) - -#FatFS -VPATH += $(LIB)/FatFS -PROJ_OBJ += ff.o ffunicode.o fatfs_sd.o -ifeq ($(FATFS_DISKIO_TESTS), 1) -PROJ_OBJ += diskio_function_tests.o -CFLAGS += -DUSD_RUN_DISKIO_FUNCTION_TESTS -endif - -ifeq ($(APP), 1) -CFLAGS += -DAPP_ENABLED=1 -endif -ifdef APP_STACKSIZE -CFLAGS += -DAPP_STACKSIZE=$(APP_STACKSIZE) -endif -ifdef APP_PRIORITY -CFLAGS += -DAPP_PRIORITY=$(APP_PRIORITY) -endif - -# Crazyflie sources -VPATH += $(CRAZYFLIE_BASE)/src/init $(CRAZYFLIE_BASE)/src/hal/src $(CRAZYFLIE_BASE)/src/modules/src $(CRAZYFLIE_BASE)/src/modules/src/lighthouse $(CRAZYFLIE_BASE)/src/modules/src/kalman_core $(CRAZYFLIE_BASE)/src/utils/src $(CRAZYFLIE_BASE)/src/drivers/bosch/src $(CRAZYFLIE_BASE)/src/drivers/src $(CRAZYFLIE_BASE)/src/platform $(CRAZYFLIE_BASE)/src/drivers/esp32/src -VPATH += $(CRAZYFLIE_BASE)/src/utils/src/kve - -############### Source files configuration ################ - -# Init -PROJ_OBJ += main.o -PROJ_OBJ += platform.o platform_utils.o platform_$(PLATFORM).o platform_$(CPU).o - -# Drivers -PROJ_OBJ += exti.o nvic.o motors.o -PROJ_OBJ += led.o mpu6500.o i2cdev.o ws2812_cf2.o lps25h.o i2c_drv.o -PROJ_OBJ += ak8963.o eeprom.o maxsonar.o piezo.o -PROJ_OBJ += uart_syslink.o swd.o uart1.o uart2.o watchdog.o -PROJ_OBJ += cppm.o -PROJ_OBJ += bmi055_accel.o bmi055_gyro.o bmi160.o bmp280.o bstdr_comm_support.o bmm150.o -PROJ_OBJ += bmi088_accel.o bmi088_gyro.o bmi088_fifo.o bmp3.o -PROJ_OBJ += pca9685.o vl53l0x.o pca95x4.o pca9555.o vl53l1x.o pmw3901.o -PROJ_OBJ += amg8833.o lh_bootloader.o -PROJ_OBJ += esp_rom_bootloader.o esp_slip.o - -# USB Files -PROJ_OBJ += usb_bsp.o usblink.o usbd_desc.o usb.o - -# Hal -PROJ_OBJ += crtp.o ledseq.o freeRTOSdebug.o buzzer.o -PROJ_OBJ += pm_$(CPU).o syslink.o radiolink.o ow_syslink.o ow_common.o proximity.o usec_time.o -PROJ_OBJ += sensors.o -PROJ_OBJ += storage.o - -# libdw -PROJ_OBJ += libdw1000.o libdw1000Spi.o - -# vl53l1 lib -PROJ_OBJ += vl53l1_api_core.o vl53l1_api.o vl53l1_core.o vl53l1_silicon_core.o vl53l1_api_strings.o -PROJ_OBJ += vl53l1_api_calibration.o vl53l1_api_debug.o vl53l1_api_preset_modes.o vl53l1_error_strings.o -PROJ_OBJ += vl53l1_register_funcs.o vl53l1_wait.o vl53l1_core_support.o - -# Modules -PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param_task.o param_logic.o -PROJ_OBJ += log.o worker.o queuemonitor.o msp.o -PROJ_OBJ += platformservice.o sound_cf2.o extrx.o sysload.o mem.o -PROJ_OBJ += range.o app_handler.o static_mem.o app_channel.o -PROJ_OBJ += eventtrigger.o supervisor.o - -# Stabilizer modules -PROJ_OBJ += commander.o crtp_commander.o crtp_commander_rpyt.o -PROJ_OBJ += crtp_commander_generic.o crtp_localization_service.o peer_localization.o -PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o -PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o position_controller_indi.o -PROJ_OBJ += estimator.o estimator_complementary.o -PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o controller_indi.o -PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o -PROJ_OBJ += collision_avoidance.o health.o - -# Kalman estimator -PROJ_OBJ += estimator_kalman.o kalman_core.o kalman_supervisor.o -PROJ_OBJ += mm_distance.o mm_absolute_height.o mm_position.o mm_pose.o mm_tdoa.o mm_flow.o mm_tof.o mm_yaw_error.o mm_sweep_angles.o -PROJ_OBJ += mm_tdoa_robust.o mm_distance_robust.o - -# High-Level Commander -PROJ_OBJ += crtp_commander_high_level.o planner.o pptraj.o pptraj_compressed.o - -# Deck Core -PROJ_OBJ += deck.o deck_info.o deck_drivers.o deck_test.o deck_memory.o - -# Deck API -PROJ_OBJ += deck_constants.o -PROJ_OBJ += deck_digital.o -PROJ_OBJ += deck_analog.o -PROJ_OBJ += deck_spi.o -PROJ_OBJ += deck_spi3.o - -# Decks -PROJ_OBJ += bigquad.o -PROJ_OBJ += ledring12.o -PROJ_OBJ += buzzdeck.o -PROJ_OBJ += gtgps.o -PROJ_OBJ += cppmdeck.o -PROJ_OBJ += usddeck.o -PROJ_OBJ += zranger.o zranger2.o -PROJ_OBJ += locodeck.o -PROJ_OBJ += clockCorrectionEngine.o -PROJ_OBJ += lpsTwrTag.o -PROJ_OBJ += lpsTdoa2Tag.o -PROJ_OBJ += lpsTdoa3Tag.o tdoaEngineInstance.o tdoaEngine.o tdoaStats.o tdoaStorage.o -PROJ_OBJ += outlierFilter.o -PROJ_OBJ += flowdeck_v1v2.o -PROJ_OBJ += oa.o -PROJ_OBJ += multiranger.o -PROJ_OBJ += lighthouse.o -PROJ_OBJ += activeMarkerDeck.o - -# Uart2 Link for CRTP communication is not compatible with decks using uart2 -ifeq ($(UART2_LINK), 1) -CFLAGS += -DUART2_LINK_COMM -else -PROJ_OBJ += aideck.o -endif - -ifeq ($(LPS_TDOA_ENABLE), 1) -CFLAGS += -DLPS_TDOA_ENABLE -endif - -ifeq ($(LPS_TDOA3_ENABLE), 1) -CFLAGS += -DLPS_TDOA3_ENABLE -endif - -ifeq ($(LPS_TDMA_ENABLE), 1) -CFLAGS += -DLPS_TDMA_ENABLE -endif - -ifdef SENSORS -SENSORS_UPPER = $(shell echo $(SENSORS) | tr a-z A-Z) -CFLAGS += -DSENSORS_FORCE=SensorImplementation_$(SENSORS) - -# Add sensor file to the build if needed -ifeq (,$(findstring DSENSOR_INCLUDED_$(SENSORS_UPPER),$(CFLAGS))) -CFLAGS += -DSENSOR_INCLUDED_$(SENSORS_UPPER) -PROJ_OBJ += sensors_$(SENSORS).o -endif -endif - -#Deck tests -PROJ_OBJ += exptest.o -PROJ_OBJ += exptestRR.o -PROJ_OBJ += exptestBolt.o -#PROJ_OBJ += bigquadtest.o -#PROJ_OBJ += uarttest.o -#PROJ_OBJ += aidecktest.o - - -# Utilities -PROJ_OBJ += filter.o cpuid.o cfassert.o eprintf.o crc32.o num.o debug.o -PROJ_OBJ += version.o FreeRTOS-openocd.o -PROJ_OBJ += configblockeeprom.o -PROJ_OBJ += sleepus.o statsCnt.o rateSupervisor.o -PROJ_OBJ += lighthouse_core.o pulse_processor.o pulse_processor_v1.o pulse_processor_v2.o lighthouse_geometry.o ootx_decoder.o lighthouse_calibration.o lighthouse_deck_flasher.o lighthouse_position_est.o lighthouse_storage.o lighthouse_transmit.o -PROJ_OBJ += kve_storage.o kve.o -PROJ_OBJ += esp_deck_flasher.o - -ifeq ($(DEBUG_PRINT_ON_SEGGER_RTT), 1) -VPATH += $(LIB)/Segger_RTT/RTT -INCLUDES += -I$(LIB)/Segger_RTT/RTT -PROJ_OBJ += SEGGER_RTT.o SEGGER_RTT_printf.o -CFLAGS += -DDEBUG_PRINT_ON_SEGGER_RTT -endif - -# App Layer -PROJ_OBJ += cvmrs.o - -# Libs -PROJ_OBJ += libarm_math.a - -OBJ = $(FREERTOS_OBJ) $(PORT_OBJ) $(ST_OBJ) $(PROJ_OBJ) $(APP_OBJ) $(CRT0) +LINKER_DIR = $(srctree)/tools/make/F405/linker -############### Compilation configuration ################ -AS = $(CROSS_COMPILE)as -CC = $(CROSS_COMPILE)gcc -LD = $(CROSS_COMPILE)gcc -SIZE = $(CROSS_COMPILE)size -OBJCOPY = $(CROSS_COMPILE)objcopy -GDB = $(CROSS_COMPILE)gdb +LDFLAGS += --specs=nosys.specs --specs=nano.specs $(PROCESSOR) -nostdlib +image_LDFLAGS += -Wl,-Map=$(PROG).map,--cref,--gc-sections,--undefined=uxTopUsedPriority +image_LDFLAGS += -L$(srctree)/tools/make/F405/linker +image_LDFLAGS += -T $(LINKER_DIR)/FLASH_CLOAD.ld -INCLUDES += -I$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/Core/Include -I$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/DSP/Include -INCLUDES += -I$(CRAZYFLIE_BASE)/vendor/libdw1000/inc +INCLUDES += -I$(srctree)/vendor/CMSIS/CMSIS/Core/Include -I$(srctree)/vendor/CMSIS/CMSIS/DSP/Include +INCLUDES += -I$(srctree)/vendor/libdw1000/inc INCLUDES += -I$(FREERTOS)/include -I$(PORT) - -INCLUDES += -I$(CRAZYFLIE_BASE)/src/config -INCLUDES += -I$(CRAZYFLIE_BASE)/src/platform - -INCLUDES += -I$(CRAZYFLIE_BASE)/src/deck/interface -I$(CRAZYFLIE_BASE)/src/deck/drivers/interface -INCLUDES += -I$(CRAZYFLIE_BASE)/src/drivers/interface -I$(CRAZYFLIE_BASE)/src/drivers/bosch/interface -I$(CRAZYFLIE_BASE)/src/drivers/esp32/interface -INCLUDES += -I$(CRAZYFLIE_BASE)/src/hal/interface -INCLUDES += -I$(CRAZYFLIE_BASE)/src/modules/interface -I$(CRAZYFLIE_BASE)/src/modules/interface/kalman_core -I$(CRAZYFLIE_BASE)/src/modules/interface/lighthouse -INCLUDES += -I$(CRAZYFLIE_BASE)/src/utils/interface -I$(CRAZYFLIE_BASE)/src/utils/interface/kve -I$(CRAZYFLIE_BASE)/src/utils/interface/lighthouse -I$(CRAZYFLIE_BASE)/src/utils/interface/tdoa - +INCLUDES += -I$(srctree)/src/config +INCLUDES += -I$(srctree)/src/platform/interface +INCLUDES += -I$(srctree)/src/deck/interface -I$(srctree)/src/deck/drivers/interface +INCLUDES += -I$(srctree)/src/drivers/interface -I$(srctree)/src/drivers/bosch/interface +INCLUDES += -I$(srctree)/src/drivers/esp32/interface +INCLUDES += -I$(srctree)/src/hal/interface +INCLUDES += -I$(srctree)/src/modules/interface -I$(srctree)/src/modules/interface/kalman_core -I$(srctree)/src/modules/interface/lighthouse +INCLUDES += -I$(srctree)/src/utils/interface -I$(srctree)/src/utils/interface/kve -I$(srctree)/src/utils/interface/lighthouse -I$(srctree)/src/utils/interface/tdoa INCLUDES += -I$(LIB)/FatFS INCLUDES += -I$(LIB)/CMSIS/STM32F4xx/Include INCLUDES += -I$(LIB)/STM32_USB_Device_Library/Core/inc INCLUDES += -I$(LIB)/STM32_USB_OTG_Driver/inc INCLUDES += -I$(LIB)/STM32F4xx_StdPeriph_Driver/inc INCLUDES += -I$(LIB)/vl53l1 -I$(LIB)/vl53l1/core/inc +INCLUDES += -I$(KBUILD_OUTPUT)/include/generated -CFLAGS += -g3 -ifeq ($(DEBUG), 1) - CFLAGS += -O0 -DDEBUG - - # Prevent silent errors when converting between types (requires explicit casting) - CFLAGS += -Wconversion -else - CFLAGS += -Os - - # Fail on warnings - CFLAGS += -Werror -endif - -# Disable warnings for unaligned addresses in packed structs (added in GCC 9) -CFLAGS += -Wno-address-of-packed-member +# Here we tell Kbuild where to look for Kbuild files which will tell the +# buildsystem which sources to build +objs-y += src +objs-y += vendor -# Disable warnings for incorrectly detected region size (added in GCC 11) -# The compiler is not detecting properly GPIO structure size -CFLAGS += -Wno-array-bounds -CFLAGS += -Wno-stringop-overread -CFLAGS += -Wno-stringop-overflow - -ifeq ($(LTO), 1) - CFLAGS += -flto -endif +objs-y += app_api +objs-y += $(OOT) -CFLAGS += -DBOARD_REV_$(REV) -DESTIMATOR_NAME=$(ESTIMATOR)Estimator -DCONTROLLER_NAME=ControllerType$(CONTROLLER) -DPOWER_DISTRIBUTION_TYPE_$(POWER_DISTRIBUTION) - -CFLAGS += $(PROCESSOR) $(INCLUDES) +MEM_SIZE_FLASH_K = 1008 +MEM_SIZE_RAM_K = 128 +MEM_SIZE_CCM_K = 64 -CFLAGS += -Wall -Wmissing-braces -fno-strict-aliasing $(C_PROFILE) -std=gnu11 -# CFLAGS += -O0 -Wmissing-braces -fno-strict-aliasing $(C_PROFILE) -std=gnu11 #Use this compiler during debugger, as it has a different optimizer so you can better track variables -# Compiler flags to generate dependency files: -CFLAGS += -MD -MP -MF $(BIN)/dep/$(@).d -MQ $(@) -#Permits to remove un-used functions and global variables from output file -CFLAGS += -ffunction-sections -fdata-sections -# Prevent promoting floats to doubles -CFLAGS += -Wdouble-promotion +# +# Make sure Kbuild use our config that hinders some configs from being enabled +# on allyesconfig or randconfig. +# +export KCONFIG_ALLCONFIG ?= configs/all.config +KBUILD_OUTPUT ?= build -ASFLAGS = $(PROCESSOR) $(INCLUDES) -LDFLAGS += --specs=nosys.specs --specs=nano.specs $(PROCESSOR) -Wl,-Map=$(PROG).map,--cref,--gc-sections,--undefined=uxTopUsedPriority -LDFLAGS += -L$(CRAZYFLIE_BASE)/tools/make/F405/linker +-include $(KBUILD_OUTPUT)/include/config/auto.conf -#Flags required by the ST library -ifeq ($(CLOAD), 1) - LDFLAGS += -T $(LINKER_DIR)/FLASH_CLOAD.ld - LOAD_ADDRESS = $(LOAD_ADDRESS_CLOAD_$(CPU)) -else - LDFLAGS += -T $(LINKER_DIR)/FLASH.ld - LOAD_ADDRESS = $(LOAD_ADDRESS_$(CPU)) +# +# Special hack to handle float define. Kconfig has no float values +# so we use string. To avoid having to do atof in code we instead +# catch it here and convert to an, unquoted, float define. +# +ifneq ($(CONFIG_DECK_LOCO_2D_POSITION_HEIGHT),) +unquoted = $(patsubst "%",%,$(CONFIG_DECK_LOCO_2D_POSITION_HEIGHT)) +ARCH_CFLAGS += -DDECK_LOCO_2D_POSITION_HEIGHT=$(unquoted) endif -ifeq ($(LTO), 1) - LDFLAGS += -Os -flto -fuse-linker-plugin +ifeq ($(CONFIG_PLATFORM_TAG),y) +PLATFORM = tag endif -#Program name -PROG ?= $(PLATFORM) -#Where to compile the .o -BIN = bin -VPATH += $(BIN) - -#Dependency files to include -DEPS := $(foreach o,$(OBJ),$(BIN)/dep/$(o).d) - -##################### Misc. ################################ -ifeq ($(SHELL),/bin/sh) - COL_RED=\033[1;31m - COL_GREEN=\033[1;32m - COL_RESET=\033[m +ifeq ($(CONFIG_PLATFORM_BOLT), y) +PLATFORM = bolt endif -# This define n-thing is a standard hack to get newlines in GNU Make. -define n - - -endef +PLATFORM ?= cf2 +PROG ?= $(PLATFORM) -# Make sure that the submodules are up to date. -# Check if there are any files in the vendor directories, if not warn the user. -ifeq ($(wildcard $(CRAZYFLIE_BASE)/vendor/*/*),) - $(error $n \ - The submodules does not seem to be present, consider fetching them by:$n \ - $$ git submodule init$n \ - $$ git submodule update$n \ - ) +ifeq ($(CONFIG_DEBUG),y) +ARCH_CFLAGS += -O0 -Wconversion +else +ARCH_CFLAGS += -Os -Werror endif -#################### Targets ############################### +_all: -all: bin/ bin/dep bin/vendor check_submodules build -build: -# Each target is in a different line, so they are executed one after the other even when the processor has multiple cores (when the -j option for the make command is > 1). See: https://www.gnu.org/software/make/manual/html_node/Parallel.html - @$(MAKE) --no-print-directory clean_version CRAZYFLIE_BASE=$(CRAZYFLIE_BASE) - @$(MAKE) --no-print-directory compile CRAZYFLIE_BASE=$(CRAZYFLIE_BASE) - @$(MAKE) --no-print-directory print_version CRAZYFLIE_BASE=$(CRAZYFLIE_BASE) - @$(MAKE) --no-print-directory size CRAZYFLIE_BASE=$(CRAZYFLIE_BASE) -compile: $(PROG).hex $(PROG).bin $(PROG).dfu +all: $(PROG).hex $(PROG).bin + @echo "Build for the $(PLATFORM)!" + @$(PYTHON) $(srctree)/tools/make/versionTemplate.py --crazyflie-base $(srctree) --print-version + @$(PYTHON) $(srctree)/tools/make/size.py $(SIZE) $(PROG).elf $(MEM_SIZE_FLASH_K) $(MEM_SIZE_RAM_K) $(MEM_SIZE_CCM_K) -bin/: - mkdir -p bin + # + # Create symlinks to the ouput files in the build directory + # + for f in $$(ls $(PROG).*); do \ + ln -sf $(KBUILD_OUTPUT)/$$f $(srctree)/$$(basename $$f); \ + done -bin/dep: - mkdir -p bin/dep - -bin/vendor: - mkdir -p bin/vendor - -libarm_math.a: - +$(MAKE) -C $(CRAZYFLIE_BASE)/tools/make/cmsis_dsp/ CRAZYFLIE_BASE=$(abspath $(CRAZYFLIE_BASE)) PROJ_ROOT=$(CURDIR) V=$(V) CROSS_COMPILE=$(CROSS_COMPILE) - -clean_version: -ifeq ($(SHELL),/bin/sh) - @echo " CLEAN_VERSION" - @rm -f version.c -endif - -print_version: - @echo "Build for the $(PLATFORM_NAME_$(PLATFORM))!" - @$(PYTHON) $(CRAZYFLIE_BASE)/tools/make/versionTemplate.py --crazyflie-base $(CRAZYFLIE_BASE) --print-version -ifeq ($(CLOAD), 1) - @echo "Crazyloader build!" -endif -ifeq ($(FATFS_DISKIO_TESTS), 1) - @echo "WARNING: FatFS diskio tests enabled. Erases SD-card!" -endif +include tools/make/targets.mk size: - @$(PYTHON) $(CRAZYFLIE_BASE)/tools/make/size.py $(SIZE) $(PROG).elf $(MEM_SIZE_FLASH_K) $(MEM_SIZE_RAM_K) $(MEM_SIZE_CCM_K) + @$(PYTHON) $(srctree)/tools/make/size.py $(SIZE) $(PROG).elf $(MEM_SIZE_FLASH_K) $(MEM_SIZE_RAM_K) $(MEM_SIZE_CCM_K) -#Radio bootloader +# Radio bootloader +CLOAD ?= 1 cload: ifeq ($(CLOAD), 1) $(CLOAD_SCRIPT) $(CLOAD_CMDS) flash $(CLOAD_ARGS) $(PROG).bin stm32-fw @@ -451,6 +139,17 @@ else @echo "Only cload build can be bootloaded. Launch build and cload with CLOAD=1" endif +# Flags required by the ST library +ifeq ($(CLOAD), 1) + LOAD_ADDRESS = $(LOAD_ADDRESS_CLOAD_$(CPU)) +else + LOAD_ADDRESS = $(LOAD_ADDRESS_$(CPU)) +endif + +unit: +# The flag "-DUNITY_INCLUDE_DOUBLE" allows comparison of double values in Unity. See: https://stackoverflow.com/a/37790196 + rake unit "DEFINES=$(ARCH_CFLAGS) -DUNITY_INCLUDE_DOUBLE" "FILES=$(FILES)" "UNIT_TEST_STYLE=$(UNIT_TEST_STYLE)" + #Flash the stm. flash: $(OPENOCD) -d2 -f $(OPENOCD_INTERFACE) $(OPENOCD_CMDS) -f $(OPENOCD_TARGET) -c init -c targets -c "reset halt" \ @@ -463,7 +162,8 @@ flash_verify: -c "verify_image $(PROG).bin $(LOAD_ADDRESS) bin" -c "reset run" -c shutdown flash_dfu: - $(DFU_UTIL) -a 0 -D $(PROG).dfu + $(PYTHON) tools/make/usb-bootloader.py + $(DFU_UTIL) -d 0483:df11 -a 0 -D $(PROG).dfu -s :leave #STM utility targets halt: @@ -484,7 +184,7 @@ rtt: gdb: $(PROG).elf $(GDB) -ex "target remote localhost:3333" -ex "monitor reset halt" $^ - + erase: $(OPENOCD) -d2 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets -c "halt" -c "stm32f4x mass_erase 0" -c shutdown @@ -493,20 +193,16 @@ prep: @$(CC) $(CFLAGS) -dM -E - < /dev/null check_submodules: - @cd $(CRAZYFLIE_BASE); $(PYTHON) tools/make/check-for-submodules.py - -include $(CRAZYFLIE_BASE)/tools/make/targets.mk + @cd $(srctree); $(PYTHON) tools/make/check-for-submodules.py -#include dependencies --include $(DEPS) +# Give control over to Kbuild +-include tools/kbuild/Makefile.kbuild -unit: -# The flag "-DUNITY_INCLUDE_DOUBLE" allows comparison of double values in Unity. See: https://stackoverflow.com/a/37790196 - rake unit "DEFINES=$(CFLAGS) -DUNITY_INCLUDE_DOUBLE" "FILES=$(FILES)" "UNIT_TEST_STYLE=$(UNIT_TEST_STYLE)" +ifeq ($(KBUILD_SRC),) # Python bindings -MOD_INC = $(CRAZYFLIE_BASE)/src/modules/interface -MOD_SRC = $(CRAZYFLIE_BASE)/src/modules/src +MOD_INC = src/modules/interface +MOD_SRC = src/modules/src bindings_python: bindings/setup.py bin/cffirmware_wrap.c $(MOD_SRC)/*.c $(PYTHON) bindings/setup.py build_ext --inplace @@ -517,5 +213,6 @@ bin/cffirmware_wrap.c cffirmware.py: bindings/cffirmware.i $(MOD_INC)/*.h test_python: bindings_python $(PYTHON) -m pytest test_python +endif .PHONY: all clean build compile unit prep erase flash check_submodules trace openocd gdb halt reset flash_dfu flash_verify cload size print_version clean_version bindings_python diff --git a/README.md b/README.md index 042eeee575..6245697158 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,13 @@ and to view it in a web page ```tb docs``` +## Contribute +Go to the [contribute page](https://www.bitcraze.io/contribute/) on our website to learn more. + +### Test code for contribution + +To run the tests please have a look at the [unit test documentation](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/development/unit_testing/). + ## License The code is licensed under LGPL-3.0 diff --git a/app_api/Kbuild b/app_api/Kbuild new file mode 100644 index 0000000000..ddf217ef0e --- /dev/null +++ b/app_api/Kbuild @@ -0,0 +1 @@ +obj-$(CONFIG_APP_API_TEST) += src/ diff --git a/app_api/Kconfig b/app_api/Kconfig new file mode 100644 index 0000000000..888fe40ac1 --- /dev/null +++ b/app_api/Kconfig @@ -0,0 +1,5 @@ +config APP_API_TEST + bool "Build API test application" + default n + help + Builds an application that uses all app layer API. diff --git a/app_api/Makefile b/app_api/Makefile index cbae7a8056..c849067cab 100644 --- a/app_api/Makefile +++ b/app_api/Makefile @@ -1,10 +1,23 @@ +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. -# enable app support -APP=1 -APP_STACKSIZE=300 +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../ -VPATH += src/ -PROJ_OBJ += app_main.o +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config -CRAZYFLIE_BASE=.. -include $(CRAZYFLIE_BASE)/Makefile +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/app_api/app-config b/app_api/app-config new file mode 100644 index 0000000000..1112138a19 --- /dev/null +++ b/app_api/app-config @@ -0,0 +1,4 @@ +CONFIG_APP_API_TEST=y +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/app_api/src/Kbuild b/app_api/src/Kbuild new file mode 100644 index 0000000000..015b24ccda --- /dev/null +++ b/app_api/src/Kbuild @@ -0,0 +1 @@ +obj-y += app_main.o diff --git a/app_api/src/app_main.c b/app_api/src/app_main.c index 7045feef09..0ceb397fd3 100644 --- a/app_api/src/app_main.c +++ b/app_api/src/app_main.c @@ -85,6 +85,7 @@ void appMain() { } // LPS + #ifdef CONFIG_DECK_LOCO { point_t position; uint8_t unorderedAnchorList[5]; @@ -93,6 +94,7 @@ void appMain() { locoDeckGetAnchorIdList(unorderedAnchorList, 5); locoDeckGetActiveAnchorIdList(unorderedAnchorList, 5); } + #endif // Memory sub system { diff --git a/configs/all.config b/configs/all.config new file mode 100644 index 0000000000..49ba903f1b --- /dev/null +++ b/configs/all.config @@ -0,0 +1,12 @@ +# +# This config files include values that should be set and not randomized when +# doing an make randconfig. +# + +# +# If the app entry point is available and no app is linked in, we will get +# a linker error. +# CONFIG_APP_ENABLE is not set +# +# Same is true for OOT estimator +# CONFIG_ESTIMATOR_OOT is not set diff --git a/configs/app_api.conf b/configs/app_api.conf new file mode 100644 index 0000000000..a92ada9eed --- /dev/null +++ b/configs/app_api.conf @@ -0,0 +1 @@ +CONFIG_APP_API_TEST=y diff --git a/configs/bigquad.conf b/configs/bigquad.conf new file mode 100644 index 0000000000..66397ab697 --- /dev/null +++ b/configs/bigquad.conf @@ -0,0 +1,4 @@ +CONFIG_DECK_BIGQUAD=y +CONFIG_DECK_BIGQUAD_ENABLE=y +CONFIG_DECK_BIGQUAD_ENABLE_OSD=y +CONFIG_DECK_BIGQUAD_ENABLE_PM=y diff --git a/configs/bolt_defconfig b/configs/bolt_defconfig new file mode 100644 index 0000000000..330ba20351 --- /dev/null +++ b/configs/bolt_defconfig @@ -0,0 +1,5 @@ +CONFIG_PLATFORM_BOLT=y + +CONFIG_ESTIMATOR=any +CONFIG_CONTROLLER=Any +CONFIG_POWER_DISTRIBUTION=stock diff --git a/configs/bosch.conf b/configs/bosch.conf new file mode 100644 index 0000000000..470384bd5c --- /dev/null +++ b/configs/bosch.conf @@ -0,0 +1 @@ +CONFIG_SENSORS_BOSCH=y diff --git a/configs/cf2_defconfig b/configs/cf2_defconfig new file mode 100644 index 0000000000..2edbe72037 --- /dev/null +++ b/configs/cf2_defconfig @@ -0,0 +1,4 @@ +CONFIG_PLATFORM_CF2=y +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/configs/defconfig b/configs/defconfig new file mode 100644 index 0000000000..4c74602582 --- /dev/null +++ b/configs/defconfig @@ -0,0 +1 @@ +CONFIG_CROSS_COMPILE="arm-none-eabi-" diff --git a/configs/loco_tdma.conf b/configs/loco_tdma.conf new file mode 100644 index 0000000000..605d4d8850 --- /dev/null +++ b/configs/loco_tdma.conf @@ -0,0 +1,5 @@ +CONFIG_DECK_LOCO=y +CONFIG_DECK_LOCO_ALGORITHM_TWR=y +CONFIG_DECK_LOCO_TDMA=y +CONFIG_DECK_LOCO_TDMA_SLOTS=1 +CONFIG_DECK_LOCO_TDMA_SLOT=0 diff --git a/configs/loco_tdoa2.conf b/configs/loco_tdoa2.conf new file mode 100644 index 0000000000..d87eb5d39f --- /dev/null +++ b/configs/loco_tdoa2.conf @@ -0,0 +1,2 @@ +CONFIG_DECK_LOCO=y +CONFIG_DECK_LOCO_ALGORITHM_TDOA2=y diff --git a/configs/loco_tdoa3.conf b/configs/loco_tdoa3.conf new file mode 100644 index 0000000000..9a443efcc5 --- /dev/null +++ b/configs/loco_tdoa3.conf @@ -0,0 +1,2 @@ +CONFIG_DECK_LOCO=y +CONFIG_DECK_LOCO_ALGORITHM_TDOA3=y diff --git a/configs/loco_tdoa3_all.conf b/configs/loco_tdoa3_all.conf new file mode 100644 index 0000000000..941b76d6c9 --- /dev/null +++ b/configs/loco_tdoa3_all.conf @@ -0,0 +1,4 @@ +CONFIG_DECK_LOCO=y +CONFIG_DECK_LOCO_ALGORITHM_TDOA3=y +CONFIG_DECK_LOCO_2D_POSITION=y +CONFIG_DECK_LOCO_2D_POSITION_HEIGHT=1.2 diff --git a/configs/tag_defconfig b/configs/tag_defconfig new file mode 100644 index 0000000000..8d0ab8466c --- /dev/null +++ b/configs/tag_defconfig @@ -0,0 +1,8 @@ +CONFIG_PLATFORM_TAG=y + +CONFIG_ESTIMATOR=any +CONFIG_CONTROLLER=Any +CONFIG_POWER_DISTRIBUTION=stock + +CONFIG_DECK_FORCE=bcDWM1000 +CONFIG_SENSORS_IGNORE_BAROMETER_FAIL=y diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index f2cef3c252..0df9ba5e40 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -8,6 +8,7 @@ - {page_id: logparam} - {page_id: eventtrigger} - {page_id: app_layer} + - {page_id: platform} - title: Functional areas subs: - {page_id: crtp_index} @@ -31,6 +32,8 @@ - {page_id: memory_management} - {page_id: dfu} - {page_id: oot} + - {page_id: kbuild} + - {page_id: create_platform} - title: Auto-generated API Documentation subs: - {page_id: logs} diff --git a/docs/building-and-flashing/build.md b/docs/building-and-flashing/build.md index 4ac078a1f1..1bd0381a41 100644 --- a/docs/building-and-flashing/build.md +++ b/docs/building-and-flashing/build.md @@ -17,8 +17,8 @@ Our policy for toolchain is to follow what is available in the oldest [Ubuntu Lo This means that if the firmware can not be compiled using gcc 6.3, **or anything newer**, it should be considered a bug. #### OS X ```bash -brew tap PX4/homebrew-px4 -brew install gcc-arm-none-eabi +$ brew tap PX4/homebrew-px4 +$ brew install gcc-arm-none-eabi ``` #### Debian/Ubuntu @@ -26,21 +26,21 @@ brew install gcc-arm-none-eabi For Ubuntu 18.04: ```bash -sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa -sudo apt-get update -sudo apt install gcc-arm-embedded +$ sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa +$ sudo apt-get update +$ sudo apt install gcc-arm-embedded ``` For Ubuntu 20.04 and 20.10: ```bash -sudo apt-get install make gcc-arm-none-eabi +$ sudo apt-get install make gcc-arm-none-eabi ``` #### Arch Linux ```bash -sudo pacman -S community/arm-none-eabi-gcc community/arm-none-eabi-gdb community/arm-none-eabi-newlib +$ sudo pacman -S community/arm-none-eabi-gcc community/arm-none-eabi-gdb community/arm-none-eabi-newlib ``` #### Windows @@ -53,7 +53,7 @@ To get started you need to [enable WSL and install an Ubuntu system](https://doc This can be done by opening `power shell` as administrator and typing: ``` -wsl --install +$ wsl --install ``` Then follow the [install instruction for Ubuntu 20.04](#debianubuntu) above to install the required build dependencies. @@ -71,7 +71,7 @@ The Crazyflie makefile will automatically use the Windows python when running in This repository uses git submodules. Clone with the `--recursive` flag ```bash -git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git +$ git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git ``` **Note** Make sure there are no spaces in the folder structure leading up to the repository (example: _/a/path/with/no/spaces/crazyflie-firmware/_ vs _a/path with spaces/crazyflie-firmware/_). Our build system can not handle file system paths with spaces in them, and compilation will fail. @@ -80,64 +80,58 @@ If you already have cloned the repo without the `--recursive` option, you need t get the submodules manually ```bash -cd crazyflie-firmware -git submodule init -git submodule update +$ cd crazyflie-firmware +$ git submodule init +$ git submodule update ``` ## Compiling -### Crazyflie 2.X and Crazyflie Bolt +### Building the default firmware + +Before you can build the firmware, you will need to configure it. To get the default configuration you can write: -This is the default build so just running ```make``` is enough or: ```bash -make PLATFORM=cf2 +$ make cf2_defconfig +$ make -j 12 ``` -or with the toolbelt +or with the toolbelt: ```bash -tb make PLATFORM=cf2 +$ tb make cf2_defconfig +$ tb make ``` -### Roadrunner - -Use the ```tag``` platform +### Bolt and Roadrunner +We have some ready-to-go config files in the `configs/` directory. So, for example, if you want to build the Roadrunner (tag) you can go: ```bash -make PLATFORM=tag +$ make tag_defconfig +$ make -j 12 ``` -or with the toolbelt +Or for the bolt you can go: ```bash -tb make PLATFORM=tag +$ make bolt_defconfig +$ make -j 12 ``` -### Platform specific options -In `cf2.mk` or `tag.mk` in the `tools/make/` folder you can find additional compile options, for example which ESTIMATOR or CONTROLLER to use as default. -``` -######### Stabilizer configuration ########## -ESTIMATOR ?= any -CONTROLLER ?= Any # one of Any, PID, Mellinger, INDI -POWER_DISTRIBUTION ?= stock -``` +### Customize the firmware with kbuild (Advanced) -### config.mk -To create custom build options create a file called `config.mk` in the `tools/make/` -folder and fill it with options. E.g. -``` -PLATFORM=CF2 -DEBUG=1 -``` -More information can be found on the -[Bitcraze documentation](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/) +**Please note** that these instructions are only meant for you if you want to build an custom firmware for a custom platform than the ones we have listed above. You can still configure and change the firmware without kbuild. -# Make targets: +Please go to [these instructions](/docs/development/kbuild.md) to learn how to use the menuconfig. +# Make targets + + + +## General targets ``` all : Shortcut for build compile : Compile cflie.hex. WARNING: Do NOT update version.c @@ -157,6 +151,18 @@ rtt : Start RTT server. Compile the firmware with "DEBUG_PRINT_ON_SEGGER_ and the console is visible over TCP on port 2000 "telnet localhost 2000". ``` +## Noteable Kbuild targets +``` +menuconfig : Open up a terminal user interface to set configuration options +defconfig : Generate a `.config` with the default configuration options +cf2_defconfig : Merge configuration options from `configs/cf2_defconfig` with default +tag_defconfig : Merge configuration options from `configs/tag_defconfig` with default +bolt_defconfig : Merge configuration options from `configs/bolt_defconfig` with default +allyesconfig : Generate a `.config` with the all configuration options enabled +allnoconfig : Generate a `.config` with the all configuration options disabled +randconfig : Generate a `.config` with random valid values to all configuration options +``` + # Flashing Writing a new binary to the Crazyflie is called flashing (writing it to the flash memory). This page describes how to flash from the command line and there are a few different ways to do it. @@ -178,7 +184,7 @@ The supported way to flash when developping for the Crazyflie is to use the Craz * In your terminal, run ```bash -make cload +$ make cload ``` It will try to find a Crazyflie in bootloader mode and flash the binary to it. @@ -186,10 +192,8 @@ It will try to find a Crazyflie in bootloader mode and flash the binary to it. Warning: if multiple Crazyflies within range are in bootloader mode the result is unpredictable. This method is not suitable in classroom situation where it is likely that several students are flashing at the same time. Also remember that the Crazyradio PA often reaches into the next room. ### Automatically enter bootloader mode - -* Add the address of the crazyflie to the [`config.mk`](#configmk) file, for instance `CLOAD_CMDS = -w radio://0/80/2M` * Make sure the Crazyflie is on -* In your terminal, run `make cload` +* In your terminal, run `CLOAD_CMDS="-w radio://0/80/2M" make cload` It will connect to the Crazyflie with the specified address, put it in bootloader mode and flash the binary. This method is suitable for classroom situations. diff --git a/docs/development/create_platform.md b/docs/development/create_platform.md new file mode 100644 index 0000000000..4a00d72067 --- /dev/null +++ b/docs/development/create_platform.md @@ -0,0 +1,227 @@ +--- +title: Creating Your Own Platform +page_id: create_platform +--- + +Creating your own platform can be a good way of saving work by ... + + * Tailoring a config that is specific to your product or project! + * To make sure you have sensible default values for parameters + * Or to make sure that the initialization is done correctly + +If you contribute it to the main Bitcraze repository it is also a way to distribute your work! +So how do you do it? Follow these steps! + +## Add your platform to Kconfig +To make it possible to select your platform for the build you need to add it to +a `Kconfig` file. You can find the platform selection in the root `Kconfig` file. + +Right now the selction looks like: + +```Kconfig +menu "Platform configuration" + +choice + prompt "Platform to build" + default CONFIG_PLATFORM_CF2 + +config PLATFORM_CF2 + bool "Build for CF2" + select SENSORS_BMI088_BMP388 + select SENSORS_MPU9250_LPS25H + +config PLATFORM_BOLT + bool "Build for Bolt" + select SENSORS_BMI088_BMP388 + select SENSORS_BMI088_SPI + +config PLATFORM_TAG + bool "Build for the roadrunner" + select SENSORS_BMI088_BMP388 + +endchoice +``` + +Let's add our own platform, `RINCEWIND`, which has the same sensors as the Bolt. + +```Kconfig +menu "Platform configuration" + +choice + prompt "Platform to build" + default CONFIG_PLATFORM_CF2 + +config PLATFORM_CF2 + bool "Build for CF2" + select SENSORS_BMI088_BMP388 + select SENSORS_MPU9250_LPS25H + +config PLATFORM_BOLT + bool "Build for Bolt" + select SENSORS_BMI088_BMP388 + select SENSORS_BMI088_SPI + +config PLATFORM_TAG + bool "Build for the roadrunner" + select SENSORS_BMI088_BMP388 + +config PLATFORM_RINCEWIND + bool "Build for the Rincewind platform" + select SENSORS_BMI088_BMP388 + select SENSORS_BMI088_SPI + +endchoice +``` + +This creates the `CONFIG_PLATFORM_RINCEWIND` variable, usable both from C code as a define (when you include `autoconf.h`) as well as an environment variable usable from the Makefiles and the Kbuild files. + +## Add a init source file to Kbuild +We need to add a entry point for your platform. The way the build system determines which platform init file to include is found in the `src/platform/src/Kbuild` file: + +```Makefile +obj-$(CONFIG_PLATFORM_BOLT) += platform_bolt.o +obj-$(CONFIG_PLATFORM_CF2) += platform_cf2.o +obj-$(CONFIG_PLATFORM_TAG) += platform_tag.o +obj-y += platform.o +obj-y += platform_stm32f4.o +obj-y += platform_utils.o +``` + +Let's add `RINCEWIND`: + +```Makefile +obj-$(CONFIG_PLATFORM_BOLT) += platform_bolt.o +obj-$(CONFIG_PLATFORM_CF2) += platform_cf2.o +obj-$(CONFIG_PLATFORM_TAG) += platform_tag.o +obj-$(CONFIG_PLATFORM_RINCEWIND) += platform_rincewind.o +obj-y += platform.o +obj-y += platform_stm32f4.o +obj-y += platform_utils.o +``` + +We will base the `src/platform/src/platform_rincewind.c` file on the bolt: + +```c +#define DEBUG_MODULE "PLATFORM" + +#include + +#include "platform.h" +#include "exti.h" +#include "nvic.h" +#include "debug.h" + +static platformConfig_t configs[] = { +#ifdef CONFIG_SENSORS_BMI088_SPI + { + .deviceType = "CB10", + .deviceTypeName = "Rincewind", + .sensorImplementation = SensorImplementation_bmi088_spi_bmp388, + .physicalLayoutAntennasAreClose = false, + .motorMap = motorMapBoltBrushless, + } +#endif +}; + +const platformConfig_t* platformGetListOfConfigurations(int* nrOfConfigs) { + *nrOfConfigs = sizeof(configs) / sizeof(platformConfig_t); + return configs; +} + +void platformInitHardware() { + // Low level init: Clock and Interrupt controller + nvicInit(); + + // EXTI interrupts + extiInit(); +} + +const char* platformConfigGetPlatformName() { + return "Rincewind"; +} + +``` + +The `platformInitHardware()` and `platformGetListOfConfigurations()` functions is called by `platform.c` as part of general init of the device. + + +## Add platform default parameter values +Your platform need to define suitable default values to (persistent) parameters. To do this we will need to create a `src/platform/interface/platform_rincewind.h` and make sure that this gets included when we build for the `RINCEWIND` platform. This is done by adding to the `src/platform/interface/platform_defaults.h`: + +```c +#pragma once + +#define __INCLUDED_FROM_PLATFORM_DEFAULTS__ + +#ifdef CONFIG_PLATFORM_CF2 + #include "platform_defaults_cf2.h" +#endif +#ifdef CONFIG_PLATFORM_BOLT + #include "platform_defaults_bolt.h" +#endif +#ifdef CONFIG_PLATFORM_TAG + #include "platform_defaults_tag.h" +#endif +``` + +Becomes: + + +```c +#pragma once + +#define __INCLUDED_FROM_PLATFORM_DEFAULTS__ + +#ifdef CONFIG_PLATFORM_CF2 + #include "platform_defaults_cf2.h" +#endif +#ifdef CONFIG_PLATFORM_BOLT + #include "platform_defaults_bolt.h" +#endif +#ifdef CONFIG_PLATFORM_TAG + #include "platform_defaults_tag.h" +#endif +#ifdef CONFIG_RINCEWIND + #include "platform_defaults_bolt.h" + #include "platform_defaults_rincewind.h" +#endif +``` + +We base it on the default parameters for the Bolt. To determine how to add content to `platform_defaults_rincewind.h` please check what is added to the existing platforms. + +## Add a platform default config +To make it easier for people to build for `RINCEWIND` we can add a `defconfig` for it. This is done by adding a file in the `configs/` folder. Let us look at `bolt_defconfig`: + +```Makefile +CONFIG_PLATFORM_BOLT=y + +CONFIG_ESTIMATOR=any +CONFIG_CONTROLLER=Any +CONFIG_POWER_DISTRIBUTION=stock +``` + +Based on this a start of `rincewind_defconfig` could be: + +```Makefile +CONFIG_PLATFORM_RINCEWIND=y + +CONFIG_ESTIMATOR=any +CONFIG_CONTROLLER=Any +CONFIG_POWER_DISTRIBUTION=stock +``` + +Then `RINCEWIND` platform could be built by: + +```Bash +$ make rincewind_defconfig +make[1]: Entering directory '/home/jonasdn/sandbox/kbuild-firmware/build' + GEN ./Makefile +# +# configuration written to .config +# +make[1]: Leaving directory '/home/jonasdn/sandbox/kbuild-firmware/build' +$ make -j 12 +[...] +``` + +And you are done! You have created your own platform, good job! diff --git a/docs/development/howto.md b/docs/development/howto.md index c49e062cc7..442ad9f61e 100644 --- a/docs/development/howto.md +++ b/docs/development/howto.md @@ -58,10 +58,10 @@ DECK_DRIVER(helloDriver); Adding the driver to the build ------------------------------ -Add this to the Makefile, after the line \'\# Decks\': +Add this to the `Kbuild` file in `src/deck/drivers/src/`: ``` {.make} -PROJ_OBJ += hello.o +obj-y += hello.o ``` Enabling the driver @@ -70,20 +70,15 @@ Enabling the driver Decks can have a memory that contains its name. In our case the hello driver would be initialized only when a deck identified as \"myHello\" is installed on the Crazyflie. For development purpose it is possible to -force enabling a deck driver with a compile flag. To do so create the -file tools/make/config.mk with the content: +force enabling a deck driver with a compile flag. To do so set the +`CONFIG_DECK_FORCE` config option to `myHello` in your `.config` either +by hand or using `make menuconfig`. -``` {.make} -CFLAGS += -DDECK_FORCE=myHello - -DEBUG=1 -``` - -DEBUG=1 allows to get more information from the Crazyflie console when +`CONFIG_DEBUG=y` allows to get more information from the Crazyflie console when it starts. Debug should not be enabled if you intend to fly the Crazyflie out of the lab (it disables the watchdog). -**Note** Each time you modify config.mk you +**Note** Each time you modify your `.config` you should recompile the full firmware by cleaning up the build folder with \'make clean\' diff --git a/docs/development/kbuild.md b/docs/development/kbuild.md new file mode 100644 index 0000000000..009db209b2 --- /dev/null +++ b/docs/development/kbuild.md @@ -0,0 +1,59 @@ +--- +title: Customize firmware with kbuild +page_id: kbuild +--- + +The Crazyflie firmware uses a version of the [*KBuild*](https://www.kernel.org/doc/html/latest/kbuild/index.html) build system. Similar to what the [Linux Kernel](https://www.kernel.org/) or the [Busybox project](https://busybox.net/) uses. + +What to build is determined by a configuration file, which is named `.config` and resides in the root of the firmware repository. If you only type `make` in a fresh repository the build system will generate the default config. You can inspect it by going, for instance: + +```bash +$ less .config +# +# Automatically generated file; DO NOT EDIT. +# Crazyflie Platform Configuration +# + +# +# Build and compiler options +# +CONFIG_CROSS_COMPILE="arm-none-eabi-" +# CONFIG_DEBUG is not set + +# +# Platform configuration +# +CONFIG_PLATFORM_CF2=y +# CONFIG_PLATFORM_TAG is not set + +# +# Sensor configuration +# +CONFIG_SENSORS_MPU9250_LPS25H=y +CONFIG_SENSORS_BMI088_BMP388=y +[...] +``` + +If you want to customize your build you can use the menuconfig by typing: + +```bash +$ make menuconfig +``` + +This will drop you into a terminal based user interface where you can configure and customize what will be included in the firmware. +To use this you need to have some extra packages installed on your system, the equivelent of the following Ubuntu packages: + +```bash +$ sudo apt install build-essential libncurses5-dev +``` + +To get an idea of how `menuconfig` will look, please see the images below. + +![Main menu of menuconfig](/docs/images/kbuild1.png) + +![Configuring deck drivers](/docs/images/kbuild2.png) + +### Platform specific options + +Read more about platforms in the [platform section.](/docs/userguides/platform.md) + diff --git a/docs/development/oot.md b/docs/development/oot.md index b2c7ba783c..b814c16868 100644 --- a/docs/development/oot.md +++ b/docs/development/oot.md @@ -5,21 +5,52 @@ page_id: oot It is possible to have an out-of-tree build of parts of the crazyflie firmware. This enables developers to work on elements without worrrying about merging it with the full code base. -# App layer. -Technically the app layer is an example of an out of tree build. Follow the [app layer instructions](/docs/userguides/app_layer.md) for this. - -# OOT estimators +# General out-of-tree build process In a seperate folder make a Makefile which contain the following content: +```Makefile +CRAZYFLIE_BASE := [LOCATION OF THE CRAZYFLIE FIRMWARE] + +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/config + +include $(CRAZYFLIE_BASE)/tools/make/oot.mk +``` + +This will make the crazyflie-firmware build system look for a `Kbuild` file in your folder. + +The following variables are understood by `oot.mk`: + +| Variable | Function | Default | +| -------- | --------------------------------------------------- | ----------------------------------------------------- | +| `OOT` | Specify where your code (`Kbuild file`) is located. | `$(PWD)` (Your current directory) | +| `OOT_CONFIG` | Location of your OOT specific `Kconfig` file, will be merged with the default config. | `$(OOT)/oot-config` | +| `EXTRA_CFLAGS`| Extra CFLAGS needed by your app | Empty. + +And `oot.mk` also expects `$(CRAZYFLIE_BASE)` to be set to the path to the `crazyflie-firmware` repository. + +The `Kbuild` file in the `$(OOT)` folder should point out your source files: + +```Makefile +obj-y += your_estimator_out_of_tree.o +``` + +It can also add point out another folder where the code resides: + +```Makefile +obj-y += src/ ``` -CFLAGS += -DOOT_ESTIMATOR -VPATH += src/ -PROJ_OBJ += estimator_out_of_tree.o +And since you are providing a config file by way of `$(OOT_CONFIG)` you do not need to run any make command to create a config like `make menuconfig` or `make defconfig`. Just a simple `make` will suffice. +# OOT estimators +The `config` file needs to enable ESTIMATOR_OOT, and can also set other config options: -CRAZYFLIE_BASE=[LOCATION OF THE CRAZYFLIE FIRMWARE] -include $(CRAZYFLIE_BASE)/Makefile -``` +``` +CONFIG_ESTIMATOR_OOT=y +``` in [your_estimator_out_of_tree].c in the src folder you will just need to make sure that the following functions are filled: @@ -31,3 +62,6 @@ in [your_estimator_out_of_tree].c in the src folder you will just need to make s # OOT Controllers Not yet implemented. Please request this feature in the [crazyflie-firmware issue list](https://github.com/bitcraze/crazyflie-firmware/issues) + +# App layer +Technically the app layer is an example of an out of tree build. Follow the [app layer instructions](/docs/userguides/app_layer.md) for this. diff --git a/docs/development/openocd_gdb_debugging.md b/docs/development/openocd_gdb_debugging.md index 896af36e71..4bea57cb7d 100644 --- a/docs/development/openocd_gdb_debugging.md +++ b/docs/development/openocd_gdb_debugging.md @@ -66,7 +66,7 @@ Inside of the file, replace everything with the following: { "name": "STM32 Debug", "cwd": "${workspaceRoot}", - "executable": "./cf2.elf", + "executable": "./build/cf2.elf", "request": "launch", "type": "cortex-debug", "device": "STM32F405", @@ -228,4 +228,4 @@ line in the gray area or press Shift+Ctrl+B). Now you should be able to read out the values of the defined variables at that very position. > **_NOTE:_** -> Make sure your executable (cf2.elf) is identical to the one running on your Crazyflie. \ No newline at end of file +> Make sure your executable (cf2.elf) is identical to the one running on your Crazyflie. diff --git a/docs/development/starting_development.md b/docs/development/starting_development.md index acdee99efb..f736b58c28 100644 --- a/docs/development/starting_development.md +++ b/docs/development/starting_development.md @@ -27,7 +27,7 @@ Then make the firmware. For **Crazyflie 2.X**: ``` -crazyflie-firmware$ make PLATFORM=cf2 +crazyflie-firmware$ make (...) DFUse cf2.dfu Build for the CF2 platform! diff --git a/docs/functional-areas/crtp/crtp_console.md b/docs/functional-areas/crtp/crtp_console.md index f61ccb5829..5cd2ccb9e8 100644 --- a/docs/functional-areas/crtp/crtp_console.md +++ b/docs/functional-areas/crtp/crtp_console.md @@ -9,15 +9,26 @@ Crazyflie to a host using the consoleprintf function. Communication protocol ====================== - Answer (Crazyflie to host): + Port: 0 + Channel: 0 + + Payload (Crazyflie to host): +---------//-----------+ | PRINTED CONSOLE TEXT | +---------//-----------+ Length 0-31 -The contents of the buffer on the copter side is sent if any of the -following is fulfilled: +The content is the text from the console. The encoding is ascii/UTF8 however +this cannot be guaranteed so the packet receiver should be ready for binary +buffer. + +The best is to not make any assumption about the data received on the console +port: it could be a line, multiple line, part of a line, ... + + +The current algorithm in the Crazyflie is to send a console packet is any of +these condition are fulfilled: -- The output buffer (of 31 bytes) is full -- A \"newline\" character has to be send (\\n and/or \\r) -- A flush command as been issued + - The output buffer (of 31 bytes) is full + - A \"newline\" character has to be send (\\n and/or \\r) + - The flush function has been called diff --git a/docs/functional-areas/crtp/crtp_link.md b/docs/functional-areas/crtp/crtp_link.md new file mode 100644 index 0000000000..9d302c181c --- /dev/null +++ b/docs/functional-areas/crtp/crtp_link.md @@ -0,0 +1,37 @@ +--- +title: Link layer services +page_id: crtp_link +--- + +This ports handle various link-related low level services. It most notably host the *null packet* used by the radio +link to pull downlink packets. + +## CRTP channels + +| port | channel | Function | +|------|---------|----------| +| 13 | 0 | [Echo](#platform-commands) | +| 13 | 1 | [Source](#platform-commands) | +| 13 | 2 | [Sink](#app-channel) | +| 13 | 3 | [*Null packets*](#null-packet) | + +## Echo + +Packets sent to the Echo channel are send back unaltered. + +## Source + +When receiving any packet on the source channel, the Crazyflie sends back a 32 bytes packet on the source channel. + +Since protocol version 1, this packet contains the string "Bitcraze Crazyflie" followed by zeros. Before version 1 the +content of the packet was not defined. This allows to detect firmware protocol version bellow 1 (the getProtocolVersion +packet was only implemented after version 1 of the protocol). + +## Sink + +Packet sent to the sink channel are dropped and ignored. + +## Null packet + +Null packets must be dropped. The data part of NULL packet is used for some out-of-band communication at the link +level or by the bootloader. The Crazyflie firmware and lib should ignore them. \ No newline at end of file diff --git a/docs/functional-areas/crtp/crtp_platform.md b/docs/functional-areas/crtp/crtp_platform.md new file mode 100644 index 0000000000..fec6f2f2f4 --- /dev/null +++ b/docs/functional-areas/crtp/crtp_platform.md @@ -0,0 +1,117 @@ +--- +title: Platform services +page_id: crtp_platform +--- + +This ports implements miscellaneous platform-related functionality for the Crazyflie. + +## CRTP channels + +| port | channel | Function | +|------|---------|----------| +| 13 | 0 | [Platform commands](#platform-commands) | +| 13 | 1 | [Version commands](#platform-commands) | +| 13 | 2 | [App channel](#app-channel) | +| 13 | 3 | *Reserved for future use* | + +## Platform commands + +The first byte describe the command: + +| value | Command | +|-------|---------| +| 0 | [Set continuous wave](#set-continuous-wave) | + +### Set continuous wave + +Command and answer: + +| Byte | Description | +|------|-------------| +| 0 | setContinuousWave (0) | +| 1 | Enable | + +If enable is not 0, the Crazyflie radio will start transmitting a continuous sine wave at the currently setup +freqency. The same packet is sent back to confirm the value has been set. + +This command should only be sent over USB (it disables the radio communication). +It is used in production to test the Crazyflie radio path and should not be used outside of a lab or +other very controlled environment. It will effectively jam local radio communication on the channel. + +## Version commands + +The first byte describes the command: + +| value | Command | +|-------|---------| +| 0 | Get protocol version | +| 1 | Get firmware version | +| 2 | Get device type name | + +### Get protocol version + +Command: + +| Byte | Description | +|------|-------------| +| 0 | getProtocolVersion (0) | + +Answer: + +| Byte | Description | +|------|-------------| +| 0 | getProtocolVersion (0) | +| 1 | Version | + +Returns the CRTP protocol version. See the +[protocol versioning and stability guarantee](index.md#protocol-version-and-stability-guarantee) documentation for more +information. + +### Get firmware version + +Command: + +| Byte | Description | +|------|-------------| +| 0 | getFirmwareVersion (1) | + +Answer: + +| Byte | Description | +|------|-------------| +| 0 | getFirmwareVersion (1) | +| 1.. | Version string | + +Returns a string representation of the current firmware version. This returns the GIT tag of the source code where the +firmware was built. For a release the version string will look like "2022.01". For a build between releases the +number of commit since the release will be added, for example "2022.01 +42". + +### Get device type name + +Command: + +| Byte | Description | +|------|-------------| +| 0 | getDeviceTypeName (2) | + +Answer: + +| Byte | Description | +|------|-------------| +| 0 | getDeviceTypeName (2) | +| 1.. | Device type name string | + +Returns a String representation of the device type the firmware is running on. The currently existing device type are: + +| Device Type | Device type name | +|-------------|------------------| +| RZ10 | Crzyflie Bold | +| CF20 | Crazyflie 2.0 | +| CF2.1 | Crazyflie 2.1 | +| RR10 | Roadrunner 1.0 | + +## App channel + +The app channel is intended to be used by user apps on the Crazyflie and on the ground to exchange data. Every packet +sent and received from the app channel (port:channel) (13:2) will be available through the +[app channel API](/docs/userguides/app_layer.md#app-channel-packet-based-communication-between-the-crazyflie-and-the-python-lib). \ No newline at end of file diff --git a/docs/functional-areas/crtp/index.md b/docs/functional-areas/crtp/index.md index 88d39f1b4c..7c956c81b6 100644 --- a/docs/functional-areas/crtp/index.md +++ b/docs/functional-areas/crtp/index.md @@ -3,69 +3,137 @@ title: CRTP - Communication with the Crazyflie page_id: crtp_index --- -For communicating with the Crazyflie we have implemented our own -high-level protocol named CRTP (Crazy RealTime Protocol). It\'s a fairly -simple protocol using a number of target ports where data can be sent -and received in either direction, but in most cases the communication is -driven from the host. The protocol can be implemented on a number of -mediums, currently we have USB and Crazyradio support. +The packet protocol used to communicate with the Crazyflie is called CRTP. It +provides data to the packet that allows to route it to the different Crazyflie +subsystems. In the context of this documentation, the term `CRTP` is used both +to describe the packet protocol and its format as well as the collection of +data packets that are used to communicate with the Crazyflie. -CRTP -==== +Unless otherwise noted, this protocol documentation covers `Crazyflie 2.x`, the +`Crazyflie Bolt` and the `Roadrunner`. -Physical carriers ------------------ +## Protocol version and stability guarantee -Currently CRTP is supported over Crazyradio and USB. +In order to allow for improvement and breaking change the protocol is versioned. +The version is available using the [getProtocolVersion](crtp_platform#get-protocol-version) packet. - | Carrier | Supports| - | -----------------| -------------------| - | Crazyradio (PA) | Crazyflie 1.0/2.X| - | USB | Crazyflie 2.X| +The version is currently 5. -Header ------- +When removing functionality from the protocol, packet will be depreciated for at least one version before being removed. +Depreciated functionality prints a Warning in the [console](crtp_console). This rule allows for the Crazyflie firmware +to evolve and remove old functionalities when needed and for a client or lib to have some compatibility guarantee. +For example if a client support the protocol version 5, it is guarantee to work with version 6 but should likely +not accept to connect a Crazyflie on version 7 of the protocol. -Each packet has a 1 byte header and can carry up to 31 bytes data -payload. The header field has the following layout: +## Communication stack - 7 6 5 4 3 2 1 0 - +----+----+----+----+----+----+----+----+ - | Port | Link | Chan. | - +----+----+----+----+----+----+----+----+ +The Crayzyflie communication is implemented as a stack of independent layers: -Where: + +-------------------+ + + Subsystems + <- Log/Param/Commander/... + +-------------------+ + + CRTP + <- (port, channel, payload) + +-------------------+ + + Link + <- Radio link or USB link + +-------------------+ + + Physical medium + <- radio or USB + +-------------------+ -- **Port**: Used to identify the functionality or task that is - associated with this message -- **Link**: Reserved for future use -- **Channel**: Used to identify the sub-task/functionality + - The **physical layer** is responsible for transmitting packet to and from the + Crazyflie. Currently USB and Radio are actively supported. + - The **link** is responsible of implementing safe and ordered packet channels + to and from the Crazyflie. The link abstract the physical medium and + implement one transmit and one receive packet channels to and from the + Crazyflie. + - **CRTP** implements *port* and *channel* information used to route the packet + to various subsystem. *port* are very similar to UDP port, they are assigned + to subsystems. *channel* are intended to be used in subsystems to route + packets to different functionalities. + - **Subsystems** implements the Crazyflie functionalities that can be + controlled over CRTP. There is one *port* assigned to each *subsystem*. -Port allocation ---------------- -Current port allocation: +## Link implementations + +There is currently two actively supported link implementation. They are +documented on their own thread: + - The radio link implements CRTP link over nRF24 compatible radios + - The USB link implements CRTP link over USB to the Crazylfie 2.x USB port + +### Packet ordering and real-time support + +CTRP stands for `Crazy RealTime Protocol`. It was designed to allow packet +prioritization to help real-time control of the Crazyflie. + +The Link guarantees strict packet ordering **within a port**. Packets for +different ports can be re-organized and sent out of order. + +If implemented, the packet prioritization works by assigning higher priority to +lower port number. + +This allows use-case like uploading a trajectory while still controlling the +Crazyflie in real-time to work seamlessly as long as the trajectory upload +uses a higher port number as the real-time setpoints. + +## CRTP packer metadata + +Each CRTP packets carries one *port* number, a *channel* number as well as a +Payload: + - The `port` range between 0 and 15 (4 bits) + - The `channel` ranges between 0 and 3 (2 bits) + - The payload is a data buffer of up to 31 bytes + +The couple `port`:`channel` can be written separated by a color in this documentation. + +## Null packet and special packets + +The port:channel 15:3 is a special case and is called a *null packet*. This +packet is ignored by the Crazyflie and should be ignored by any CRTP lib on the +ground. + +The radio link requires on packet to be sent in order to receive a packet, in this context the null packet is used to poll downlink data when there is no data to +be sent. + +The NULL packet has been extensively used by links to implement side-channel +packets that are communicated outside the CRTP data flows. For example this is +used to implement bootloader packets that should be interpreted by the +Crazyflie's nRF51 radio chip without being passed to the STM32 application +processor. + +## Port allocation + +CRTP ports are allocated to subsytem in the Crazyflie. The intention is to +provide a bidirectional communication between one subsystem in the Crazyflie and +its handling on the ground. | **Port** | **Target** | **Used for**| | ---------| ---------------------------------------------| ----------------------------------------------------------------| | 0 | [Console](crtp_console.md) | Read console text that is printed to the console on the Crazyflie using consoleprintf| | 2 | [Parameters](crtp_parameters.md) | Get/set parameters from the Crazyflie. Parameters are defined using a [macro in the Crazyflie source-code](/docs/userguides/logparam.md)| -| 3 | [Commander](crtp_commander.md) | Sending control set-points for the roll/pitch/yaw/thrust regulators| -| 4 | [Memory access](crtp_mem.md) | Accessing non-volatile memories like 1-wire and I2C | +| 3 | [Commander](crtp_commander.md) | Sending low level (instantaneous) control set-points for the roll/pitch/yaw/thrust regulators| +| 4 | [Memory access](crtp_mem.md) | Memory access for physical memories and register-mapped functionalities | | 5 | [Data logging](crtp_log.md) | Set up log blocks with variables that will be sent back to the Crazyflie at a specified period. Log variables are defined using a [macro in the Crazyflie source-code](/docs/userguides/logparam.md) | 6 | [Localization](crtp_localization.md) | Packets related to localization| -| 7 | [Generic Setpoint](crtp_generic_setpoint.md) | Allows to send setpoint and control modes| -| 13 | Platform | Used for misc platform control, like debugging and power off| +| 7 | [Generic Setpoint](crtp_generic_setpoint.md) | Generic instantaneous setpoints (ie. position control and more) | +| 13 | [Platform](crtp_platform.md) | Used for misc platform control, like debugging and power off | | 14 | Client-side debugging | Debugging the UI and exists only in the Crazyflie Python API and not in the Crazyflie itself.| -| 15 | Link layer | Used to control and query the communication link| +| 15 | [Link layer](crtp_link.md) | Low level link-related service. For example *echo* to ping the Crazyflie | Connection procedure -------------------- -CRTP is designed to be state-less, so there\'s no handshaking procedure -that is needed. Any command can be sent at any time, but for some -logging/param/mem commands the TOC (table of contents) needs to be -downloaded in order for the host to be able to send the correct -information. The implementation of the Pyton API will download the -param/log/mem TOC at connect in order to be able to use all the -functionality. +Generaly speaking CRTP is connection-less and most of the subsystem in the +Crazyflie will strive to be stateless. This is not true for all the subsystem or +links though: + - The USB link needs to be enabled in the Crazyflie using a USB control packet + - The Radio link maintains two packet counters to ensure that there are no packet + loss and strict packet ordering. This is called Safelink and needs to be + enabled early when connecting a Crazyflie + - The Log subsystem maintain a state of all the currently enabled log blocks + and will continue sending data even if the link is lost. Therefore, the log + subsystem implement a `reset` commands to reset the internal state. + +When implementing a Crazyflie client the connection procedure will usually look +like: + - Initialize the link + - Initialize all the supported subsystem (this will be a no-op for most of them) diff --git a/docs/functional-areas/lighthouse/index.md b/docs/functional-areas/lighthouse/index.md index 4851724a23..31710b0f63 100644 --- a/docs/functional-areas/lighthouse/index.md +++ b/docs/functional-areas/lighthouse/index.md @@ -13,6 +13,9 @@ The basics: * [Limitations](/docs/functional-areas/lighthouse/limitations.md) * [Positioning methods](/docs/functional-areas/lighthouse/positioning_methods.md) +Experimental: + * [Using 2+ base stations and improved geometry estimation](/docs/functional-areas/lighthouse/multi_base_stations.md) + Development related pages: * [Terminology and definitions](/docs/functional-areas/lighthouse/terminology_definitions.md) * [Kalman estimator measurement model](/docs/functional-areas/lighthouse/kalman_measurement_model.md) diff --git a/docs/functional-areas/lighthouse/multi_base_stations.md b/docs/functional-areas/lighthouse/multi_base_stations.md new file mode 100644 index 0000000000..897b140e38 --- /dev/null +++ b/docs/functional-areas/lighthouse/multi_base_stations.md @@ -0,0 +1,52 @@ +--- +title: Lighthouse more than 2 base stations +page_id: lh_multi_base_stations +--- + +WARNING: This is experimental functionality and may change over time, it is possibly instable or unreliable. + +## Using 2+ base stations + +2 base stations cover around 4x4 meters of flying space and if it was possible to add more base stations larger flying +spaces could be used. The lighthouse positioning system currently supports only 2 base stations, but most of the code +is (more or less) in reality working with up to 16. This guide will outline the necessary steps to set up a larger +system. Please first consult the ["Getting started with the Lighthouse system"](https://www.bitcraze.io/documentation/tutorials/getting-started-with-lighthouse/) +guide on our web to get a good understanding of basic use. + +1. Configure the base stations to use channels 1, 2, 3 and so on. Use the "Set BS channel" button in the Lighthouse tab +in the python client for this task +2. Place your base stations. They must overlap, but avoid too many in one spot, there should not be more than 4 visible +at the same time. +3. Re-flash the Crazyflie with support for more base stations. Start by editing the +`src/utils/interface/lighthouse/pulse_processor.h` file in this project and change the PULSE_PROCESSOR_N_BASE_STATIONS +to the desired number of base stations. Note: more base stations use more RAM. Build the code and flash it to the +Crazyflie, see the documentation in this repo for instructions on building and flashing. +4. Acquire calibration data from all the base stations in the system. The Crazyflie gets the calibration data from the +base stations through the light sweeps and this process takes from 20 seconds and up. If more than 2 base stations are +visible at the same time, the process may take longer and it might be a good idea to hold the Crazyflie closer to one +base station at a time. Use the Lighthouse tab in the crazyflie client and the base station status matrix to see for which +base stations the Crazyflie has received calibration data. +As the calibration data is stored in the Crazyflie, it is only necessary to do this for new base stations or when the +channel is changed. +5. Estimate system geometry. The geometry estimation functionality in the client only works for 1 or 2 base stations +and it can not be used for larger systems. Instead you should run the `examples/lighthouse/multi_bs_geometry_estimation.py` +script in the [`crazyflie-lib-python` repository](https://github.com/bitcraze/crazyflie-lib-python). The script +will display instructions and the end result will be an estimated system geometry that is written to the Crazyflie. +6. The system is now ready to use! You can connect to the Crazyflie and open the Lighthouse tab to see a 3D rendering +of base stations and the Crazyflie position. Note: The client only shows the full status of the 2 first base stations. +7. Give us feedback! We still need more testing, especially for the initial position estimation. So please +let us know if you've tested it and how it worked. + +Enjoy! + +## System geometry estimation + +The system geometry estimation process described in step 5 above, can be used for 2 or more base stations. It is +better than the standard method currently used in the client and it provides a solution that hopefully is better +aligned with the physical world. Errors should be smaller and more equally distributed in the flying space which +leads to less "jerking" when base stations are obscured or when there is interference. The estimated position should +also have a better linearity, that is if you instruct the Crazyflie to follow a straight line, it should actually do +that and not a curved line. + +It is possible to visualize the estimated geometry in the `multi_bs_geometry_estimation.py` script by setting the +`visualize_positions` variable to True (requires pyplot). diff --git a/docs/functional-areas/lighthouse/positioning_methods.md b/docs/functional-areas/lighthouse/positioning_methods.md index bc9071eb46..81281a6838 100644 --- a/docs/functional-areas/lighthouse/positioning_methods.md +++ b/docs/functional-areas/lighthouse/positioning_methods.md @@ -33,5 +33,5 @@ One base station is enough to estimate the position using this method, but more ## Ground truth -To use the lighthouse positioning as a ground truth measurement for your research, you should put ```CFLAGS += -DLIGHTHOUSE_AS_GROUNDTRUTH``` in your config.mk. +To use the lighthouse positioning as a ground truth measurement for your research, you should enable the 'lighthouse positioning system as groundtruth' in the [kbuild Expansion deck configuration](/docs/development/kbuild.md). This will default the position estimator for lighthouse to be crossing beam (which you should not change), and you will be able to get the X, Y, Z position from the logs ```lighthouse.x/.y/.z``` diff --git a/docs/functional-areas/memory-subsystem/MEM_TYPE_DECK_MEM.md b/docs/functional-areas/memory-subsystem/MEM_TYPE_DECK_MEM.md index dc0e0c2396..80feae6121 100644 --- a/docs/functional-areas/memory-subsystem/MEM_TYPE_DECK_MEM.md +++ b/docs/functional-areas/memory-subsystem/MEM_TYPE_DECK_MEM.md @@ -16,36 +16,47 @@ Some decks may take a while to boot, the `Is started` bit indicates if the deck Data in the `Deck memory info` record should only be used if both the `Is Valid` and the `Is started` bits are set. -The firmware that is required by a deck is uniquely identified through the tuple `(required hash, required length, name)`. - A deck driver may implement read and/or write operations to data on a deck, a camera deck could for instance proved image data through a memory read operation. The deck driver can freely choose the addresses where data is mapped. From a client point of view the address will be relative to the base address of the deck, -the base address is aquired by a client from the information section. +the base address is acquired by a client from the information section. -If a deck has firmware upgrade capabilities, the write address for firmware upgrades must allways be at 0 (relative +If a deck has firmware upgrade capabilities, the write address for firmware upgrades must always be at 0 (relative to the base address). A deck is ready to receive FW if the `Bootloader active` flag is set. +The firmware that is required by a deck is uniquely identified through the tuple `(required hash, required length, name)`. + +Some decks have two hardware devices that requires two firmwares and memory mappings, like the AI deck with the ESP and +the GAP8 modules. For this purpose the system supports two mappings per deck. + ## Memory layout -| Address | Type | Description | -|-------------------|--------------|----------------------------------------------------------| -| 0x0000 | Info section | Information on installed decks and the mapping to memory | -| deck 1 base addr | raw memory | Mapped to the memory on deck 1 | -| deck 2 base addr | raw memory | Mapped to the memory on deck 2 | -| deck 3 base addr | raw memory | Mapped to the memory on deck 3 | -| deck 4 base addr | raw memory | Mapped to the memory on deck 4 | +| Address | Type | Description | +|------------------------------------|--------------|----------------------------------------------------------| +| 0x0000 | Info section | Information on installed decks and the mapping to memory | +| deck 1, main mem base address | raw memory | Mapped to address 0 in the main memory on deck 1 | +| deck 1, secondary mem base address | raw memory | Mapped to address 0 in the secondary memory on deck 1 | +| deck 2, main mem base address | raw memory | Mapped to address 0 in the main memory on deck 2 | +| deck 2, secondary mem base address | raw memory | Mapped to address 0 in the secondary memory on deck 2 | +| deck 3, main mem base address | raw memory | Mapped to address 0 in the main memory on deck 3 | +| deck 3, secondary mem base address | raw memory | Mapped to address 0 in the secondary memory on deck 3 | +| deck 4, main mem base address | raw memory | Mapped to address 0 in the main memory on deck 4 | +| deck 4, secondary mem base address | raw memory | Mapped to address 0 in the secondary memory on deck 4 | ### Info section memory layout -| Address | Type | Description | -|---------|------------------|------------------------| -| 0x0000 | uint8 | Version, currently 1 | -| 0x0001 | Deck memory info | Information for deck 1 | -| 0x0021 | Deck memory info | Information for deck 2 | -| 0x0041 | Deck memory info | Information for deck 3 | -| 0x0061 | Deck memory info | Information for deck 4 | +| Address | Type | Description | +|---------|------------------|------------------------------------------| +| 0x0000 | uint8 | Version, currently 2 | +| 0x0001 | Deck memory info | Information for deck 1, main memory | +| 0x0021 | Deck memory info | Information for deck 1, secondary memory | +| 0x0041 | Deck memory info | Information for deck 2, main memory | +| 0x0061 | Deck memory info | Information for deck 2, secondary memory | +| 0x0081 | Deck memory info | Information for deck 3, main memory | +| 0x00A1 | Deck memory info | Information for deck 3, secondary memory | +| 0x00C1 | Deck memory info | Information for deck 4, main memory | +| 0x00E1 | Deck memory info | Information for deck 4, secondary memory | ### Deck memory info memory layout @@ -73,3 +84,85 @@ Addresses relative to the deck memory info base address | 5 | Upgrade required | the firmware is up to date | the firmware needs to be upgraded | | 6 | Bootloader active | the deck is running FW | the deck is in bootloader mode, ready to receive FW | | 7 | Reserved | | | + + +## Using deck memory in a deck driver + +To use the deck memory functionality, define a `DeckMemDef_t` struct in your deck driver. + +``` +static const DeckMemDef_t myDeckMemoryDef = { + .write = write_mem, + .read = read_mem, + .properties = propertiesQuery, + .supportsUpgrade = true, + + .requiredSize = ..., + .requiredHash = ..., +}; +``` + +and add it to the deck driver struct + +``` +static const DeckDriver my_dek = { + // ... + .memoryDef = &myDeckMemoryDef, + // ... +}; +``` + +Implement the `propertiesQuery()` function to let the system know what properties and capabilities the memory mapping +for your deck supports. + +Also implement the `write_mem` and `read_mem` functions as needed, depending on the capabilities of the deck. + +### Secondary memory mapping + +Some decks have the need to support two hardware devices with two firmwares and memory mappings, like the AI deck with +the ESP and the GAP8 modules. To support this, it is possible to add a second `DeckMemDef_t` definition: + +static const DeckMemDef_t memoryDefEsp = { + .write = write_esp, + .read = read_esp, + .properties = properties_esp, + .supportsUpgrade = true, + + .requiredSize = ..., + .requiredHash = ..., + + .id = "esp" +}; + +static const DeckMemDef_t memoryDefGap8 = { + .write = write_gap8, + .read = read_gap8, + .properties = properties_gap8, + .supportsUpgrade = true, + + .requiredSize = ..., + .requiredHash = ..., + + .id = "gap8" +}; + +And then you can add them like: + +static const DeckDriver aideck_deck = { + .vid = 0xBC, + .pid = 0x12, + .name = "bcAI", + + .usedGpio = DECK_USING_IO_4, + .usedPeriph = DECK_USING_UART1, + + .init = aideckInit, + .test = aideckTest, + + .memoryDef = memoryDefEsp, + .memoryDefSecondary = memoryDefGap8, +}; + +And the names visible to the lib would be: + "bcAI:esp" and + "bcAI:gap8" diff --git a/docs/functional-areas/sensor-to-control/commanders_setpoints.md b/docs/functional-areas/sensor-to-control/commanders_setpoints.md index 81ad16e257..781e5172ca 100644 --- a/docs/functional-areas/sensor-to-control/commanders_setpoints.md +++ b/docs/functional-areas/sensor-to-control/commanders_setpoints.md @@ -19,7 +19,7 @@ The commander module handles the incoming setpoints from several sources (src/mo It is important to realize that the commander module also checks how long ago a setpoint has been received. If it has been a little while (defined by threshold `COMMANDER_WDT_TIMEOUT_STABILIZE` in commander.c), it will set the attitude angles to 0 on order to keep the Crazyflie stabilized. If this takes longer than `COMMANDER_WDT_TIMEOUT_SHUTDOWN`, a null set-point will be given which will result in the Crazyflie shutting down its motors and fall from the sky. This won’t happen if you are using the high level commander. -## Set-point Structure +## Setpoint Structure In order to understand the commander module, you must be able to comprehend the set-point structure. The specific implementation can be found in src/modules/interface/stabilizer_types.h as setpoint_t in the Crazyflie firmware. diff --git a/docs/functional-areas/sensor-to-control/configure_estimator_controller.md b/docs/functional-areas/sensor-to-control/configure_estimator_controller.md index a302eb20b2..f773e8da5f 100644 --- a/docs/functional-areas/sensor-to-control/configure_estimator_controller.md +++ b/docs/functional-areas/sensor-to-control/configure_estimator_controller.md @@ -23,7 +23,7 @@ Some decks require the Kalman estimator and it is automatically activated when o ### Setting default estimator at compile time -It is possible to force the use of a specific estimator at compile time by setting `ESTIMATOR`, see [Configure the build](/docs/building-and-flashing/build.md/#configmk). +It is possible to force the use of a specific estimator at compile time by setting `ESTIMATOR`, see [Configure the build in kbuild](/docs/development/kbuild.md). Example: @@ -45,7 +45,7 @@ The PID controller is the default controller. ### Setting at compile time -It is possible to force the use of a specific controller at compile time by setting `CONTROLLER`, see [Configure the build](/docs/building-and-flashing/build.md/#configmk). +It is possible to force the use of a specific controller at compile time by setting `CONTROLLER`, see [Configure the build in kbuild](/docs/development/kbuild.md). Example: diff --git a/docs/functional-areas/sensor-to-control/controllers.md b/docs/functional-areas/sensor-to-control/controllers.md index 34a5609b20..14a8c8cc4f 100644 --- a/docs/functional-areas/sensor-to-control/controllers.md +++ b/docs/functional-areas/sensor-to-control/controllers.md @@ -54,5 +54,5 @@ Check the implementation details in `attitude_pid_controller.c` in `attitudeCont The most outer-loop of the cascaded PID controller is the position and velocity controller. It receives position or velocity input from a commander which are handled, since it is possible to set in the variable `setpoint_t` which stabilization mode to use `stab_mode_t` (either position: `modeAbs` or `modeVelocity`). These can be found in `stabilizer_types.h`. The control loop runs at 100 Hz. -Check the implementation details in Check the implementation details in `position_controller_pid.c` in `positionController()` and `velocityController()`. +Check the implementation details in `position_controller_pid.c` in `positionController()` and `velocityController()`. diff --git a/docs/images/kbuild1.png b/docs/images/kbuild1.png new file mode 100644 index 0000000000000000000000000000000000000000..f4552b25d85233a4be797ad579366a1f548bd620 GIT binary patch literal 69727 zcmdqJRa{kT+cmxrV`G4VQc|LnbVx~P2AA}!rr(ozD_Dc#-qzb1P> z@9){~|KvOQPCgE{Y+$W5=N;o3*BIlTpI(a#-ndG56@^0G5PJ1O3Wd7jhC*HB{OdCO zWWJ~NIs9|c@|lp#U-0GhmzEd&KY`UtIV)*X9V=UP3vHCHiK($Ry``pwwzi3-o~hLu zdNm*X5d-o^&n>jot@KSz$Yk`5wNU~V`eZCDWD=TIWGqZ9%w$Yl+>h9}nOMl4ijh5k zDJ^q1sv#YPB0~wicq(HTy*z5G6f$$xw9(JydCzN7Mep0!kBY6nX)`|~^k>>4(%XJl z8AP(?$8%ZEWGLw;&o+;s(`U+zc%|PSA!7*j+&Xe6rIKt!!y?~y;UnP*wp5*~;_K(E zvF4krGC@~+`r=<-EIgVHSIEf!^|c#+F~{q_zg|)P-}}j*FJow4Cn1cgv5eKnf!Gwn ziSza;5^*}~6SYLV4s5>UIJf@m>tJoZKbth>v2CNCkE^nYl@%MSPTNHJT2;Pd%;g%5 zz5L{N?=M}uV0N&nT^=IV!o?b>>T=)_NFkEESc-;*#-`t0-*GD`rj7gKf8X(@)g#lf z=r3Px?yd}bHfY4%UTg`XGPkf0ULQC)+M|)pidJ5uZh=2G zcFFSgzJ`g3IZ|%xA0Cd6%F}88d3th`D@P}jsSvWhzWyRE?)4xQudaaQY#YJ#%jikM zA(OT4SJ-)*YHQ!QxuNKAuZ@k3VT5jMZ6!Q;ci~g4=I-*KXf!AD8|D-N1>Zjb_XSIA z*GL{2{LX1TIG;Hd+Mt#fow2-Wip31byA2f@!axPX6G0m{y0*g4(97N4Gt2xd3pWy4To$}s=g@u;xB#|`v z{8`N=LN>i8p>*<`_L~oMJEHBsIdsJG-d#KT8A981bedg#`a_LLu6l2C)8X48NhswW z%A`9H0~Q2|&1it>W#`FqekZ5Z{7o2$JU-l8w`v!9Qn;KC*#ZIrg!uU5^CTQPd?-!w z1|pB0OP1?!RwN(Wu4?rpix~|Sv>fVmyhFc|;eYA!Wxl7sX~H6!UL!X#Eg$yC@a@|y zrIxQx-PboaCkJvhOG-<-^0YB->^ zjcOIorK`7VdVB8#wR~vNA~)>IxcyMZ3*Jns+Qs=Z9%HN)X6suL5)!AqQQPUxIR3FI z-Okt~i8#Lfqa*Z*l@(uicjvXlySTWxYqa!(CnPnUyn962KZDh4-(M;)8I3&LS<+;Q zh^1yVWskBy+38!lersBJS7hE;pUV6(FnK#FUnmUGFk0?4O-hIq4R z|NO%5dTLkBmK5yic||0Qp`otse(eE85tqdW{?mPQH&|b003+XJ(lmJa9Jcj3qPg6&vrQ}Viehmz@J{Xv z_^8cw#1Qe=eKD;%`1r%)nxa34-mmkQZO=;rUNzZ{JHuylEq9yh)2!a`Ora=s;jx(h z3qX0SSE#)ke#ojiH{@zIQA;KMb%T%%B~^UcX#loVru7R!K}Eb{tn4NunxOD9 z>!rZ^f|%^aL-jMQVebYWt?-{6KZiX%vz&|^^&ImZFO=x_B#)F?dq4NYlwUg; zPL)is(-!a}Lcqq%%uEeNuB5EYVr`5UxmCFScLN^V+Y3E78TVJEd#b9c`kEGhr#*l5 zs#!C_Rk0I;$@$QV+j_A+{<-Hsk?~!|%57|@G__hRl8LWwRh=FhZqBqe!B!fCC8n;f z4xdcx!Mkmx-5DEGeR}B4F=jWF)*B(i->5&BN3m9Q!~lP^F*c3twL-)G+2z6f>DDlR zEdJv!uvw>;m)qc{)be=`07FGs)Q=9AvXQZdh4>tn6;hk6m-;Sg6#HhWR`cO8s!aE* zx;8+iTOBR$DK;HvTf#bD2yKl$Jvgx6{Yd%6ky~lvFde&oe55~1rHsz^U8&RpB{b&P zAIf4L>$qBav6bYp&2=|qITz1;f14S(W7qZVC393eTp3nE*DEtKCa6>OfSODF+307K zZYiP>Ke`eG2XoZXiMg$6^p0)0R;Q=y!^ZWD)1!`Nf?JcX{gB6f!2snk3$yOOH>u8VV zEmvM@fjRcZV@!r+#Gd&k?t^l%8^hZSd_F$D4Eg-a`Gak6|8T)jv%WBEHmc>yQNR9* zG9E6PIX>88GalN`UCAWmFcEDDq>wK%lKF7$P)vHQp-y@`5fq9ENcV9LBXo_1YEb6tj9nJ zhcUE2UaNQxOoo8lD%JWV7`LLLg41D}8g;NWSG>xg-8rghIrFmt@I?Y9=+mc9>Hr}& zk8PhVN22_OPX}QpXT+pK9UFA{>55TJ+JQcfl zfab4L#EIRgejYp8+oQ>46FG;Mmp_KrkTWse1pK#I9chVR)eYyeq=eU4eqVA@a5>%( zgq8mj&|-ffB`yPpgZ@FkzxnD|<&oSbp2phJ-LQK9$ERjeXX}6JV73BSbvs^YGDWom z7NVQ2Pdr5}@ol1j6wa;`k0)z1e1>v%&As+3hy3}lNRh2B_5p9EwBs>R%z}kHq>FOCQ_4)kN`NgCMl|@uq|PdZ}eoojBK@M3<}+|+4zQ1`=!4|>LbGx0MTbNi%Z zv8e89SJ$OIsC#X(yip9wWr>PLxZVvWbCDdTB*5}6)abG0X*67&7~dUM1V%0r@=)9O zMYMn~q51r;dm|$ws7Yhn5JFgp3Bn-{VUNZDI^HBAG7{TO5*T^g@z`d0BEOT*+jbck z=!=&xy8yLSTn@As`?Dp2D8<6r^s$RgMm6?UM96}+Cu*ad)^8(Om&Hsvz=CYiPAmu+tINf$wU^#Mh!3@fg&pfWmXYcz; zm)jVV+0>e3Kl+-V&k7a(o888w(^fl2pCV>bf#|dM@87!(NQCp#nJ7DMOui(u@t(I? z8It#(YYuQ{tlIx^_(vAVP$;9SxQxtgSgs;wV)@3y?7%x=p- z^!>nB6^e{nyl&l>D>8}@TRAVmw^~wRjB(eFVDCx-m1Pt8!be9&R^xCtequr`PpjpE zT7R?xlvNT(I(kzytzt(MEKxV5mYHFSUu!KB(L;m2w2iThO)!)8-W5U0VfBkZi z$>`5t@y|2kCU-YBgjhPZLn%aXVH4NEI?f#CZyH_ZrZ-`VQQ}W7hUxqR^(RZYeE(!` z90LQRR*i}0?W)jtG&VBgEeOUo`SYK80mcD0r2;nGrwOz%nnvYMq^)Koz<~HZ}yG!ld_$ zjEI<6zRHQ~X89wV2o~*BnY6%1wMX+&NRe!W4rR^RX}bAVD1)-W$q-;S3Xefa4XB9! zO2&h~c{_ozjx54X52@d567KA>}(F^RczEM|oJGUT7f zKl3oNv`lb4JKf*kXEGh*f!Pt3lf#E~a-p03u_;o!WwMkanwwuOY3d)m7s?{-83u?# zuZc3P!gw;6r)_R!4%bp?>IR-4;C8Y3Hp;H zsnx!>b9BTZB>b79UI&O}Bahw@&g>3d?bbvsI;_Ya(7B@hb?utw@<1+on2;!3P`+-b z&P2_-428naP$ebvb;QW;--l_rdebp(9HtzHh^P(Fh*~y_sFVMU2dH&lX88qlbOPv8 zp!2wR>CzwI0}}ivFZA@NL2PKOt8<6TQqPqO=eB-$^GKks4u)DB3hl<;p6+yWfCQ)T z-zY^OSn05{zuhAN9X`@Q{|L0sUB$-@(BF+L_9wkz!L-1n8mv)*r9p1^g&d z5wEn{&@k-I0ahhMN$Yxgpz9-WA2|LLRJKo{`8(0j$7@g4e}cXm){Pi!+!EI_zRT`t zz>Cz>$e_d`)b)d>Co6#T?%KGi;uYSP=CaQobJox9k&2&EQf6iF?y^dJhnb9XJ#FzJ zV6|JHK+9Ato(19v%!ooI0){Xem{kw#Jl)f)rswdsjob%7a>q!iCC1gOlMQ&P4=5== z-l2W_D^Umsh;ycPoYf7WTW`p0M!PHQEthtiZ`@@Um5fM4dN;`g0rQ>3H&8V`R#)=_ zL{GwcMv*BN8G)2>TS7u2@{wVq`xPvpY?py1QE+n)ImE*CMTkYQqfN}uHzUQ^&W;mg z5i*Hbp5@WE?m004)t>A{c~TcFjV;b zE)~|-=10qI`FHS0E4GOAE&S*+nxz z%i!a``^W#hS&N$}B@XBZKCvdW#PUAI%*x98gzL4m4{Y)a zluRFiDQOHk*nyz7K~Lk+ppm;OOaHGMz-Xl|t@Odx6_{RJ{QT!&F0S~&?=*AbIDS_b z&?T9fnI#@^CUkkh3WdrL!F`SKzhAwX$RcdQm{1=f2@|0vN|!(qW?d{tCs8?dkDb%I z;XM#oMAE5r`Oi=w{|5MwCrt0M2uBlybm={)UCwUMs5N5e$00DKqDo0A+bcfr5dQhk zD}g{s3r7Nq4j+cD8-?_^*DkyN(QgbSbFckE^H}A-7h}E3Ki3k#^}w1bpIza9&ice)y`=iv9BTMRh+mYPa!GE!0ts}ey?KzXg!P+vp?nw}0G`y=#lkOGYe z8yi$G()Gio%w`az0=1t%e=c9?a058?0}%IiQ_t^hIwFu!l@A3F8`=@z^~zxW5#9Ol zpIri`d`S(cZN=u#R-~N)j+NtlgsIaOE(A2t#KdHQC@JxyGIq^c)o>V0{g4bh=%`jp zeRtezYx1;PS0pQu5pHI!>+KBzoYBaY)9X$o0OVUBmU_R~m)V*U=kf!{e+0kl)9)QX z3Lg=&rAll}Hqa<+bD4}?{{!`Gd8p9a_pYmu*|ns;Ox3x};xTf=x3WN+aa3J-r_!2E z3Y?Bs$a#3ZyHmT;VQ6`J%WxJ`KZ2bjuxD*wlISd=qB6mp?TER?UtM|e^5yOn z2~)nKj{>8WMpnGCu5r3i8RY2r9FJUfii<-)~tX2`~lv=Qu(}Povjm* znOS0C*rC%sn2j3T2&}kt4L9+*=Wc<~;Cq$I!<)BnZ;L8nI34esNBs<;k*4&$--5p! zh;mse9BAlvT&wH^MifM+2-Ujk)Pb8`vFIOQV~GJ;(-FrnAu4(u_T&Pwcw}=Z{asLe zzN*(;2ogN=BM~I?;8f2+d3WK`U;Eh+TsB9yInrn-|ajU7RvGZ@z zo+9liisK`OeSc3jO9}t_M^SrCZDffbtg%G3TdkdnJ9q{pf4qJ`3eEW3R%f*qNY5Lm z$sG9CVOo5!_=)O)z~l4SdF{wSfkE5fY{ekrTR!_eoT9?wlcK{2`EPpn)9F?AfF}e^9Ca&uyDlk796eaO|#*HUR<1^uKaB7FzBu zA0n;`s^bb)tHX*I@E&klci3l=z->UYcn%B_`Ku_?S?oJ^F4c%I_9jD%W9NW7 z1O13n$vVe(rHz1!;ibb}8n3F1-4ehkm zePA>|-4T(JS^dWggc6Q9EYTw^6=>f3%lR7>+fvKVbQ*oK(eOFfp99-k7%j(x^@l82 zPpl(W>qWMu-rA!2n84FYi z9>Wbd0wou_K*GI2^FnPUqXjAybeo{8K!^S2uNybC2lHk!l#lY;oex^5G!cpu!K_IM zVp3gQ!cA{HR|yyhU?;Pno%$D!YO!?1KX+W>0-vh&{UvXhq$l4SldM7S=QOL)G(MV* zDn*1MAl==?Yvub#q-bgrHE0DUC)|K{g(@pY>qMf>3E6+>Kx*WYzzH0HE%GCZ&)I=h zzgz2V<{?cj-+a%UGBhY{9Wmv9VPU0+$Ao!(`t&Cy?rpx~9tq$6gtL>A6XH6lIzReU z=Xnyrv33`Jg1icp%J)G*_n|>-Y-#!AM?5|YdNWu&x%xcGEez&!pw_C# z{foyOg5sa^Yjl!SKTvhu-5=Fj;<2^1PI5hS`Tc!HMARo@XNhBdHp+Cp4vQ9Kk=V-Q zPY6&aKB;uDMw2VE(tqCsy2A0<8rpAXO6;1>&5+6>ZrfENq;+ixY_OXDb<6t_F5_t| zK+HPu;o(At1m(`%Gtg8?Oc_@NtMSfvF(U!f1iHhnljr=r9y9#LA&~QM@lmd+dMq{MRbrfvHsqQ4Qx7jMsq3t8! zFrkBsI$p24y4de}2?`YOt%Ev53wdtj$`l z=MA4jygq+MJUk&kF$?AaZiXU1Q&<2XM3O-}MFrJEYklJE&yr&6Y*jRyJ`2c>$%g$5 zh~EQ#DAH@}Z^W6eja4=ck=S;qR61y|a81k=!`{FH;WKX09OF7q9ZUfTw_NFOPd)(0 z(a4=xTk8b71dSoy-RY5<#IpAYZ~v*iKvp<+`{Z*TARzvZH%)|kgOF^;osgBSgqcw@VF%oIMgKBC;) zp}Pyf!=O^}0ptjgP&%EKz=7#CU>5{z^R70Tv@Nu5(q+7sBR?Lm{v7ATV{FvPy!*)E z(O5{0`OLXf%#!YOunCsxHKgM1t*(LFkB;;*zvA7Y_pRJ{_|grEXm^#f;J1W?h2LrS z0ph=bx_-PnyawIu){tSgVk<7gaxHYJsW7|%xqw4VAbfAlb>=(lMB2%w-l38DpipRF z0ga}^wwG_bb}R06lH>hJ@-%!Dc%%YPYG^bxH0+i#i&5b`_TL{3_E$P`AR-cII)Es} zW7Rs~#so8}Vv>@Qx}4hcg6>i?JnRiyB;R!0)Q5!kG72w@?sukQ7*He}9GoDd5t@Z@ zhyfh#I{V{LiOZ3Se3t;*wG*lsG$9De)Ku@4u@U+LtOWy~`UA5+wWR3%)cw?R(5SJ! z-4~mpg3E6Grle#SvRtPOT1yhOoJ_|mggRp@5qN_aY;82vff@UD|6rvsjq&+&Q)cJG z{_CWCK0tQ<%*-_S^Jh&$%YMNwW5s@JHtD7JtWt$tK=JshsLb!Fz29OBC5WGRUr?i| z5Hu*z-~dh$fGl-6;dVaUP8JTq11g9HT%!??-LU_(ZMI#JS~^*@f=kbPdRAW}u3KEM zgNibq&uzfmv_FeiCqIIOx2y;B9Cnjq4^S$x-I7G`qzm*UKjS+4@j4#Q_oh<;H9*%0 zngA8Q9&}|;A2+u;mk_NS1s-Gm-)MVOMjz9{s$K_ec-7<^v=1V*i_mZR1{uTY==RSVujyFV=x8DunN&m<0Z@^f!usp48qfy_2r%j?uff_aaol70^JkCr zNRcnAu@tU0p+ZY*rx>ss0)Uq#d-k`o4pxyh3MJfR1 zoAmFnyX;1ZPu&lZmfI&;~B5c32KUD@lVD^0V``dYxza#u`%X&(j3LX2#o86fPe+8wPf*_3pJ-FSC$ET^7r?~dp@@q zu|sI*<&V0v(L1y(EP-&H>THB7W$99pBj}taq^qFqs59Y{lyAx+#C5v*M?=FMR6^JG zFHLzc7hb&hKy}EI&viPEv_4y?K4E&?S`UK_|g8XD4u|<{u|A4Fpl3)OJ-`v)=xNmB5@>!Nr>0eic zk7K`@$1#l+uW`V3Az2#;p$$uW)5-xWY`FT1Qykwz$K7Qhh={S#I5-#q)`e7&5dp}{ zZBQLEXP{hNSF3!;o|X%Gp|Irj_5G4W8z=B6FVy?dCT1{ZE0<%s)v9?Fzji5D$;vXH zD>zwwUgYBqYCfqe&-H|^X5Bbval8HM4dd}DV$!&|G?_H=-tTWHfLntIJgjt<5EzvL z##16x?+udiGHb<(i#fbJ+^Z;N=2^S(ql1-rkP;yg^$}R-#;EOh5QnJ>L@Azut3>_g z`)%5{^(LLM-3O#prKes&ypFrb`T#J(7De&Xm7eAlIzwA{@nSS3&~wn*BXc!1v(*X< zW72#XAI(Y3M}OI0qD||Mt90QQ{83wb>(xOgHA29I6lvdPOozy-B*SiL3*UHz@KJC? zAjDG(f`HyY4jKUH!Imcf`trmVN}88ZysfgkLqCn!^PLV@K-kwG-AMtXh)%KSLy_?? z{}F~7^0v1=S^xQQ2?apf&=!#o6z7}6&LVVgFV^ydzCfXz1c4iRm=0A};(9%{&0os0 ze;Y0~Yh!f)2m42B)%JU%RUo}@9IYLpA^8`WIP!-N2|0>0XiN~H@`p!trfg%w7;ASF z8af}6N@6!wK%ZEU4<>x1F{UeBSE&}prw?yW zfQC9->#m1fi=CIRm!RZHDEiVD^HMBmUmF+HQjs1tBVvOb)Ih9}(~+ zslk?m&_*t0)D+T3*^aMxXJ<2l`>6vSeqVZQ(B#CI^=FSu?GK?&zRpiJ&6{&2v{~~L zL)k8L6F?*dlxB~psJrf$ZzMYIWg#~1>gctBzD^7wz>Y-Q>VY9X_$$cBq{(eKf^lCD z;`2a$Q|GKrawu;HV2>Zv3b|a3zW%U;goGVCsce476O+7`N<>&^K0hOSd$X(&s{iVz zGGt1aHTl%H=L3FcpIzNqs>WWk0|5ef3Ibh>&G_~bG$RK%s%>$-5^g1B@_2V!~VoBbh= z>cp|7Foxkk!P1wh;5ZZsUe^ITe0gxH2@Dkbo!)`!28n3SS}>y5fzX46Nhik?2e$HN zw@9IMZ>)5RcvptJG{`XWC1$sPXf*T};{xZNP0yy>>Qgjjf&?KrlRz70HfQRygUf+k zAY5W8yLd9nR3~4*CmRvyGvt*Cm^FgM%=v}RDEm`lV`G7c;<|xlXSuW3t9699c)@h? z?@x}fw+WnT(?&Blz4_Ou)70$4-c$ zQGe-)xiSc`nA+MfQp2N#o_C;3NWj9A%TXh_f=N`<+Ujk;g)3Ja=p0OMSt*_3_rz#3 zOCoNpZ{PLAniaxkvmm*#>2E#+Y0qf5=zhy-x%t$`!Yat~Y#yy`tYt=y@HmHpXW;`z zDELq}Am~>QW%YbvOI(qPe|8Bx`qqH^xhJto{8W?)+k(PFwkt!AbULDuAnCp5AFnr# zpS_AK?jf1;hd$+Z8p_EjR(Gtz6oPV>q17uSkyzc)^&b~2`V0CD5D=+Vt15PMWmbkd z8$BtJqHwwd7+e2%ZTp+?aB=F}%!lAYKVV}EM!#~ZHd?j=@ysvaN#r>lNDURQd8MZt zmQlQOcV`@wS(UGnDp4xUi{>M4u6gJF7X}8}#Hj5NGqhkoLkmqOhjyJyus6&aPxd#a z*qtf1l@QnoXj`1}7lfFrf|9Z_(mR5jlK{1~ew4fu`lLN>AtoBteklJK1J6oTR0|rIUN5rS-u5 z${Cg$5R2}wPw&H}H^atX7^}R(>#&^&$x+}=nt5|kJoZdsWZ z>xzt|tpEmX1L?kaIra~jh8DVXc_auWI@dk{-~$XjUZd0eG96nN4)WJcfWCQ%luwmGc!B7srk5 z?L_mb598FYOk(+H0ZW0up?ifjkL6Q=Rt=6_rQk|XTcyu0QX1oNu*JBX2I2rLnL+j< zTN&2(-}1k^M}u@Gb#>^_lY*2)v~~_S$0m7iMmkd=VYGF;l{8RjSUkFf*gOHhOFIvd1Fr*uZxC&b5X^-i0^^u7xx0Rm6T!%6&{h+Zw zJa0Osh7EtGcF3xVX-ze%;&ZI_SoywuLDK)d*YJ4Fm2`6^3`Z_s$JoIW;bVa`i4Tgy zu%ShEI+&Te&8QMR)g-ZAdj;LdK}DmqvfwjoA&ViV)E)|A!?$kbCcr|0uq&teJS zKpn`7Hi52y&u&Bw7V9TlP{0k5j%y({J)$c~BuuWt?$hX6e8ept5#wyxYz9=h?U^mV z57{OiOmdEjGp1az8=vE@l7q*20D%1}7})C*@jj&dn1X^ycUV7DRbANEZ_XIBMLxYn z36d*v75k6D`(5ZtpaL7um7FdU(*8K6rl#<_5{I3KAd{XAPG|GnMP$<)bz&f^YzEs8 z&z1bZW>=N;x7l6bv?7^07(#>|Ap*x*_1RMgPfBD!^tJwRkfgt#n-2Bj5X3CC+rW-< zVp-)6IKBij-wYVBuhy7eWI#prgBl1cH01b>y#RD1A3-yN0!#smAcWRct0UU9`!OQt z+zJqFeGNqFVm7J@qUyjLCH%ul0KeY=Ohhi^ts&(2f4(Fh{^JS7{2$qluh`IxnDC+X zwU5kw(5#WGRo0qyt{@=LhkPy5{PTYD-!Rm`^|rFw?(v~`3T_m>Z`td=^G$#M<%d{QJ|8Nq_eL`^k@q<>hFuHIu)+tN)C{Ch62`h&FHg{WG!;Btdls z$M#WW-QTuzfapAk>|mHA+Tz9g zU801Ijp7jqr69$!&`@>@vRRW03kS&6R2^?x0&E`rUUDul)#;3V2?4wAf{twiLksLk zL&z$8gIs7Ii#INPPV3QqjNV?5kRHDIzF$8VO0~tNf*uhtTi~UIi@8AT=GEL&0J6S# z9AC_s`hahX);GoCEJ)6NfF8#$&gJlv>uKq$)sa$aFw!7(p7bWYe}4Q7{n>L<`M7?F zKC9Tpvk*V75(91xyI!d2ZQ>1x-XCnvL~xh_JO<9j&J+!?^h2Za{7FqvH3QD~rJ1m$ zTO=e%1LZWih4^u=7NGzDB(p9Me;frFsLX0X6rzn#2{4cl>STim6f*>`$6St#A$nK) z^(m>v(%bMIdtgJtAb&3m6+XuGrjX96gYklvNxSy_%LQWLNXO7&gI1-jt>IQ+sQOZ} zK$Jj#ock4DC*kB0kRoQO@Pg$UYywy;yUSb)#P5|0^?N>oP6LxTXLqga>sK|%4i4rw z`7>(@Fl#m6{D^td>~IWO1&9ergLur!`@`j-aPT@L)o$Lr`DH9Vu~fG-v#70*%d$!+ zzig=3b(7Zn<%^5ZDzvN&+j3ej-iBTm1v?UOoyKC?Om}U}d4VY2i>DR*+pw{F18+d1 zhjURxqvh2&ro^~JP7xCmtuQ>=)6M<_Y_J}zIMxsVfvTEiI*l;nnV+&8Aa+Uz814~b>FJth@@|&o& ziXL}xRwou0e^xnH^Wqw6SfeAY3n;2D{7G_6xk3J8(P~x$l^r<;0gk+AEKf*wRb)LD zKV8ZT4ATqfolNk1g#tayC$L^PZC809Fi_jo6#yd9=1*G2sp)Bm1v*0Zj1bIj_69={ zRghc)VKcujE5IUkW{a-##G)GXyzKrCiYvmhfr?78_1$?WGYvV(@b=jUO+P}-MNQ}e zm+pEwPgOV{6(Om8kf2$>Km?gS+-Tf-ZOoV3TIHKz|0+1bN=izQ3dMl2)~x&Ji&1oE z0(N~ODCCIm0fa}fGc3~F4zj89{cF5Sr8h343nT|Mg1|g!QW+_cRA2xDBk#f_2mmf>Su(i6II}1H!7A|DDE~UD-!I z$QAlpDQ)4}t$+eE(7B+33Pi9ZLE!2luD1t^w(!*KY;v3sd0+;d)Yt&Id7y-OcTE;J z0t9iP!)=6MpF7+utM*SYKua~~t93g`4ElE)dU}FH!rmj(0RhDZm?xN=*9ih*u;W^x zso7sGUjw<;yRZuKYu%ZOvY+p4O*i^RAt4FVs)%dH<=#>!huIU8lRrS~Bk_KeTXxTe zxDWhLi0IY9)Ke=IP=mHm4_*Sq3npe~8w>PRk*fgn+6N3qP>Ug^{tjH3+itM(K>xVp zux$tIM9^V-UL4TD%%I?` zW|eGXtO_R`jg>u}_eRN~Eo)}1HWJRsM447}eT3Bz$f_$Y9?hxGt7_1Q*mMiS#r``Z z=1VzmGgVSt&-O3?#k?6Q<@|vx`9aqV*7AUa+ko`| zl)wQ%I#4JsX!NTu^`D6roABz5Rp{qg(#4lZ_25Z7OmbFlHJn$6?3MOxTlOIIU*j%E z%AbeAy9dY)4uVIPj$jn0dnB3^ZMUEfJ%BS53F>u?eI?kAmAox$_=1QvnZ<3IR7a5rZGpy7>))i8s@ZEE$!mMp&P{2$elGdJ_ z2fB+c7@TQYBG7#VoE3oSBndHqk2j9O0k>}5xzn({APrzu5SsQjlMsheG!c>>oDd|0 zN{8X+=Le~N%Z*72fO#)ao^sXe1VEQ>wCv=)@&poe`^Q@wMguwTaOqVv=YRPRlv;-E zR3Od{D1_GuSc2grAddFB}nS|CQxP~nmsjs^Nbq3ZXXt~e2XkLBTvlxS5t`c;#eoH7V{)aq94f|- zwg^=?ZWL`a4ohzjD$y^9;v>gyYX|e^GFrw3hpz%Va*Slt|1ppo_bW|W*X3kK_Sy62 z(!7`;xF*`HaQ9_{#*hbK{_NQ^5P`11>ABCg{`o_N&V#v{g5NCZxmwyAJ|re)2@aFa zK`L_s)?W$?H0aYhweJsZQ-6trbc<+z)+mkAfIH+G(7;$wt#G^sx)7S6;1z@ZEMw;k z5R)NKynz_-Y)#moX&*V?BM-!dCJfl_MObP{q7mJ}^oo}u&;lW404HcKsmKXL!M9G6 zPQiqMN!IOD0a*@o7uk(IOpxQ$yAxXq4EtlDCuY%Uqh8{sqoxkObzcB>(0S$xq&VQVP5Zke8g9S;u7b>J%tYP|45`vl>Wypjed4Oo%KY%rrvq$X|Z` zRc=c=ZUh}vFuGyt#+68NI(EfQmsA(EsSlDuRx9t61eU;IZRUDoenHOb&Fe; zdw}Oouc_WD2MF#7qG!D0o-L9qfi>4>Ehpt%tkoh#L_}l<=S;xAolp-m|s{px6!7z48kqR9CzVT_FMdeE@O?Cfq&#@ITb za%|VC?Do{@WwUtv{4jrK$oCw|!NUFv6LS`n;aq(`ircaVhd;oq#4D=$NX9PD1+xTl zWNY!bOW1ezS&f`ea&`!AL>-=kceGKUp@LPmT`vD&B4mjm<5g zN+{CcFTR6AOGzR}uaiafp=lNd^$R2&QPzput^dgatgRcdxSn2^?UXoWcaexZ_BhjA ztD1`Xe#r0U$}*g|FqUM+^fNGiotouhFa^U1+2mUyw+r9)rAP8SabDh*O!iVFxY|r| zA?Zn4MmMhTUaWo~Wi|gZ^rV%_Vj$%+TP`Y9B#}{$YkNMAJ9xR~^3cP*vNQ||uVV4C z5kl7NrG^GMgVL)yINp2(3kz#|slRi#5)$2$P&1laTbnj#XeZ|8JlEIFP&E*&_dyEJ z4-Ymtd2ErGqWcwcb0|ohJ)sT}2Ep%2Lqh}PIp|Koh44OlJ5_bG+5kqL`QbJ}VoJ(X z9TxxhTM56ah6_Fkz{jE{LKRIu!hu^zLQ&nmb4PtJ&lC-nl$3;Wo1WGxFnF`)n+m^O zt=Pf@)#|pr{Xz53pFhG>8EVBicEWCF|E|a&EOdZ#vobO=UbFX56^Dy#is+Y6P+C6& z?I2cl-k|{gi~^r%Wn;5d+GsqWMQeVnGHPX%?wR&Iip%kNUlc;Ck2mjP?g;PQ1ll4Mcy1G+q z)f}>p_}yPDQZ0s!?*pPKIvpi(`=eFj;VR$yA0o?$B2CLs!#2G$g@9z z9uD?DoWl1mD6j(JfTNIEq(B9I(1|aI{`dK-QJ{VU{fNsH+k%7FE?T*_n9I22Kj$4AD`>P%k*J5L$+2{ zx8bv{q43%C$O;tbT}}`1kU2isyn}KJ48%q~5f;7*S=`08YngAohfVIjL!MP|aB!)) zqoc7qY5XhXsRAcMBE!a=^5H;+(F+ZYZ!6LC!Ja5c+?uLE{HLhe^E!bKY}{L_PPxr} zn1)~NkKFNPPn;>l9!D+vF+rq*k2YfMum7I^k&qEYViT&VU z>5z&clC;ocTb&>1I#gCwC52;`Z2KYCHLSaC8Hn(!(ND4JVh6;<`GZzR!V+;EJdc+S z4%|}G(zz}t0#++ScTnr_s~<48Aoii$?wXX4z#Ke#;#Qrxm8sjg1c!-SP~Dq*KT8Pj zGcHQSReXu$ixB+?1msD2uLkstD8jd6JDp(tvKAMsC0$|)D@qkq4lQo5F!`U(QB{CH=?9v?oO8A(elSaGAmxZOraeskI~ zREI(g;6=elC59#+0#ev~jqx%Mp;x*`qmpKy*5B{+gf7@q_UiTP_4oaK8sOC7J0!aW zJEPul&Dm`())5!no*ZXJZf-(@!XaG$*YdwPE`N29XG^5Jarr)Q;`b0K7P7aUZk99} ztGEPs*Lbw%>IR1kxO)h3aj(MC1DyU{diF8YJuZ%sQWW1G#NmY0)W1-#B_-d%ZcR|F zu0j|ZJmbI{P6Vo@JMK`t#UqX{+E%5Zp&>v`tgh-D(Q$DN+8l=j2a_={VDdVucmjTH zuR5cvI9n5P+bp~BIUi0Ot@VEcOJaXINCNe$b)0s}z^8(Y{5zI+gOqWkNdDk;?}p_c zUS6qOS8v>Ke-@v*J0?@9;(vDf_+4|e7o31;ZcgpP-Q3t{q%R(MpQW7rE-Z}W_}Ixf zfm>Qi%52|^D3o6DGXTyX(C-Vx?h(Ra+fpSz=@bT6-SqX!qmUy%ADG0BkanlEHzg4?VC5ju_5^} zY+CIOhjrCYk4wY3EZJl2XZ3;X{o1XBVTN_5$n#8Fava}nQM>C)9vi+7q@(dcEqpLD z+Lh1*8|W?g!Q`t5NhCe$S3l4?L(<$u-WD@Hz;pYH*+Fr1{ zkByD}t9;KFlBQoXS9`Mn|EoGrha?$p+)7*D^BKUC&UczBE^cwcFhbJZfx&@WyCR$%u{C zo@9!dn=_*Zk{ikbRjCP+Y#wXIZxYeJ3qMPj( z#7Ci$JvUgq1U&~%@fbHz6#I$CuU|Le%Jj{MaaymDe``#9oBj*ZWO{`P!{s4tgg3HE zX9??@nub@oyu9i;zY!Pdb&-R~Puv>n?3s~4*A`)^|B)zv5W;XOa13^o*=gUbVsEV6 z%a_-OV=R32i;bMQIR-qY0S!eNl&0X>7_ySHyP}z+-=k`6Qf!d84rhbM=U15GVW><@ z7(jyYIX=Dsfw}F;a$Q|=hSk6NvQBDuE1_e1<4Bc-;K;{^yG3=^FW>hONSLk=5fO1$ zVi0`!a!2x|Y;td1-Oz#RxJ`I450bgX#y(B&>NtdG(Rw|O|5SQ*l|Z$Nb1t0W6*|iF z4K6EDuRAn|tK6S_0C&h?7gva5>iI?t9X2aTvlfd|$(N4+H1Kefct~%w19h&2-9o6; zRKNL6805HIBl$X#UvW@e&8K4T9E6&wyb@Cd?1cxp5bljR`mZR@OZ5!=8Xzn?p=~An z(tF4oKh-{F=yO{*>D$veQu4ewh0JGQWW;yreJ>P&$?<{H**g6UCx=T(si~(QnL})(vQ;?6!o~>BEDD#)$^X>KGdKf<7$-0=kYYv|7g2>TIW;Az zqJrr=+ii$~(07U1y&G;jCuwh>EOJ|UL6a-=ZOL~2Cc3F9Bb~}IMPN|SQbqA+@5k?< zrJVA>qx=f>1|Ce7YCXOK^+YQwjf$Ne7jgi|mBXObRqlyk9=>; zc&&tmEnN~^?z65Kbo31%r>C!M5 zdlEtf7rA3Fb8|g6cKSsizIo^;N&m2#Gy0s)) zgMpobfPkR6rKL7i5=#RzS*VO^D?TXmv38T@!|353`*LT4@m~Di1ezBd96TU!M0i~Q zP%j}dF>wgpoSd8)=0dL81sxFh0|XRm`uiPRqvKRx8*~3Do1UGmg|vh1s5O<7Qw6i# zI=$X-QRewf2>6uT0iPU>u|Eg&kMCcSl#%(6m&XFn`@#S@MF2+Q&(e}RJa4Fv_Oztr zK0ePruAQA78321I{Sj_U1ZiH5>fidd+UGL?pi;3v`2z6!aPmmN z^tO@brQi@Qlap?9gV(>2w3)BfU_G@RABa_-RvlbUGza9p*Hf- zxavs%1*Da6J=x)4VPP?FUTcQ~^I$HCfad!0by_f|^WpW5XrTfzUki#{9(+D0d=M7a zp|KfGet_oIui{ATxTo-#3!X6Y8Lk?X(s7*j>_S7=ngac7GI#`9Pjcm3%~CmKBVl+5 zjK|SxIo`%blW`m*UeTpfs(hS}GO$RN(=^*@7`;-p(<2uqEc$GR%5`urlN*fjo$lj6 zeZ3AxpLmfm=dru_|MB+TQB7^_yJ!#rD_D>sO;oy6Dbf`L6h(RwqzEWodM`l{5Rf9$ zL?HCukzRsS>AePsfT4vB0YXS}CVTJSclUSBANSrd?zn3V1|TMxYp!>`{dwM!CJ7Y{ z$@SB63^=$_fNjoWEsp~brKDL?VO%AsW?V*8JdRg$=I7JeK|Heq&`5~s*MQSbup<&m z0{W@|t6Uq{030Y zlMaRvn|rhcVJcm~W;=lVL_uXMKx&2n-kYgu{e#i6r6^jA3KAr@20_ZW0kCzP+X3Wqzd@|?2$%){W2MU286GW`>-;V~L6^rTOHAmCootvo&c9djZR-rX zQ+7TJ_Eo=$z=i%ACy}qN>yuwB*lyQZ0|pq4LeP8$WZUSZr_Uc0c1|CmrLevaW=t2< zVuhWYov+f+gr=t6Vw9jpuU%o71#tg}M*-v}-7$H^l;TWvkSSFD2#cKul}>=zCP(`0T?W$n7&y8haSmt)^z6)6 z#!LZo2%SKxtdVc!5|xxeVs^|p$m)R(OKPBSt5?pM+;(CH4%!KzE(93Zacj-R>*{;z zAUez{e-r-A?qwe9?Q5wz1y^p|2)Rg`Nvx~S>*?tM?P8IgcGa^%po8(Ygv13vM#9O- z**xFs2t4!kLE{Ia6bcG~dexg8y1_%7;C}3Zmk4S-zxs_&+eq@W^O@q};+EFb7}@#x zsjhQ0eVhkDOTXk6XcN8*-1#VwJpr&iR=&+&+7pyKe0(h+3AwN)a8p53*#?Nf6$XYL z@K&@S`6CU|m}blTS4FtiA;6FA06h&+NlC*C*&jhi{VDun8^}kEL=5}u&WPk<%CsTj ztRG+{bRbn&QP2ub>Fsp(w}1ia$%_|1%glDb{-=RPS3nKv57ME3y$%$x|L1kU_k!aO zzJu4*t$|LOTes6iC|M#v4uK`b@0iPnh(9c*4EvOO(+vW&=A$&VTkP!Qpx*Ow%GS;G z!DC)!4-X#D98&yL`W)-&K1^tGQUk98=(m&>H@YtYfx*>sAUj?L{3qRDIhV1aRTnzR zgV)bt1oHBViXD&$nLk8|3j};hq-g>k&Rc6j1 zxl;$}-&by+35CG_^aB0$UH#%uN|Vz?{^v@k`M}it@3(pUnD?xt|6ef2|Gby}xvT=x z(_IDEwXbsoHr?QVT90_64!_VBQ~sQPaOVI1v(xVJ@wvHP`sdSJHS)BIDc}G7A^Mzv z9&>lsNjG?2tsL+pgcSt*^Lc-N>h1Jrpf`ZwN6IybvV^LGrNDnLc~qe8qw38DKIH$g zC!B^%kpJ3(tLOLj_uGI;1Hw-R0fm0XQ)L3g(;8{xc*p;ZviMAkYgB!iJ%T;~u3Noi zMCgn6y&=a;1JaDAIuLaP{r|{lbIUw-XoB_Z9#{3Y9WTni{?nz*=$1(_o-hU^qLk>g4_n`uP#D-h<@O(QALUo)@|L z>i#M5{uuw~=xi4st~%?Wv;SYH zBwLtBJ*5fKuPAj}t2N3fNIBsAQfRkzPk8MO0o??XUwrQBA6+%EHovDtt|&vr^tidx zWOYABA2I6ny6)?UKeEH>zBGTh8Y0E{L%BxD<9zW}%tYlg%$;RhYBzzyxxKYF6v z!_Uw+U{~xUSv)Wo9?gwA3z)}^9W#|3qm;wdp^1AtY*{|volgF>*2k`Si3hc!LqlWn zA_bHtfx#CpsETm+JpYBz!L2mz{*jB}pcQp8Ul?U&-e$T&`RzHmnUVM(YcgTMCQenp zk9ofaIR($5U^}JEYbHgO6b1M9CQPeRPh3c-Q@CyXF!q5-m@WaD6}C=}$D3)_Uh!r5 z)he5f!j^n|NET2Rg*T4=EzWBo&CSN|Y)Mh-ok=Star|g?mAqo^({9Y|80LY%cH8%_ ziu{X}Zq2kxk?5tW@;>VNaxWqL?XMW8vq%cCGh_>)Rgx$0pH-j`Qg>rF7ACfRLh(}ztrVa4 zXCJ;O6w4U2zP9sB<2?9K{XKM$n&q^I{Ef`OyvGdznw-QL1Mil4cTV#QXr2=MyNJ>v z-B`b}ldJh~NGkddm0{$=KkuG@ka$k9(JqlxjbRkF^97=;@YIJ=%Jf9d=PpAcrz&UT zOjI={DU$zNwwP|4a~WK zB_>tdwMut0EID28MX__bY;Ld7f7o?ac$h_o;!;qvj2-+@=9eRtbq-=Xf{aVpgw|ju*AvA4N{iPa5t)dmv%a_(a+~!F9$;Kp< zMC=;UcE?h0SDfW$aj&T!epDQTnA6mEOgkHkVn@4YG_bnsHb)teJA1Nq58P&POjfcgii#^T}P40>(FzDg%3XVd?bF0QJf)q&4;Dn%C`km zi{z<$t2jBZJtFDr&q-zsJmu!|J`i4AQNa^W=*jl%#pD&LlVB2M~AG`#owe zO?l0}y&|`hwCPMc#IOq7V7N8GIX^yqJ$tcbi;kv9mUrbSpNF{4Yy9(wDeC{+Zr6z; zQixM^c5_S)g~`VL8Axok(@ddodtR8Rfkn7h1=ZHl`(HT3ssLwsTKr)n8ga22wJ!=c-0w z{hC|2If`rU5u3Z_t(BC6O1_?xdFp%Y^DmUMa$F}X2Q4QTChsL&jx9~-Ra?KSJoyUt9_gm9Gj9jrSwFHSSzEFoKx)TD#YU zQ_)q}L&CS+ea`+7`8-Ye`8>I)Zy#=MeY(-j5*z7&7r9}i4C~enm{H*pSQ_)`w0-(Y zRCq{6&4W{Y-(KQ`TuE+K`_KW;ZHmdsV=?A778??t9@bDIOc^xE?MU7x{>PAl9}+{o z+#$NIYeq6oTG<*8i`83fbAPxJj0jADIhfg`?2|C2H#E(9slneBDFTX%H@L zK+-s+w4!*-ufS6EK9ULCxS6`^7~uVpcE-R!(w{pl%XLQY^IK}dcO#tK`8$>c!#RWF z_y~<}^=D+H1JWqRrnAe*3X==8(FH!Uzwz)Zsl%U73~6nX>mh1>*^uc2nZd^%>9?Pq z2f~DJS5NBC9(_5&rpxntq(r;&jZ>xEuA8|Ny_5v@r>Ssjly=avpAtsMD$DOOb*wdJ?_1=h$s^>bryPbtks^63;!-hV0o7zx>4lge}dHBs$iw%M( zzsV?7V2xe%;q)eJ#RXjf>f%&+6XZfNU3uSL?7q%GC?4n*y2IV`8HkcY8OBnv-#dgZoC zlLRuxDc|4E8y>cJw%q=u*C~Aeg#kpsaDC-In&%5%>Gr)2ul;vMP0$RoZ*0~b-T%`I zfPOJ9D{~%eVThd8&e_+Ppkw9El)+5cTIx-c9r9~H6OVV^)K1ECyX#I8khi~%)g|li zm7I^=%1VAZC-pCnMLi>5m!i}lw|^YFM6>X|yeC_RSNaRZz~=jHCdTuvfu;!WXU>G@ zAIj9IfMOYaq<=KaNb;-j&f%riH^5+PF$^u=o8}`3#NSl+%7+Mie=YbeZC|5xly0P2 z-#=ac+x7-6C5)lP#CD6~n+ADSJj1TLp5za6Zt*{*&C)a1yKTnBjQz+E89zi8;4Wv#F3gs^q;r(E%i%sn_dn z)up^N4gm(iGQDwL_U<)sR)Mv6%`KR^Vxrh+YWc+Hf>@RV!O;8{cd&Pin2RAPP`3{^ zvmgF!x>!mV=)KprRx%PY3JcwjX7}~cj=T9fPgs6F?E`U;6Zk2J(It(VIv<8R|1Dfu zM$!z&d+h>Cy3Gl?5urSa199ws(pOB#d*3M5R33bk%aFsao}m~_)=ibO(W@m-x&B64?*C?H4Db%m+Cppl#*tmWn^;gcFo6sZrRZ4RW$YlJiMLy^lV6^^i57Dw+R}J{k8jCPb}~ zu8%2JH%lt-N*2c+Nd?)d&21P}vus2DMBH5~d5E{VwsW4*7zk@9<(rnml8-Zn?}xv7 z=H_;;>=&f@{M!X$Q~-FI*IcVoKgS#&!~%!o_r?&ggcl)Ec_=k9%T&tY^nJJ#b?2Mf z2XYkZzpi>12%BG^FNdncNVP=&V%F{3`FFGb=o6RoFaSM89vsGSeI1y&zXs(CU0Hl8>;>!bk#K!7Ps5N$j)mCF=)G|D`9x zyc$VsYK4|o)q#mg6qVWDyiOkW^OD(nW-Ow=HOg>vZF`;lYfoxDI|OJAm7$Kv4Rxh~V~ZYu5j-f-yNR)HBu$NeM~H|N4OR0=bU0>n{*hnUp3L zc5}vr%(Fl{(oKb>ZfqqWbt2DzyB5g{%7IVyYKuqSagHcI3oUE>{o8c zFaviP2P*|x7U*!0;~Sjmf!F>M`Ha32|MNf5s+OjPB9{zyQTRm`&Cdz>a8_ygL5YU} zhdLaSO(WILkU0iKPAVM!Uka@gN z=34RmM~-4jrmOA>*k}I^UwG;NiG_0&BpjR0854bJa`>rRpB1$D7eAu&cBUTf?)Y}D z%eL%yU+hjzlx9%MhVMK&EhsYhW)Z3Mwh8f^IrT~zUN7#qs6^Gl#{qSbIz=x@0X~y$ zBHj#Vu$cYOC5nQXi7}kmel*4-!wM&U52AvKhj^-DnqBRDZ9*~*POP|-4568sNP5&3 zr|G}K8GWIDp0aw_b#Hc-5@mT? z^&a=vtE$~o8`6gl4*O<5YwVf6s>S{>hEM4JBfni-`46pMh|Bqi(0MuTr#&Y(SZ0FQK1hhrv{G0Z z>u0eT4l6r^GiuRs3vMlVD|@?Nc$iv^!F{K+NKI|?Kb7ku&4s0RKaWlZ?oWE%9T_KS z;9zCAPoS46KaXz3-RnKF-aWdU zeoB8>{xT2hW^|%|2sdGN$2bbw#~RT8911qx2?-Iq(ny13r04v>!S*QUe6h|gY1PP1 z+D)aIsWWsueQI2P=w|NF1$dubEYY)Bwl9s8%#K_;Q~R7e{QH^Hf33z_J$az}X~l$z z62|+m3y3(ge$&PSKgA!pa*Lz!%Ukoxxc}ph!c^$uP4|6-Z z!yGtKLYb&l#|MHeT;H-jQSLM={U_?GcZT{kU7=%cJ_k z9u+b=16~ui7{SnD-Ec>T6WX5{e~V9$g|+GCJ)b3gy?f30`|gXsSEcS+#NFmT(l~yR zbz?DXNBZhWcY)$(RVw>1m9o`e+_}x8-jc6dXoDU2>YoeSnPNBu`8oO7bF!UlHs>#~fo|Rhk6Y`iGCBXr!l)Zwod) z77o{5>;B)jzoxvRx{7i>JMQ5F_vi#R`NK6*8NvoNWlFzN;LfmCCAOU$ zAv~BfqFnlW`KuZa%42!n=C^B^GcDvDJne=b0u}^~czcI`8WPa;`KR=Uc+!wb~TWFdLr(CbwBUD#rgO;#-ak!*%!Z>|2d!cfFnVG?&SRR72Fx5~>MzBGAvpQO z7B*r#O$5FPV&)<~S)pFqv24h>u3Xs+X(YaBHOc?@UHN@eGU?iloO*o{$S+>eQJcnb zl5zL_x4p+Zzl)4AK`^OYOJ-N?kI3i|Oc zg)|p|37~^qSu)1s+ay$4hxr~C5=J?ixXa0(_wfFSV9=k>FjjqiPrc3ax+TCN9 z@^RVN;kt@^Cmv40)M48^SIJ70u_iZ?JEC9qFJ~h?F5*9r{Fixio21$HQm9!1M*fW7 z?DtS1c{Yu5Wx>Uvcb3=Fw$%qos z6f(%nh}ev%@&Q=R>$ymdD3I}>RD6w!nqAI8h3!g@!6YrnWW4s z_vqWz64^oen_no=Rjd*&_KN{`EG};E>N@8B(2LTw^`&q&kNUA%w85$aEsorw{k*k% zwW7cLt@i~v{0#pK629PZmG{n5Z-AA*J=w|hj+;#A;yxXGsTaR5`v7(BE zzrEK=7fiyrQ;Z~NO!Qnqh(nxV>+kul&OPbp4$K?=989j5ZXqdq)~kO0+q_*`Oiw9Z zPxN+xlx>C-33>&I&)J3ODXA7ur~tBg%c!c z@=&*GKx+v07oX5AxZX5%|JSWef2DU3ABHSDws8Ens(<+lKs%clDvOWWc1yZT(Q=N) za0kvzAVYtf(7&J2&l6R}PaSGeK*y4iCK( z3Ex1tnXUEMKRYhYC@wMG5ku{T6^JjKu*bkb!Q^tkEsH2c{}cHAX$p)uG}>3suP>5k zlRwjPZFyoL-SqT*XP{fxJ$6HAcRKCAGgjHn8DAYM5@%$QTP|6;0&h(6^mB>pXlPQ$ z3)fTr-Wh<0cJVjPuLC zcpsWo5t+TseHyEVG|GAY{(JLaa5 z9pLYG6_o9K0YqTZZ`%SoJ-fw0LLfF62Sn@ zGIB7L`UtWD$p?LJ~u(Hqspp2j0Cv1Q(PMs$WIq!zHV>-h-P0$hP^^Ux}3C z!~#S>hT_7Q?%dCmSz@{>5^c!zqA1A|idw^q9WJWgBP^vfWp7#!AO+1>N)6)y*zS{9 z&OpnwtFe5MPW^-OXRmw-`A(j=44dkH%Q5atppiBJ@s8yTWU}0oBK`KA!CL1_kj`0I zk=T7P_sU8Z_w~sz3TDRhEG%0$k?@<-_I+KksO42uNV4RC>F7#52?R{33kVJ#Kk$u% z39H~wq7xE6esPcV&xxmmq-x{=IW;1FCt!@ZEu>heHIRGHG$Tty`{qwKSmf=AZk)Z_ z3(7jzjn7?QVe)TvL_1^;FVwkjfqryJQimB#*SBS*x?|x2%HOk+hiQl5M0wAb;s@&z z{*p?R)x12sysZ}p8b9ziB?YsE#hW3Ut2;in62s-lv-@TA?5(& zPcB7hlUFMk^O&#I-ERA8HdvwSj;8jfxj1SqvaqQ78_m{Jww*z>mKRO7tyD}-L@Z|M~&*K`Ce5Atd&OIx|Utb&@u zErdR(W4~aq&;%h2Sr__z*1%Pa{Q8Nb84@jMziBbqxNIzi z-*0trcQ4VcYmYGYUW**X9BxSh5}65Bp8E`QGDbCf{RWPR4f(q$SmQEF876CVp!dh6 z=iV5@yD3i<&CP9|ryNVQ$OU|F+cC2zJow&-sqO|$mTlKnBl31?2QfFU$Tqc|i-pDP z!i20Of+FcY^Fr;FOihJu-)fOGgKok` zBb4MLI%g20u!hH>67_MtL!j%G7Ig0l4d1_db!vKZ_xdx~q*K{m@FYg58>@n?IclbTzaJKbsAkyEyYXduIE8!A4c{iyEsXB^$?k0VR@rO(TLhAgEo=J#(-InM3~}(o z1I*`*$uc{BAXhu{E$w>ys~V8uE$F(h45yAvf0K-8Rkj;GK0j@MB5U1v<%9=wGCaCi z5FWD#9ydqF96DnVTflmFcv)K)TV-_}%+yPV=viFc!(f6mJoCK!=5QCw_A)A9B_U=2!+=}w9eav-iZS3cWEe? z?_b!yBd=8@z1=an%$zr-JImu-0n?p76o#kFAvI(@?aG(am z(bWrhDx&~JGwG{^c!I7iqKWpRtPd5Ek9cX~)d-+K@VNjvXU`Ora9~$vvz+B_!X%mK z-wcsOkd#}aRDsyGRwH>^#E^a;z%0Vc+;8xHOyv$gY3W2=ZpHE8=m1Q<86a>i4C1_G zE=w10gbv8S!nF%%Ng+*!RpkDdj}zG{)CU`q<~YK8sM*yUOs~I_yNoA^*^NMcSTPuy zA>sJCt6a+(J7dFvNAKEiTUOre{YI=R+Tr^FRpnRp!S{7~Y+&LCCDLQ9AS;`0$L*J&&I`AdrgOR z%QNhsp_gA5$qBj>Br!7BiC7t>F{-pZ&<4p#bI*tN(R-2v0Rr&~3pCxREQ|?$Fb+;4 z72|`?ffr>uM+QE(uWiNu#vd97=MCQ#C$38qA1s_yf`|7!;YfJA5P!iAq4;{UFw(B) zmG8-e+rItg_INV?@^Yu7XHP4`Lu-=;F_oc{7=urG*pOIR$jJ_l{kg}GK6JV=yd!C9 z9MirAk3m<^jT4vV6N{4_j$N)@A`d8#&+C}O@jN^tw8!Gy+c3BhRk)&~x?r5|A=gqr zlz|M)b{=6593-?a*nh@d_l}fJ!`XA;st*ZdSBhbB6SR4UTiS`%xZW>-eeH6ra$sqx z8p7fr2Bvr178jN*3zrX+?AU*KSv}_BjOO=M05M& zz)d96qxBOjd%VNhvV%q50E*vRDhL3-d1Yqk-J^OBKOjSVaIh)ve`H2v6%?cg2L5M_ z1xqEb1@?TrajjA0ds5u%<%q_~gMoMZ@KwbXO#IxTr9C!emL;sxILR0K`tO7J%ZzF| zZr~eHbVhj7!*%*G!pUMHGAG504q0?`36Rva7mTov;|QH|%K;h>_Kqhf6Q?o87raxb zLN|=pPk2y&9mp_nAiMV%SPzU?0{e|4B%0cv@FGujg<-wu%INB_vw(Mrs_C+lg{<-Q z#ZL!@h6Bi7o{1_H4sJSpp%=og(%*=PU_#uH=Dm*9oiPXK0@Ll^-vt(`Sho)jdTk#i+QVHiV83+ zia0k68ZuYrO9eR1FUurgBwSJ?5X-XoBMN4ykFZpu6ba;T#+%^Owt$B?fJezdquh|6zv(948<8p!RTTBoZNLd&C=kn4FXZriOSu zEYfL3BB?f}mF}>;JSMb%LY3V4Oz#a%##(F<0s_tqvI^ zrNK9C*R`_IoAP%C6}-*B9{w40>}HH#JRIjm2=4ZmRS}CDK23j>;lgsb`F-g@c{q$d zX&@Zm?S$Z>?iUem7BU z$`$COsd4_Hb3|OZdpAkW@eAh20RwE|@tlV*c?#6cx{^5Oi7ukxpHd9 zD{@V0`kAgeyWzBejr;h) zyVc;?dZNO5TE>d6*}zRiY0k;V&B*p*{&2^?PGA1;uuubj$yVxkk`u9?<&Q=OPZBnO z=?lDH5*eN*ZcIG=r3%WmY@BxL?hY_V)WE9_k5{ZUx@jSPL_B}yU05qmJl-Qaw4kSR z4jnMKN1^m%Yu%HeczMJ&+kdXoo5iWzS#T-M5x8_MrT_=eQjL`PF@4yUy;NZ5`BpKm zXkL%)ZkvRY2#+L%prG9jqJ+Utlb=&t`7T?s{KC{9>k+lEkTB_>_nLjP|I-W52ek`+ z&aU*xSL0|V56Ez~Knx~`gOFFnjXob-ZD0!d-Ak82MPLM#^ zb)iFNjY;|&WgV6X?Q8a)p7bExICCaTRqPc@e-qrL4Ik3j2^+<2O&@IKp5ztyjV>SG z&djSGSjP;u9)?7Gva`g7WDQJhXmTynvT97T06)SX>d&vmeZy4ZN0F=K!HjIkRb_bN z;g%e5Dai(45xFb}(HegK7}iDS#q3mjCMK85T(Wt~)hQFM!L`Ge0)DF59UJbxqyVdWvwsQ70mtQlW6iD@?%mcS6slx`jK#DWw^;Ww5vn~DCK#fWDE+J=U* znd``u}1F;5iUz)fQJ#*uwSHc$K94oTF6)&;)-G`vPH33TA#GqIwPr&3M3!wUC06bnR z-kMtys?n8JW8BnWl(Sx$4^xaX+1q}f#_97awnPZ%a#z<B4`(L@siFs z?%11TWC~U8sdQ@|NBRJvOK(mo%M~0*zStW1)?_T-)ng}cVte?H1sfcy#trN{@!AVO zw#_f#U?O`@@5nD%jHL3agdb>wWfb@Zkh9U{eovm<(uf`ylkl~_k#=mqg;lck&BIhE zzkMqTLG#yNybyX8j-cy|tQZ)GFi{0wSF=Cl&!~`)#`>`}f7xzNcYofY=zYEdS?!N& z@JY~aO?0^}nu?a^ntmBML$DM$6DOls~+!kVdmp_g4w%Edd zmCl@jljiQ7@Vej+b}(0^pc3hRlh3TN*kdoV->HPjx;IMUZ!Ytm3NZ`x1Ptr`p6*eMylU+|j42{8qOh2TJ-a?`|jlGNE8oq}chn#$T z?FRNLMF+J7=-?PUV$bJFE)I{roD@GF)pCGV8v6i=WUL z(mRqLMt75iv9&~tt^URI{6pCYIK9b3bM-4?aaRv2M?N3q&FA4lboB7 z1BW>2Wgn`&`6TDD>)XW+V<%cOjCj9e{bCHBA!#efY7NO#?x_4QStFPvzQ-t2xZWUQ->~=)XsUq+ZTdEGs#3&*%Vo6E^~6NHr(Zs<~5Y{ zAv&jQfT72mK#+N8q4!bt(oaXpzCq(OO*NY zlj#0rmC;ZXi&5nZbg+TtUc9jlI?jHyZp=538;U>XAFFZnZIn29bD4fzRd8&N5LhV< zO6j^9)?Rf7TSS#;7lK@QFEqt=ebw0D0ymPzV$?VZrSw>9JSg121?v>#ckBosFRVQl zd?_ye)^+Qk+^b38*QOh7P)^RH0jZIv0YUrYF_RDMW=f~>P_)a0ohb`r%jr>%%ibt9 zSHI&OIn~W7kZD*f(5n4FsCF(XuW*h@N-{k>h^(^DXb;m_Z7!i;d)n2-r7r;WOi}to zJqnFTf!f;H4-l|@x{@&|e(LOBpYAx*7NAJ`c4tJ?qZVZRLUAYSoJxnrPcNL?*LA2h zVY|!NzgJs?9ivZj^qZ{`aVK{5_KwcfDClm;R`nCf`Ua{>iAXv>$*9!QQss%0g7JkE z7N*ze<8C8tAOA!KA-x~@{yj-uDvR6N3HLk*imFGB{pemhUTd4(Q+_nZ>gs2{`E5SkCvLOB*@j zju%}I2KdFcf-_wVOJ?ya3y;T@rcI zrw>V=R0*YyEjnhx`whq?;B_Y&!;2Mc@WoboB=UiTti&}U z^2YK>%$F}|e97KiGJaj!h&m^~I%aS#^vm)1?|&wb=B>rrn3@srxhx4+`+={vyxQHm z)pIl%_DyqnqEwYGH4!rSIvyFnkUHsj+~fkgyO>t>W^z%FjaEtifDBIV4wCL}#ZUXiCKh8N^~Ds7O8u7s*u zm@yZ=z|q&Y!Y+Potp1Mb5N5MMK8(gU^()5<9u5vAl$^Y<>7V8nH~A(n%x34VG34xtNApVHSTxB?ER|AseI(0#TS2xj8s>&&&Yp_ zOqcPk=em1$viLaQ!KVAV2X*++pp5Vzbconr)6K{Vm{6Wvw6k0Y> z$||nPZZ<6_d@T=3WY)+0tdl}}c-^F?r{}ZJ&~4g!=n!NrIFu;iTX>C$a6RjRXTNpLzB9QQp`^)wIlRgrpnPm;~N?8SKd%x+9_(RkocP?asVprquLp{#?n1Ht`rKmX?96rZo~ zyOVQ)f!a>4oSbJms32G7@Pgfw^-qQ56MptbHwd^KuDgFq zWS!`QbAQVgf-xms`G-Ht>58ySCk+O5CyEnRHC;1Q61pa5yuMT{O9Y^95mv_#sd^dE{ zsY>xKF7f%S*eI2n1BMScyFPh=0$|$ePRfZy5f+=N=&{}11D)91C*e1xwSX-yrlf+H z>1(%Ayu}VWW^xRDeNk)2?-$yz$V#w=VB-Up!IW9#YPcAA zy#zHi$sz@WGb91_&h|5GZ|9sP<_RhZ?QV;N%yL*s6CI3$&=>XwDYxT~@m(%O~dBm(_iVVm6M>LeIT8t36tTd3X1)U3GgdVo$X|IrbM?1N!t3 z%@4O(M}T2vU6(+;S3{OiTJpqqd#jVVjDQ1@n?$bx*jD0o@Z_#qWg28?3m{NEy8W?t z!#!MWn-J_N&xSu;HM=2Z;nakt&LW^!6) z%(d~x+2g5vs&VTr;6W6NvCc;UHsO5#hfq&%?;hD^{zByC>1UeZNm$Es0{GNv(E=zWn+!CQHSttwO?A6?c(*ws9 z6-yj{xL1yU>#son5ueSapnB4yUDTz%Gx*z})*nn8>?f zwXf{`gJKBxJf%p!T@3Gxnd#zU{7M0@B;a<)eU%iryjACEtcTO-t>^p`<#z#x! zT~YR@PXbRhDwb4s?}>fyJ9fZy>cjmi0O1m~p@_0CPP!y2%H-*A$n~5bPVGuyY>2;x z=7?5>&gIK=N9jOGZ$#KHS9||(S+!16XsN|xj*Db6k`lSJp z*kDweulC}&b>Q0a0?^*TffTmsklL8THv;|`b5R2=c22cOck3KVrs?S!KcE)DGYXTz zyL@%qn(@}g@f-IK_R{bD*h5HPIDcNJXu8_yem`q~QdcJ;S8&mYC(dv31T_JagT_cU z(aCGd;suzX(mJQr^0T!L6?g73@C*tAaY2fI1(Y#Dg*mKulmukCO zTy|=W#bD7atZ2uP#=DrRIFL|Xl)8ujN&ZxlJ7dRbamS{MI>3F_`NKPZQ@ExY%m%ZZ zIMDdH*MqT?{af%HGHTN{A%xXV{My8!wl-x!X2@1Vm7nL4>UU9F^&X1vDC|9{hG_QA zro7!W)M&|;%`tY__@cJ^;ilo~VQz2J8Y|U3w!wDS>NhzRi_{969ZMyH5!YJ--m1=v zX0B_0=-d1B6LLNSmcvGAb{*{k1*@g?^EdCmt1;{{eri3mYh*{*wdo5n$Zc!OEQl*P zF)6CDolTqMvbUOFps&LpAcP%TT+q9VoiFk?lPg8c#yxlqm^q&610y@T10*Hclw&|6 zk~U>%muktnH(|cJzpK9jzo1aC|2v>h*#4&$7;h!-ymYU;z-}U-i14!7tuca8y7@_Q{E|RZD%wXqvo1eN}wsWLL%5+Rvp4TjKue77wq8%0(KLi8gm7F05R)w%}Wf zWkBkXq5R|laCz594{~x)b4=c=!*AZaYD!wZZ*l6aW6(%>9-WQ?>oHChAB+51MAQ9w zSG3>_!%F3}wae!&4dV84*2d=fd2S2!>h~ifRY6cTcr>?SyMg3C*vw}_xTBY0en7v! z_N{cS_i8#~K|6W(Q$>`zo=k=!Qx*r*$@)~|?+cAKV5O%I#whKkPQnxC5PHBYkE0^` zRlN@hsT?rdl6nTQ(I1CPmNSxf-fRn4g5jTRQz@{}dgwZcS-L^q)FzF1@h$qOYWaBK zYox3$Jl<(J*jl^5vgN475cWU_IDvSozlDU;>RrOw?TC$f%v_APm)WR^>&l=gvaKI5 zJ8Oy76mlCb_FoD3naYu3TQc8Du`(NS|A)=8QBK=ip>IDx5*o8Mq7Cvo+#s-!_Lj~W zkU?aw7s8wFs;I(q<9N5m6R-w?Mv*El(w zw0S1-dMc8&>fKa4LG|s2&nITzA1WyuFu&}K&0NiA1VR?Z1dVgu|K*62EK8VLgMpdD zxsL+sgl>kPs*}nDZV9KOnpwoN;SocLR~2~R{TRPH2I>zJ1p*f=Fjynm;l-gtJw2L0 z))kl`HZZ_Gk2S|DS?(>BfC&P+H-$|yWe6Stn&_3mRdwK?MT78~O8Uef#OS;O zGHc3rf+DrIXoLS?kqjk~{95$b@Dj<=N+=E&KmLr=i(AyKD!Wlk7YsIxheNH<2M0+k zq1`EqIjMiCkk>YTLf@X(m9$GAFMT(hu3uIH0TsTW+riu~Urk1!7Y&kgH);Iy1Rde`{nn=A{ z_nMi+_gU0?!1Tq4s=MqiHJX zd|W8v=%LUaieh_eF~GJ|dwq42{c_kVRgcGS8VMaH$qyWrE8g}__>9|g>wxu zTU~s=5~`V?nQl*K;Y@F~2!?PYrf<%@;0fqTryx@H^9SucoE;eeVgsJMKEeSc4>S| z4_}u}1#z48+zGQv1k)W=)#aHtFPr3Y!Sex}9A)g=fC!c&OaTyEIw~_kX=!wxl2RNF zYpOP+{`sR7%gb<=2!S>VdL^XiKYr!>{ruK*&5^g^V|5sOw9{71T$6A0tSgxOmk%ss z{@cDa%J!7RM0P6AR}-13Ks>+WuY)&y(;YJu-Cl5-Is=2;4B(~J7Pt>FTn36jODU_u zE$^@DC*dpJ&3XZPmKWb%vKJ9uzDl@wjjp=B0<&nvpVqo}JdLidR`2S{8+X@B3{8|0 zc`}8SVn$dBbBl-A;*5n+1T-`$At=2o zkkE;WfDj<`79c7mLg*nt`h9{k|M|XizL|5*U3cAe?p^b)H6w)N&HL_mKhN{qzx~_K zCOj?d?FB7YURmC_5nl*94jC@SWgB7uPfSPqQ5IMYYY;o)x$vn%kmQnvQag@$HZBYA zIM9$WM|NL&X{oVDI}bJU^Lu=-4VFgTGk~9%c-~h>CeAS5{g^`-?>}hA1>MvVJx40* z9S`wJAMT{(JfE@LfeSwmHVX_0og2UK53;}9gf@DxTfHg`8> zIM1Kj44xU(gP$emEpc3`jZk3lt1hndw2ZfYk1bOI{Il`a(D7G2OyrlcjW+9LXHRR?wsZ|Nc=0^C!67{$pJvJ^ScjsvoLuzxCAF?zDHLH!J zi^KFfu&bpxeELPcNL&As4~fx;FRG$n1Vd!1>2#|SWUws!Jl)`Sh1A!@t#Z%k8H5nu z0r%Y95Tj$YZL2Vq8P-5eVa%3cimMF4zGm4*;)p(xhaKHHFhpasf@uYnWq?i|88sQL zMg-{(f!Sfx7Do5GU?T4GJ^sm~4VlR*1)$qC3|CD56W+^ytN>ws+OE_Vr0*=jH-OGC zS9)?9$%h*1+UKo;f}#cM9*DMoKF{+c7vn(Ieoh{2Ii3FD!>wTPx>Oz@*)$OHrZIeW zl@`|Uxw5`x!R4V@>{RbaT)5#*qGRv+(hY?hK2?f+%4bn=;a20ely=ot5g=G0qV`w zp}%YZr5xOS%7$I?xLRr@c;hoyaqA=w@m-jQ9cv47;QqbMsoZJ|r|?Ab1~pH!pP zR8fnk2_afTduXh&YpB^C+h4@{P~>|N2oI6{HDt@lo;)op~AUf93xM}v3q>fP0KrzIplvbj<3dDST!7;)vHrloI}gyw$@w67(not*#8{E_ z?8vxL6ohkW+crHJsE0?b-hkx~5TW@GpFhp~_X>UsvNv8iw{_d~-#;rPbZg&V z5hmxq{o#+dj(qpO>w~RUi8;S(0sfWe`Con6|3A6Z`-ugcDif@P0|Eknb8La9_?mzb zHaYXXefFhece7GLP5yA7^y7EFQ~u-E%58h!{PEkzV^@U!aMHUeKYp|Gk3ahU^4*y~ ze*5bWF8T!u-g*nNMyMVFW1tcmu0F409kKEGy9T$n{+DOkyr>c~GoL(ZkzL-^_hKZ@ zca=vNB7cp$w#OhgE_3sjE)840uSD^}UU_#D&Kf$CJ)fKY%($<-nBLC26lKpb|9P@7 zG$W4_Ue_BkVU<+c-YDh|EhOZo_VHz&MMO*-ZaLC&vuagi!1L=s{^|UN0p860;M_G~ z3$sCWD2L8mdPvaGG1rZd-+Umy5Ehelg1I{use!)9ogJ)-WL+nlTAms&aK#ZdPy5=p zW7q_rhSrd(@Ljv2`HrFcVj^z@eS@cPn|>-OC~JZj(Rn}o}f z?)JHgsz`*bI6)IbExVV~9k^&wJ!5&=3-N6?nsPdA;86ih;iMY_1+{qLeIkWsU%-7D zqHSSF;h=M+gk=CfyJ1{?iH!$(%g*NDJsk}POUY9A5g+oT(}!m{5jLg=HmOwb9fnb- zEW_RApGg;?3X-&TNrqfsKM3vSM^#&Ag=rPJgfqu&qHO9(^VM>j*`A9|75SI%q}Ll% zrc3yL+WRWX^L}pl_!-NB2?bb24pJ+waIK$PT-GAG3?5id*P3SoA}3IBsnoWwL|Q82 zyuXe?=fk=jj2S_C6r~p&=q;A_!32UaCiW&m()!Q}F~{Ya3x9$rFx(w15frhLk*8Q8 zsF$AyWRGhfwbPc$&);o$r2Kl%VQnO$atg_dkj%9^zvl%~Y(KxQBini73#mU#XF0Z- zjgwoa+aByRvykIREWFT?L`X+{CiD?fCKkiH5N#d3xRW>-yiP2Wn<7xgEr<%K$URr%<_ zdm5j3r_5F4@P1j1S#}BQWqX^|b40#xClSc&FD3d)FK0ni+5L z8LSXW)rA2susWqUc+xuZT>@3rFaC@iJ83?AEmv_^IX81AeiKgZ(=Q#z-13)vCY8$@ zih}!t7dsw%l5ABOslup^m$lRniyxWX#4IJ(<|vQ$zzh9 z4mqY#-|Jd0542<&&1>^BM zC4&cOWYxj{fN#qN+rE4tFzdVdBBg@LJk^BMiy#GMqbIs52O3A?2-^ zUkIU)>`mBnMGyk_BKn~YGG)F_+k(6%I}_`WV^JNK=t(iFY|g0ZUbiH84ka87Subu) zhNM5^1$zBjaPeD#nbY5Y+BX4u6yBV%*=>Y?Ufo@^THQn-Vat(1A`fuw6Uomqz@mU2G zwrw_2bSOkVC{ftnsC1 zJjb%a(*{$2%VyJ5m@Ke=Ux8uokH@Z=9?-WVKCjU^Uo>NB##GcvuACAv41HaHZ4ipX)P0ogi%l~|K6r2o~9MQm?^l>2;>+!vbX8vb=as4 zuFyBn#5TeRR$gpZ=XcjJwO)^axq_?V;e=m-_n|Y}?Ut8uH1)_ekH(XS>t% z%^U81Z@HUEtHWn|m-(G?b#YB`QH41eI%cCGJ0Hum4R}-HtueIEP0Hog{;d zw0dn)>0Ps6B7X%XyCa^%4P`QMeEo0nm1MU99%oe;nheT9#kxOP;d0F#PtU|9o*)-C z!X*2<+w~-(6@a5ay^wWNrG?&QZLX0QiTwG65`4WW#ebK;EAN&)6XeD2k~OiD1+k)8 z2j&uA=9+nEBs-X*_wRkmzBZxAyBy4;54{QcuuhL|k(;!?eYjm3`uI3i4yt(PN}A+q zB78;hIKCO9Pe?7YKFG<17&-K=I5PI{naSbkGehTzT`UDxXSqqUgkLzdmbjOMy+%0( zXik_`;0Yx?S%BspBplAFJ)8e@*s@^%k5G-I02^G!Y-rr8h{kc!x>kUJdmf~sg3B9~ zTc+`y9#t*LC8=W(SoL)rOhs564qEu*XjFLlr1dvm-EmbYM0PxNm)7?EI5)qEw9{c`0} z**`GGO4IR7i01$ts=G^6U@4)B3mXvFmq>0X)H^9*rata6I**ku(+c=vmcG|tF)k@S zW4lY3N*y9RUU>OgL~|ch(QY@kF4rJQy;|jn30&LUZpbzAvI5tJAYov6(=$6iVzXxu zg;jb@%c8ZpC+KI?_LFm`AyURbaWrbKWI-B{%KDX{tub^^kCvEeNsi77EpTsKtu-fu z^_TgR)Ps1s`M@AX{~V@Rig(2d^F=Yax2{-ATMPJLZyH76%ZH&z{0_ePkPzH(-qHH! ztf=jB)EodFW;#XL6{He=Y>(matF>~;LTl5W(7u7Rd(;D#V=&O>s6Q@d_x^mT;zlcW z$;fM;f^f(o{jPfmD~o}x8J5~wCN7*>#%e3K+Wx0^)n^#gX3BuU5_iE2+>&64Zu2fX zb#U6xHJUNMjP||OY}Z#96kNnzz5f})Z*awKjKo_?X9pcO1FKe^4GwX$sZAiGBB)hySKDo{>E30!?Vb@cz#PNtqoOqxh;k&< z_BA7S%#NM1*Icm=EU*v0W4X^o?`!RN{o&@bv1_bY9WLdBuJ!Sv51rjC=K*M4RT%6A z`U}-eQQ)-)h^!eD0XvZE@_?*E;3fJ=-LCyYY*=1Z&GpGR^WZzN9f643=VyHgMXTZU zYjIYnaQmSlS6rdp`A;v9+jp%5+H#pKA6mQE;yjawC-i*8Ec%@g5wwbZ9%8Eetu%)| znNi@nb^9x?;{o$y_2`Zv0(N65`q2SH@CF}^XeD}f*1VbA z5F1FpXVKHoZ`xtcA4SO8@!Jo*5qzutyNrJX_s9JxQ`w|XE@HrbA$C{-91yJw58K6OB+$d-IUm` zmnf9l?)}WR9zVqLKOtpoc>-q3_ps4LSPCb+z23PT?=J&A>UhX*B4jT~#^e33uOFu< zsZ#2!lEP+W3yb~4V>{mmN@0ijdgt6azFzLk~bU=b2h zoi@|kP(9jx;5Ja|JdT0AEWyv3n3nP?HL)kpocZbE!&S>Dy8eU+fN zVt7bB>?(jw$@!)k5qUQLW&E(PFbPQ*5El-kWE{qi?Nagh zyfeHkJQ4K%IRbP7))8PlfrAIcci^AO6QC^Q3i^rTs>8QZkRY?bg~B4yvL>&g<#W*P zcbny;q2K)cO$dr-A&`6OUV7*O60ZGx(VbP3h=noT$b>eNt5+>|g=XKAs`h!Ord^4Em6rBc7?WC=h@A^Wj6Y0X z9g~O(b<5l-xkZqIcbFsBu{9fw(y}BKv@#~8+WMZKZHULj`Z)L+hrNz=ojZ4A45P!%CodB7V3dIsI*CxNAuTYD z(Bki|-rB0_wVJav@JpNS9Ftr7t$wf|Oqd(ScM)a`&{u72G=eyp?~b1IBF#&s?o9*) zdv=r^&UxsR*M(BI66#9W01xO33u=6 zuaEy)>R);Q=;c1$h>uHiWxsAjmwQfX1P0?{K^xK&(Cw$CUg}BtnCGqodB=G~Z=MV^ zv*`o^$~6X*M4!;mXnnC>EUhS$=Y2~QbORZ3Ae?5*40M%d z74&7cIOt!kPQQQuXmye6)fdE;lHczln2o#XfP4V#%p%pBTsOIU1ir08Q)lCvO~QV= z=J@5MUJ!0qe2m{|h93&+Yu-1Ol%3r!RuD*I%RoVI(9YGl8hW9VvRHg_s@0duR_lMn zgEKzZapI(%>J@YVs1>j|W#;aF=#=|O##r@&)Y*}>Em+Nz+yg{NttWYLQD5KCcmT*0q390Sl?0|(%LEFvelr!! zu4-QO!23Y{FIhA6nJuo`7^pdoJZ;`e`cN)MiWGJK3=`Hzqhrm>uk-v|+*}fWUGFd9 zn?;Ea3=}lL`k&t5?l-UVOq<-ZCmwfW{Gda9>I;xEDpB#$Ibp$^*QsxuDa8|~pQ5)dq`1!93pBgUu(6C3_^Ol+Js+@NX4FU-u5Gxf}FBIG2opDx0_60zJ zii7i?W*bM6%6x#kv(`}O!G0>wq#q>g=r1VQW#S%8Mg*@;4>$lddhc8n51sj&$QMI- z6E;Gg^#xfQcYW_va2NZ3m?*)|*%Et*WtZL$)G|lKv=(=rcBsA#6hxwRQTcnrBUvkR z*+3h_m}N*ArYRec@$)7?0o+SK2^X=hV@4-y*y{%?zN%v7Nlt|uck@%r$33E=9?+5@ z(u)iA5(sSUyOzG0pQBk}<0o?xwt)xrv1zHbQ;iW8Bmel(qXg-!>}-!VyV$;TiiIx~xL!&xxLEVzlw-L*P@dAKjN{=aq+DE`r)=gIV73hLUeiYX1NQrG@csp+ z{BioX8w=JggV%hnrIZikFx*&J_de)ua`D7t_DcGxgVuUMPS7joG6nS%KX$^vcmQH0 zQrXDBfc3pCYgX|mpUFQygYuZ3RvkFevh<&jhsn;bp6%<=TmnUBZX;3hTHn^|Uu}=e zi8wDQSw(DjEz_~*z6Se0fOP*UUF#?_3X&uzEqx6w5K#uw#&ogBYG@5&-tzkOyFhMV zAXZ@&yWugwhmq{L^jfXD00WEr)mlBKf?Q7~Xp-6<3Co_-41zA306|AD&dv>}1EAfF zk%_%I119ZhN>KmtgETUH?L%{OuFdUzpj^bsTKwO`jO*51{IG}=;S$Ve=tX9xwBe(u zp0;VaG}OTQearPTU*ccDXFhy#LTASHS^&A~Rgg+6Yf8XiGHjHlJb+3E6aowM^or+y z4yiTQ1WFq#VI1PRQU|cmF3HAtNyGtCzlk{$~0U`4{Ff z`4`9dMg{mo<|i_8TDRP>OZ@lJtQ9Sn_eN<&R>G~zx-XB+3* zk(PdT{2>{`8U45L!y%Wo!xQ%J1!y}Tj=&C&B`sDn%D&`V7C(lM4l^|TRw_SJ8$G+w zNy~J(tZfU?W)Zu6<~*@QJay!3=FFIlosw(RK1ho8r7Q#6k+Pg96ZteXF$?Zb8dCAG^pGt=5?9J`0mGlH(^_NH=r{0&o z^EHkTd-|h?b=@!k<TK-Ug9>x}(xz&zRVpr0oOkGkydg+%czVQ7Jr_w~5!0ZCJv&>;TDxIu#p|5F6>vFU3#=bINM3NxIwvW7_N z_qUeBc(6h0!l4S**YK?cK2G<wLVE*wOiR};QMBUF8J2HAf;`QA6IhHs`$qSEMgyf zeLYtaG;oj>>`{Ra}Nl?OOFI2Lkg$>x^`1fd*p7l!A4T)oNfK zC4oUBSd2F@7&D8BD$ds+jTR-zZo!u>x%doL>H1(F0(07)_%oKt25s7r}xxE{s6o-NaJ0HNwpo6S0x>*%7{%Sg77#+$>_RrGPw3P!!P`??Z@vaSN@Mknj6jQc z^pJP|TZ-@rP)aGLTu7T){CdWb-;mjVT?rz1|C^!>5?bhEfT`sXW%pZeV-z=BT<9)c z)Lsw?{C>Pq|F3hyy>HDz_o{SDF0H4k>nN(mBhaHhl-uOS@?PC zY6k*?miU_UJudb&W9kL?D66~hd6$x$oQTMb6Ix%37RY@!M#}f@-JQoJNSXbVPLOUP zlQqFbYU%t7QnT#yw6rMU9V*@}U{-~Ma*lqbiB>Pr@H?CkB4V_ZW?nU-zyFENIL$Ldxb12paeaz= zPvYq2EkCE%keZ3KU#Q4_+cp0Zs_+!6!&WW=hyiSIoG&xo1`#NQ*qkTCi4)_cOUJ@> zR;!97!^2(MlQB{-K2}r>={y!f$2>XYZ(bHd(8|!`pTnu(s&2pgVvlTmkfankzjxf+ zw?;NbOXKv>63%(wYUYeLu5YR;Y+a8=pNQqhK0Depaep5nt)(K02zoD<-s*a1tkti8 z#??)20UIdC8nb>DW3S+$nNrq5tu zq;;B}>$~?}-T1j`QE&n3v#%6XXmlBftd!}lGP|^p7mAd^{!(a1gwsO#^lF)Yy832JX%*?)pWL<(##CetZ#nY z+Ebyubd^v&kfZJxJlQwP;(Ftb65A>9|t+_5_6#F`^jy8SsPjXu^%*+aslBhH(aHXI70;vafkv;kW{k46loAZEf}LtQwb% z9_VW)g0B$`@2kvg)FW9I7yBj#2hzLoJ}I?JEW4pQ<6}gv4tN@mqO~5W*Yi?WcPyBO z39u(zESpMnPGZAGX+08TABsZ7)#{@)v&#?Ko~vsr5ICNyl$Q(J7u={0L7evkDfF*K z6K6Hht!V2J;HLfgBuV{1cF;5k*wFQNjvZ{EcB)v%qV7@`JiEt#EuB6qqLNt#PHmc{n8g^Bd-l?kvJ8 zxADassiZ!khEL+Rw@`HC-7wl`2BPXt^ziovLskYedd#fV6u{0D=bNH1PwUo(il+0M zD=0~6x;sQK$eYNuZlZ^`cUv_L4;}Co&;DFeob8YR<{7t=x}f&K zvuc8&-a9bhHiFw%N%Tz~p~GQB$zbp_p)q@<=cc1C@NNZ3m1m_oP*VP$qm z8@B!qKRo~^+NrVS&RHB^}T74kZ)i z#p8*nfEy=I9&ApJJ~TiYa&vqRdJ$81X!<6`8mkw3KxDCg-o8n9^n9^?#vyC-Nf&pk z|Cpf{1z)YjlUM0UXFlO;nN4Jya(7eJFC$<(tXm;v75vpV<4Q=*dkI8ax^#=Y=EC#w zXh{ALS%LKN?ItC^37q=OhkZR|)@!qB*GXE7Z!!$WVQkhX4>I1cLx=YAR4(j!em-9P z%TsR8g2Fhzn%rSidCRm|LS5B=s?9KvogX%x{*6t}q=wX?iIQaUBmV8HMjA^iT1)Fa z?IF8q%YvEEEjWl0PE8GFttokQV?0Ky72}Ie_PC9cCdfIGiJ*J^RAww8X%9p!wpkLB zaEJ|0Jb%yV!(7EEd6uh2We)%=ZP}%9wn4s15!xMmFIs)Awx_#_xLoqtp@&vc&6pom zvW^nNzxNSv!|D4^hcIUPu2nR?i5~{y^wn#+JvQEi@ZBVmRAqSl{5~NetM{Tg4o^{# z70Pm>1G{88Z&CP5YMDYuh0!^|N}4L2mzUSxDfQ&TZt@y&kDg@dLpMPlwJ?`3m&l&; zU)P_5zuld4%D;AoX*%6IodNr-GycQ{cJ5Z=fuqU1*#XQqLf21-r6BZNuU&9*a&iov zYZv$=gTe4{cmE)T^B4R<jC$;aa`~F{`Ye8?T3FCuIvc_jr21Y4EoU|)Wrrb+inr}-*@nl3pM}M^}B?yB82Iu9t+fX!}ROkGl z8G4N3XMN15R@=w6!Z)PIp(cOi+>L;;wXRgUoh8l@Pk8~W_AURU-@eu(DVWVa!Z+5t z>1jw?$4Bur=tU(X?^{L6Wrs4}Ge!v9=7C1u+B|~Rldv$2h;*9IU z{X5_(sy=>ANm$+wa-kfQRRtx;7+@e{Jef!L3ONZ*|EhlvFxv6%__f+N-Ca@Y)L+}n z7Vr)|gU_#GH6h|b?y=HI5EQal3T2RO6nX?2Ia}?;mk1p(6)f}U)Z4?G)#ukU=sj4S z=dw~LjlRA)zy6^d*D{h%@^JES*qY`=GdoR%K(I^y%pO_gLx}DoCv&j61k=Ze6~;Q6 z1CDDMPsBt0kz86%cXgmk(~~-_BD)I5`6X+six)4>SC7krBn8a%?Jt{$?!=C%2Y#&R z&NFY8wwdR6dV2a#et69sUEn=Upq00|g{-x8!5~_|r$-9M2}@0ONq)4^;Q6XVGjsEl z)A*57IT)Ul2cYwgs^kf^R{01St?rZ_-=f)&c`gup}60*9O;JZ4C z_d;jtMk+T_q^m+!7)dH#LC7#J%5`M@Sum{~p%~O=XfW$G@vB!ErM`yB5-({@u=7oz z6{kFv_OKq6htz+{Pt}~LnI0+#J!ULAu!7QU)Z@P|E-E4lM?|+BOIhtu8Q4wgym7Ic zrBEU5+p%Qb6i1$KIT~X`{2+&Vn&MlpVdsMjmY1rZn2SkNA=n)ojaT)trDks%n0T0S zd9w4O@YRBZqU3PS04!9A>|GOBEa`9Ko1~=*_3MeMkY7s6DuSb*NOH$}`+KLFwq^J; z4~V#S%vNE8PUv)%>cGIj%4#4CtPD4$tS|?8SR^Y+B6#BPP;ky6r5Q*G@s)wX72U|k zTFeP}LXA$lI?}%$Qb6YvduWGv$L%?)V3gP)?))*2IteyfvhOhm6vbWKLpgy1ccds$ zjM3hMFBkxe{WBH$5|LzY+=b=^z5-c+ zSGeQg1a5$GH70Fzxi^+{YQs9=z~%lOAb)ki|IYdgJvt@E(2K?HG|M$G{6;-|o?|Ql z_IjBHb)n-*g>Phu7?1XS4YGN+%_~s-Msg8=(cWf$`OM6e%lt_4 zlX}mQ&jwij%j#-}slIlv?QaZHU}%Yu!NgL>zK|s4>1fW!vAbCu<9co#RPDAX2+sT@H`!n6sl6 z+0f~4z>Wst_|Y@aWsaB1&`Y<#YVSdHuv{f+hx+!z2;l(auOPq#7%)GQH`FYc=LEgnzfLjQ>q2mAgizMxR)l z3Kg+WPLXx&DV&RdX`HN?$D422IjGd~nflpC0JQOQe_&-ZOWasaGJjc_N)I2VlO=#^ zF6$7=G7M$C$|>IEc!!tuL?YNHh{BB&J?rP*Z_~Ul8rU}_j%;eyif@J`C;cS^oq}w|>BU5unOi4Pv3v)#bN{Tm$2F@$ZsY-V z-sMr%Hhw5)`jma;asR0>j(VkmIAlHj8_~eNz$8#0hR!U;%ab@$L#`-1*ui}Yp-`9F z)YMd1H?zHeGMJ5-wkf{_;@{C}B|q9kO>&)2U|@|mjxkjQrs*TcqB}1K7Rz6IyuyKJB+$@3Mn92n_ zS)D6Ta_@X38q^XGVqEcnwkj$np~E2ob->j>2DN@;tIL`LtL5*@pGxFf+)-4##NzM)< z_*8#OOO8Z}-LbJ}T4^p@gYBS}eo^U36n zn*hq)LGs?r6$D_xTLcHTiLy%MuBwM+tyWx*Fi%i-WN>l=W>&fsUSMxGm7;F5K}%6( z?ofE?*Y^wk2UNB3>hb$vd~F;i{}A9h!IA#`w_s@#96ucvgre6PLENz1chu;Eb`N?gCKfGk>nDK)vMAe9I|w1yc$ERYg^z@Bw-jn-JsyqdzA5KI2lt)i+r zRkt>!K-c!7a{vUptBA7~z`nbElg}tdq{djR0eUC5sedS#zKbW{T$G#p0q|95X(~3f zg);e^pX-};D->LVA9t5^R(o|3{Vhj0;fIoZTLtn$r-Fmp@U30#x2Ay)Dp9d0Xp^Jb z&AbJh6sj<|gSeKBh{kI;Qpvn8+vTLKbdTfns_dKw**nZhhzkhp3~Ofc%RT!}TF&Js z`~U}!sN)*;<~66N8F zpYJ)WW8&)GakwzdDdMT(}g^ zXcC9Jv=$+#Ra8fPLqi|@pkG;P5DNveSTnV};S_al`3^;a*MR*!(2W)1yruyHhWHSr zFj^gG_*dAfr8anBk}QkGc$KihHq3SE1Rld&5XU$*Ii`WBM9nKYhR@2onjD~)O`!V@ zBXT>=Z=J(#;`?BwTR2R*KX=_uF>rVv)Yf^xflVD8kJbmesUFrL1gDERZ7kIWw$s4O z%mk^^X3finsH%qOCsL5Je8z`z5GW{FM@yu5U4~($lLBai78Vv0f>@=Xq&7i1$<91> zhk{rt2$=?yD<13+acS!tpti`OVnKME{o=*hL&MfoqMk%t!)~_O?jIK8NAC1|2^f0i zkDho7(?aVZ)m1J=U|x88MuaR*`-K_;8ib8F+Ng{;P_=LN&ht1g&gPD9KwHaG^8o1$V+ORdLp zv^Ej^FZf~n@DTjWY(u+EmHDn1W{e1iND~hUk%i@ggW%QoCldEL^Px z1zEk*!{)D5kaFl-4yAm%@@Ke=kSl2tSz^*o4@FXuxoV@gw0b1GYA^{n4}+z^~H>d%d` zb|IctBL)r_x{GP@)-#lDeD0ejBVu>zxS0SPnb>09l=brFQq1wh-F(L$FL8J`)rns7 z*OcpNcz8k}a@ewh7z3|7DIgpGH8-YAI*`O~Ev5_?gB3^d=0xK(h;VmmF>1BgvyomQ z`oP4C#tj=I&SN+-63(pEEtzRctN0ON9DdXsi%lX0mH38`ZsE#8!iPGvT#H~{aP{ah7Y;t_ajn;PkfYuue<$DE@!xN=}Bu^@Q zb+RhFwN9NSuICJj*ntXfzWc;hsapPL^?yd4J^vAPHa{sj6n;&I0w+qh)NfjCdqAo2 zZanBA`j@P!lMSt`mR_d#XEf?VOVrsln6C$Qq189@YWlIfw) zb_LB@+p#cpkM*#zr0(3&=2`ku!P(eMw-VpH+}taMK6A2L#wMdma;yBk6sMBK6%oFM zs&h5B#Y9w`K}#>gam}zBDNm}V9&(s{1Kl{g0Rh>eJ`IV6mY%ckz}UrhSw|W4Xcxr_ zSghLWV8d_DL4(FznvGQmYK|y5arg*(U-MXSXo&e8uE{{L16sO=g6Rr~UyeAe^g9Q; zZsE_A{tNQc5WjC+McvPB`wq^N>_!>h#QvhdF|D;nrrt0eECb{9|^zF zp8>r0;qvgXyp!k$yLL&12Crvjl+t`PUEbW)j+G{9ubE{Vg_nBN>P8*}bkH|~d8;GA z4kb_aF0<^nEAc^R=6lvz;0jvTz>G8&9#T9QmYfH+u?Cbk#4Yl)4*T_MdGR140QX%< zX(@~WIGE6MxTDId$1s`*+tYNW-G7JKOhh2jnWd$QD*m3 zJzGTS=0s;2Oxrcot2!wh?4pMl`lu(9%4*DSY?`PPxFgdC)d>2MKMqA@F zAZC}}Zk2K#8QeA$iXBa;obd}DZ+~rCv5U@^L}~00k#7aH|0#~B{|MW3@N@O-`DZZh z%#zkhq+aP7Y?P-C-L2#P(`AQ12iK0&c2_3kGXyNrL|ndbt0WNik3d4S&S_s)t)JHZSN2X8j%N_ z_8{{=1MYg!sKY@9PgP`+AAYDPz2Mg0)y@V&v z(+!@4&(R{lG^47{E{rTqN8ZTKAD&9TxN1x|koHNI4S4yEcMl@{0LKB=H{X1+^`ybp zT^}?~4?xjjp&yq-A;_E2nyw~j=R^#m?RK*i?s0T|Sj|Gce}@*A4Wh1&a$kp}R;&2w zvL(0WIrA6;kd^W8ZlSdODizh@1Y&-`X%N%T%TBD4=Y3XrDgHu^tVD?uCE65&R2|2Y<$iZv6GXgWd&i{QHC(=KA}gd^N-0 zZnI&B{}|^AUi^Ece07n(r>_k={5?`O%<%U}*)YT3lf&PWgV07s`tQl%&*@@;ZT>$s zQux?9&6wqa;sppVImuaZXf59Rdy{X!+r~a}{KrMP<1bYoTU|QtZh1Txa*5@I`o+u6 zX4bfzy08E8wi$;zy08E8~p7D0t@`@2Y=gO z!wvqP6aO9y8;mt=g}*NYK)BiCZ8cXg=We9-%MNz)st%8(fsS{+Jd^Ns&!uK&01 zs~wgI>IfuX5-#sV|l75OMgQwol|M+90b!SVH{yY0RG{Rt`X>eEF<26%>r z;4kP6QE}~OiE8#Dr#%!g9fBHPz_w7r7V1wUQy)IeJPdV1cXXVtXAj&*F;|8C8V^{z z#{T~6H=va2?(BRO<$8_8AXJ@&ao$;1k4fF>tY@}Wx%o7RzLg`_ zj4i!AWrx>-@F#`4>q6Ijv_X;ZN0*I@9o!zGT-;uBSu5EehA^nFq(?D7RE8?mQ?ngp zG^o>)%&uKAR2DmJINv{vT%WN_Q;ekY0+bY#4+&nXP#T;Y(1Ht%AJa?r~2 zIYpP&*Xrv}H{ttNX6(q%;uAS%ie%H$`q#L+38+<0VYqh(KCREIDS29{bbCGIBYxu!>f{!hEOhA))-E*L)+q&KmB$aStYfSs# zi@MTV)A{zH3+7B7uFU`A`?u;eW7%vse{=K%$9(mR7#&ad{jiKz5$z_Zi(oX=A$5Vi z^?`j)3a{Z-WlH+`U-`M%)ic@`Tvwg^ms`qo@Vcwaww@jSGbaS{uEUi2390U~EE(Cf z#n*+7qiK-}Oq8gy`}Jte)%%6^L;WgK?QosrR1Q`tR$6W3mVvw$pJr3#XNjKZ!Lq!8 zHkiJ=kcn7u6vs4tvvn?oPT(+rLaaPBWQN_--Ce4StT^rCRfs~DS5#2hl$uim4hSc! z!fQ#RL1c9)?jTfDCu|({tGkyMkxU7?usVeBV=-tccJQjB4G(v%} zu}UytH(keyi^-Q`^w9y3JA4M7qB>V>f4eb0N!=-EczC#!ocTzdyJD^g^jn=Tsj4ic$SR0j^`Ot z<;%0G#g`=4!Op`#H%Vv(m2%|*hw$(!LZR*Nyj5GykaA4>)DSWCQEfg^bD>fV%S<5{ z=;<{dTNuZCAYA&N42i0HUIJa&PhmCJh#qh_CG&OP0D;+guaw0~DP8_-(6coAj_N~T zwR$aoIjU6FJTVF${>PCJ;-RXK9Mn~G?R4k8?%v+#pbW5xMpqjR3#|_HYJE$3Fp(I- z%P-+S3#vCf5 z`%$0h(hg;W8|umP`r0_Gd0G0>xMQM1T4iYc%J^0#M)77oKL8I}uvecJJmIjZ+A>Ag zwxAST%qoQ=UjR?;XVkA%xn+^%ON`OAj3T{>YA1?+mV2KElLWBt6B1uSUZPl(|gh=Eb6#=(G5Rgl5L`A_sAcZKmKtPei zB!mDV2}uaq6MLEI?)1w}KkWJR=H%qfd+zUf=Kp&h8RY3+9O47oP84BCiv{c^2&Uo- zgh1NxaA| zrj-zBTYwM-!ol>4oapLOe`vl*Ke%$yLm)NJ7j;=o>CM{cJPRyICLCwU7tH>o)q9nc zhk1(md7=4=^~^%2rCLay|GvN#NjNXZfKp{}ZF9jiIbvPGfF1@KVQJpv_c2|hCqO>w z5HC;xLGL$?g2YFI%1$^df}!pW*pY-U_ceG zsGdXQ>*MLFLX`wR8+|Dq)+x8|HKZ%(ALUoHX3Z&?s}nL|;V5)5sQc@_gqI)Vzd#NowNqdmIb*e z2*mR0r$Z#KEV;N;<$la;4^_Z5qLRZ^!-WO0=s6v38^VugRSL6&aaTb+%6K|i*{U#5 zDHQ<$NlP!E7)_(lLHCYn8i0AT3jvOrVRJ6f?S%C1h~)UdrZb5qgQ@~dOgF-7SK4Wo z_WHAUtFs?Y%7%_djAf9Dv#nbr#}Y_eeUD$u)5>fE*EJZqd22(QRozQfo4yI{zD%-T zq!4SFVE4Y4k-iZ~{3~AcN#yb-J`vsSI_y7f>d()7`9e}}_I&G66#+VUhdx)<%%yZ8 zFdf1)741S+@&QA;FfVzk@wzhdx^J{sTAQ%QR4XdH1NVpbo}Nv2|=R>fhE zb{%OCE!+kVMjJ0e>uPJ^7}9)Y1m*P*gl#SHkt|y@mJjV5^e8Z{n4N2V&e`S{-ij&g z6nn5Lh|(%soYB{99JH&J9^R&29U|*qfElm#sn-jHC4E2 zCpXApo}cUcq181{&bcxOpka2X-qK`U#^uQr-x;5c*UP)i9>Nm-TK2l77dgP8BGYMx zW+1P{D4(gcRY%CBVRfRzs^VI>SxhObM#Kf~*Z27}2}I1!zhNk*f{4;R>)Y3M#EC{? ztsCu3a!6Pgpb(zpGG#855`@?|j#FPG~85xz7FQ|r5HNBK2gU@ah(JHg+Za-4u|DJ)Fk z%W_lP_I*%jXF4SiZ*D#Lq%R;K;8mR~uHVVmLJi)FkrFsxrKAYXIjCvV7Cn9C^>{Jx zlfY4FbamDIExUN@uFc(!vn^)VoB^I8_$rE`Y>kl&71>w?5idtHmLUP;;?}gqH7@yO zl#5@|8IQcPhoa15_2M7zEo_EBd+sJ!Hg5QO3syPJx3bg+4aD#u_!2~dyQu)1x6gDg zGp|9VQ?^yH=5Au}BH2Ck(XFA z>siY4zD#e!%>Niao_5@tm9v9i8i+KA(g!Bw3uuH&a7r;g37Ng1?|Z!0GCVeRHAU=P zm;FQ;;UF$PnLUjEsM(bB#=S~6N0k)W?z;bsM6CWs^E(c^8HVoure*esvsml zOE`cV2?Q!*E1`FcY|co*#sMTvcFJ^+db_5^%e=q|GZ{1;L&nCK)*Tv(_1fDmey^WE zdIImkReWU&qt1Azdw8q~S9}|vft_e#y$_+7+PwuKVDGH!sS}<}I=aA;p9{#w3O#5$ zPhFQ+4^?_kadJgqv@I{LglWeT@xzlfVR{LrDN7@TAlEt=pl%^*rbSy);!HGK~| z7-EJ3uNRugaM)T?CpNkL44XLr<;BXe6ZzW!z`__p|QO`Yav<)5&V4Lhx3)HcHLjv zcUi6Sl7_}c2#zAzr{tmzuAi)JZ+3X7nL^YpT7| zIs;lxUZyd|X5`S$nMcggYMW231Mn*7a@hbddVKcXuLzpW^aSp8whaV=K0BA1k`kk# z9|uJiV`qQQi~_#>FVo8J=dpQi4shkAUC`bHTOp9*#qsnFB;B+A_$%E%Ay*X#u zJ{@@H7rW8_ay8ZdILfwBUk~UqtF4{e>p$wRUm z>ut{j(&7*DKc*9YYX`I4`j2V__V#ebG$lJu^pn5j4PPT~@py=VfdNXEXRe^8F#~J!90(gunjjhIHdMeKk67IG0#d`QDy0-HHbn>V60rSZxD^f4*rM*2Z y^}m=$J4^8!*L8YLQW6#*(%qmUT_Q+#Bi$W}3P?!^0s<-m2DRu8>0Bb+&64h} zGp2j*?|S!lexE;QUDqq`hPCFJ&-0A9$35;bgB~eJUOY!}4uwKpl$Lt<7=^<3L!nOd z{e=g=`Q23`20uUEXbS0983(HENpGAt5{f@pd=hDu5)l)S2S|E&cVjPew~e9fQwy#i}(6{ z`Riih$|_X9!p@^m*HO|B@2j}IUmS7u3dNo@uJ!TLi`}dny<2B)r78Hj_giK@?H7%_ zZ{|u*3saf7EmcAZzN;v`9=x5OFP;&o&TL3;Xu{85CA!|acaEIi{$^xla=0YTShk}c zwp66AIBFQ{xV*rFB6;|q*X>%)G*{Hk|GW%oWKh9@|NAl~{{QYb1ank={d)DUukK9- zv)*~~P385d>wGuEtBcnfI#VRO?r;$O&nx02>=`cpuL4W8kcQoz83{>APS0(t%9CTy zvLXu&izVN-Wsj0z8BAJX^IW}rC^#U_CFUT zaxc{^w33#R`s-kOsn!s!c#iw}brd~)`swhID&ORDSMTN7V%oG!AET_ zk0ekDy7V9Fl9G~AQc@PXu28eHv!jgL=>cdzvxy7OiWA!Rf>MhRn#<-aNOUt zxPXUi(h@yUtex+RHv5C-KmyXmHgD{rM05_qhC50`KzB_*9ceVWa(m&xbkXvYmr zKrQr6B9vlcdAS2^)cB6ef*1z}ho@%@Z6qmjyld2(snz)Nqu3M`Cq-z!Wnb70u~(Na zUAh-eEo>F0^~a=cr#RTzeXyJOG2I#)zBrIKwYYfU-ouBd4Gavl%UnXo$MxGx^F{_Y zS6B51a!tCkbPmu)p%mOVWn-A&M(p-C=jfke+u}Iaj&%y2_PCuKAFNcdlU?Zb_4M!% zQSu~|jWR>=TK0wn2L6Sj;I(=GX-6TRD@ZJWKq*a*e8Cb9^u2)dYvptWB69M$>4VF6 z4?a7aa@hyh_fD_Wn$YN)hYuE5k|yxlnr_Ut_Yyi1Q1a<}GvB%M`|Kx$k~X1$KR$pd z&CtlpecE&P6#>=j^mO{hFsj=2_E1>;#zJ=0<8%e8gn8-T9!myrPBV=(zMr4=e0wkK zNfKQDU}>;m5AHcr@YKL#I)0r}Cj&U2D-4fc_~R2k{`#1fgjJU^n}*Z0?So`EwRVZa zOVN|VD9wVWXondYndmzaR!1d{*x93A4!!{G!?Tre$x&mAfi|EpHjSaty?f3mOx&s=ho=It%+q z?|Xc>rYq_=d*Nt*t9ERR#xIgqc5+;QjDGIb%a`>Btjq&P_x#E~Ng z7c}IEOS;2p%ah=~RM5Am$wTWblMybm-?V>1ukgOqNxRA?G2)hFuFsJN zEYHr#X&Sb8HZ@jv@~xV#L31Q6&s*u&!NKSK@h(noFAe2e55<6AcNN^7G^qBXuUQ_#_wSK z+-PEUw&8GZ!`{K+M_*qoyiugn?T>2(8lMJlqEpM#MhzF+`v;SqV|k>kIpP+6&v&(OwFYC3&TKtV9^287 zswz=NwXC|Kp@g}v^sZ0h=TW`cdW!n`AGU;%XaH9{tq-R5Uc@^dY)>OR{2FgqSGwZV z(h^m6MyB(FMe&p0ohbug>iC4z#>aR~c2+^^%3$Ii?!J%^;fW?(KCJ+3Lx}Ff|vA z`Luo%3+RWVD0laDBAA+|7Ia~P=xKCvd~|_=tI0zMb}@uTG-+w5$Z@fs-DPF8J%Gl? zeB?1i1b9Var>7r6?D&h2(5(B5QZLm9h=h7Rd$izhDdwG@xSn?ZB{jl5UaOj^X<~g# zKw!|L=45s29kU5B68PZOYi8RMO1$eV-r>_WBCLgRhTMQM*&&q8Te%g;wO9 zot<3fKT&}rS>PQSZ3(>L4_=YvmQ z(RLk{0Joasd*bus$B*AbMXU>b*^Ogk$+ZE5d8X|Y9@~q}aCl(-sECLNIC1(X)`|R% zuQ<&*a?CJEZhPN~joK4<8!;Hdl+M8L@Nj3+VXwV)VS7=Uw}P%Vu(GZG?VsDGJE!d+ zAeA^TkOUA=<%%2>Nk`GGwS3dXImCpsEB9Z+ zeV~n-iNW6F)YZv~Y(``z{u0(1$TP#jUe0dy>G?jjrsEE?*DSJW_rt;c>-_oPog0ea zL|?UvSj;-!-_Wb{Du)Pd)t`f7W@eV9QzijPHjdxXH1h0FpRQ*p-bD(C=oZL9RX@Z1 zs#hg)sm$65_PNAv;#Aeq_B8?mg3ZN&sNgZYU7gcI}=o*$(D6&zjNw4RVa};9SiX zdNcEF$C8{EdOJUg;R^`~+1cCIKyX!5R1|PsZUdjT==pXPj0!!Azm#d&mj#&$-wyzV zX0~IV8OEm1#R1#V4pH@sX2Cc4zHf>@vb64zLda)jW$n$<2_CRA#Uu&8Yd3||)wa7f zg;r;}a_5mB9Fg7fFn>I+?QuVex_kmps?*#rCcR3b&W;l}vJf)%Q?|CYY4QmYYm@a1 zn)!DiTe@3)p98}hQb~`EB?SvG!*(QIpcWzvtcWVG=t^^Qb9mBsZ_Id z+rKEKpmvvv##T2sKR*m24(c$`rUJLxTpEhq8S_o1;C`B8*~hXxRz|Le(l*IE!e}8|RJC8p2>15&1s6rf#=do^q2;L8X?pv!6({f1} z#Mz)74|DT7mpmgXjT#BEbSqlUvzAx$j(OF>qAmJ!Zuo8OsleMlnzY8O&HwI7Cj>t> zSf6etH^gW7+jr9~e*gT4+TGt@Z|5Deo2)yFYKdWP9kQ*GA1QSvXE!`O=CvUoUq3!K z*9;gFrzu&q%Gs5{bD27UQH{lCcLHA~mPKAp?gE%tf!4@bwB;#2n*%dA$=Qy?^Y@I6 zZ||+oq&Up9PzpE&a+tK}bHuG~ZOL!teaO--`Jv}~R1bH2FN~7^i&{2y^<|S|VPRpM z4A(G`wvmxU*lVIIS6&nq@s_zPO+xwvLkd7bUyrp2v*^+0Wr!(ei~Z=s!wpRD%LJ0h zYmI!aE2G{_d*{)Ztx(y?Mk-&vufDsh213HZ&2gM&pV;BZKBc7K@VTvC_j9Ik_xtwb z`+BdItx}1D3Ebq%8vUR6?0*0R!zEW&%bMqBE(=i?G4}yBSai5Z)(AHRrv*+!F6wh= zq=zajXiJ%_969>!e1Ev8A{%_$&5f_ZW6QAZ9XkYT{|zjL)8NNx#6r$pxwERiPG(qT zHwdW#GW$a*slY73KQaCxHBqeE)6sH%$ltlgf^2MW05f%FY85#Jw;Q7SGqcW|CwT(`&)OUe(|Te+6cCkcBdePOl8 z^gh_mx9EChKh=FuK%7A0PAz`b>3aYLNz3=oj^If5^+@bi{U~f`hari;nF#eT$iO;%Cua%4PfBjB1tC{4f)g<$ok2?$0b{>*?`V7)X~qse7Pgwve>jbQETtwVhZ3R zb=(-&MZ;GRmgies|q7N9$C7S639+X%6_4$Ih~~cLl5f5PAS?-0FZ?vOPo~ z)O=rdI4lEF+1h-N?7py9k@*82o8hK@w2?*k7b37qY}rbsEZ1dQ*~I(z?}N>KQ~CNp zN~*~Sv&~I*f$Rcaq4iJ${EyFbn+XDaZENd7oWga}=E#?8Q;nQYyYE28J9BBJ=GV4} zJs9<-YO2iD_lb!E6`s7lhimu}DMbD?tLi26M|LvEj^E}4EJcktM0VA&z-4*`<7z*g z;WAf36b%i{eD_x|ef{g;LAi8F$o{^3o z=1VK`Jvk!v;~jOw^#hBW`u&?MTMp`C@z+-j0Kt15ZT0mM<|3!_aN0LbF75$*cy${f z%<>4Outzj}!{c0I=?7wBn*v|K>}ntI1(@VwoG z_%aDe1MDFg0B5-SS*S0eEKF!>l1fudCa4p^x&jfD0*RzFm&04wpX{(bvRpzbXj~8c$ zE4_yX3I9QNS+FZ!`*Z2)Oe7LvlzZwO>;;qShT!<7kTC%!ZK91J8Z=n(Gz$7G4RV*| z)0j=x1@ZCm*#S9$AOiTVCC!uCMVAC4?Nj}IeLH9)A0MB4i#$J3Pyu1nlYM@Sj8K8C z0xqzwNc4buruVY}SZt)6>hXFjD_N*SN0PAYy?ggQ%~)kR!u`p`ah%B;kOQdwRpl%3 z&dOM$KAS#3nRDkpL3~Eg((0;RLkM{-fF|o}AJXqEUz;N!K<^z%Rv9F}DbC&Q#|VA3 zm$UUf+9HK*pM~lRDdV7K$^%&AW+HO1$O&jj0XB7QeO)G9N*p=JBx_#V(a&H=UK{Pa z+NDlGqk2& zO$F{?_u!xlVCV7Sh9WB0q!rcc32fMpJ0qtDrx%E%Q>%HDgKaELFK6U2Y>RfvAV8yX)EX%~ylu?Dos!|la zB4lL2>6W*_O-)UW)UKPKTVh!;u<$OSz4Q90#huee?sQgz`9a`M>GKuziblOf3p-| zH@a>UFK@RVLB$TiuWWhz`)NBm6||wa$jRZx720RaBY;whM#vziwf~Vfyz2e}1lq2)M24ziv^>)a`{MfA^b!7rIWauJjNJjay?bLSYahF%$IP zum7bCxC)ZYaEW68a52OrB-`rbxcWf{0Z~zx!TlO~dbTVY82+>Dn+#4Xqr%Vq{UN{S zR+`GA%1TSuhaE8n9C5!GMgGrP|NI|moV~R*!T~_=9Kv&p$%J+S=Mp z@!X6A%gf8sBq3p8ba(Hb%NscCj;zD;#{pgj))W>NrkE=KzZZ`PVM&?Z=8$<$E)V8p zZ|CeRpMP(-8ru}%4pN=-t27kcUbDS^>vw@78ep-%m%-zg2SZB&Y2Nxl2cfXJkme zRh&^)Z|FXcONg!kgfl!c0^95{zw{pp*!T)LZBeY!WZM@&YiCV;!MIKH_Trk{1o`=e zdn&5_bI2T#A(2C^ckE4_f^5E6qt)5SE+}%woF>2UN{)Z^KS!}0`Om$y-T3MsSJ3sJ zKgP-bv&?S~S5PGXy9T0&kop_wqoQPPjG^3rJ~OwZ(U!u1ALkP}d}>gYB={vD%S$9A zuLi6XaxQRivuXUxIA1^3w)iOo!xb)^#DiU0}s4Op!m3% z6EoA}RM>1WbplE>#FizN$0I0h{$&Ej^f{BI_6vqxY8mF*_4IC9mx(JIJF56VniyKs@~5t4>*;8^rwP_@1{=cy9EloRM=gD0>>v5<=Qw-#DI zODE?U)Z&#T*M?{90ke%q;1%#yIXw@G!viOo$0kFC%r620x^hhxHVJ5Jy97w#Je%D< zefqQtd@$VCT4XuY1D}=YX@GMOPZl{UhC-Oro_-a;qxD^&VoJEeZX4yuo+_44=nqbH z9d(ZdCWKa_&}r_m*H#Y`d@A+ypPe=qfE{@+=o$t|43bTk;CrqJW-T7T3vyEpp?tuB z0X7!>s}?2=Bv4KX}~yUQ^PX9c7-*B_k!7A^s# zD#DE?m7F%$^o%(;UZoh$HDP@A>{+;X!blRPB z+4%(pwW6wg57EiM_OGr~k>1hF|0_|@HBB`$9zd1$V3Du?yLaz431T6+^{ZsrPo-H< zO3x<(LWGF$M%32_q7e$n4M0i6L`3(4h-nvlV|0kC`5k9ZU%zwb&gg>uz-~a^Qo6z! zu!^R)jHGz}HT-IjFDePI9IBM|u>qM>S2dIm@wg*Uoyty$twu$He4ETW6 z;{!3MjQIe$7g2xLEVzr(^J0&y4LCw(N4`K!OT}!d(uye#o-d=a*l1G zfXh!Yot+UQWjmM1$joDT9{x(`Edk4sYS`@6(y?=NG=l0CxWJlyk4HZ(8p5a=-?68x zZD)z~W!a+L){5px&FLKW)@gx{zc*HSWar?3cm3wg=TJR80t^dC-?FH1F%qTFRv&y~ z$VO&9m_NX|w)7{@MWh>mT|JbL_*L#Mi-J9ZJ^dsd)qr#$e2@16z@pXhrT25s-qFJ8Wk;I>o+c1sHU8wVGd7g@ID&xU^ef)x~8=GEO_yVR?EgfCKXeSj(z zVS%CI`;}{Qo=!38hob0FU4m_;^wxY2LK*_OKb;zD$ZBkgIX>riLu?d*u9e;g<6wVQ z1G)CQIik=k$RDM$k{xCsAA>;l^V<_y$btZ_RU(b%7Z=;W1p~vv46FT~0l=!B~!f=P3e8J~bss;4~Pa)(0f)dimn z+YPL*7*GRul+(D54hXBqPX0o{AJQPS6npMuZP|mZuvtjqEd~l&KISDPFbmzV$Uvqb@6UDMng0i^vn(8<^iTl<-otHZ;?{pIerd{4aJKtLZT-=+>_ z=AK&n{8)gx@UYG69h7?KNxa0GBa=cXxZ`!oTvERzGlUXG>Zvs+9zY(q8ZLeYNUy8R zRSz}l?Rwp|au4s8gjZm8wq7JQS=fhnptM6rM~9rt{JQhJ+vUqfrnRs1+^0hO_SPET z0){^2$qYRYXoNh&yEu;v`%Zc1=zvLM~Pito&6TYcscK|H?-Tjr5 z=f_tm|>Ot$J=QAMpVDwm4Rm{5Vwx z8imGppL`?B*3L7{ehkh6SXdDt1@QlZP??-TyspZHPslMJVjTgmKu_MpH?k~+Ikj%<`t8K+#j<| zJt}dy3VsQnSC4dz(!NmG5uCY^a7&T^D5xf&+{A&_BIjAeEw5Mr%ZAi21(YYi#!%?k zumfnOq@s%BvnPg1)iYzo-g$ej>snMZj%ZayadEL7bS+dcvJjmR-+)Z3Ua@Zm=!qow z3=qgJi~Ts&&>1k#tb)M#;??M6D{DGkF`jkA@E?N~xe*?A|Gk=?wG3oiKHp=c;FC`l zsn}aCtLNkAcZ41iB#xR!8pZ17<{)8DFPsd=Cm$c20ib1XZ~xI_>uIkMY#=aGV5}Fz zR;mBh0ua9j!!;meAScv1AO$XJG@5B{?*#ihOJ$3bc5Oh@*8%I}5Gio7xK`HO>n!53 zNT!)@UIWDnvdJPMRB&~42@PUAhQ1|^jk?$*5pTMhDf{g7O%J}zx*3Y7gB1-PI&rR#8Fl|74gV7n%-#ro57!Dmw~ ze*l!RDnf-iFQ_63H%WNxPtj>L5D75nPmv}|G!A{P-CZX-0fBfH9f7VQTN>z2w?LcW zKCp_`gXN~oTA!s1R5R2SfZN-fO*nIfMf-GAa~&9@dY-9uxnTEe5@wl1fr1i`t-BQ- z^$kBuof%(U^gNur`IQXynv_*Re_Yzo&=7$#IPZ9@UvUrg<(Uz}w>CmxfCf{MNR=rN zG%6YDIIu{cJri8$U4Af@R)qFhbES6`r_Hb%gvi`S&{#rRNOE#=x%TPCUPgw7zsF9f z6yEb0cr$-x7j|8yoN83Oe)n!Xgksc*XK!k!x$)dD69^bVaQd%{T&Z3W9HH_CLV&Xb zCz&3v5ql^uPS4NV-2R<;BQkQY!hM6T!gJ>agg5v=izH3xWI^BHB(|t84SnHaMSX^b z9{ba7g#18S&!R{9@m0tB*T&J6*!F~2;I$kkew-Qg_|B-L6fUaFo4Ntr6{0Z8pu|2P z1Ps{75UVAi*~Ws`Ap!PjkFN2=4{_i!UI6=uwi#iCSo9XCsd_E{LMfX>x^3pA7rkDF z#1y!Ji92Cb-fRi&oAW(UuybhpDLSZP&!YesJU|=P-%(xU_wqghuATSlT@{pJ)HrYk zb%3QQxW8?*vMzkpF5y5(usruAVG6CMjkVN|IjHa7Pq&_@sSA4&Kw#^zze#S|zJIsp zNKV=dn&oNxQ3rAFqd#|udLG&2nk14kYhJNk5m@t)pmm|ILQt*{zt)D$jT^iS>N(d3 za!ZIvTn^Av%gY1L`WKq=n`Tn;6||CZhQBC9b-&n9K|0AexT6NMZRC(FaO~!O(E|RA z1++9!z#Q-bZ&yREMI@O0PnY+0y&?M1r-$}X$3V=Y7ZBL4-2pa@!?2DGx;~Rfi3j5~ zxC_0X0U4YzEZPlWR0dzw^{o&#Y4wQUG*bW-2iyjWi9{Rm*iH=s{~ zHfdEA5_*lZicR*_G%9vxY@|eER2`Q_z<)gk>Xb#hxM^{~F{6=ssSFGdgeIcL{ffrZQ$x#XF1jcr z>;9aOXVI%{1K#K4q_rvbufO=DLCgW|$yC$!{<14Ywqw*Q6@=lfwI>NbHRWv*OUueO z_7|3>Dk>q)L08Y(hV>)6U%z{}mYRKIyJ_B*H?UFZ>S#k_Oj06PdjgPdNScbImw(zd!vO#EZMM^v=a4Tzb9 zeP}P&K^BC<6M!zZH`%Cgo3mhTrYWKpEKgvEMyD%WX8te_yOI`QBxRmRzsWfdMNxQ# zB!ZWd_2*v%oY<;m0DlQP8Q5u_zmO$kv3_B^*t-}S{vkE`Zi^jy2sVf~IY&yW291v{ zv22qXL-ZvgqNLvkZL_<+qTmV9!0}$9q-=wd%6@y1zc*t~@A)*iI21%z!AaaaJ&_6$ z!j`(tSfzjSl>@UAh$_-Sl171HKKGoQ^11EdJ3w`Th;bk;6SzXxM-W;K2u~&(OYBQ$ zKFk}kvviOMxwc-=Wv0bszI#o9XSq9!^|okQcuSJ7;6m@wkKvLHI~SKUXc|Pb>V-ky zD$i^0oMj1+%cs;=UdunVVF8Bh6WREhyY$i_@cd^26V;>!)OE{Ts8IejmsTF^byk*@ zHuP8=bOl(&kwV{5WZ(R@L`sBol*lciL*KQfel#v?HWqsJ9NbS;=$V*8;7f*vk5eMG z(m~tu*>lHcO?Rx&+Ozz0j^20e|{qaD#Va5zYF=5rXuF<8pM*foMHG==;4pJn*L}tWdY<4WdTTSW z$_O_LB_A~FS>DwR$Dp07;I=5)B|HZGJ*)j?BxdO#7v~&P(Nvf;A+j6gmWMBNoYFLq@vKhin zD{EDoKYr)pz*Otr1{Pr~E|ocN0M&3gqX;s}Fv#S9(KscIk%y*kROk)$$}0>+TKoT_km!E^FE>gDJyi%-4ea% zxm`!+SLVC`fp!8qM-9*bf&TW-JTq6Q4}s`CV|5Mt;x;d}{Vz{#tEJ?6Luef{sAL`D z0taFM^&E7K2Zk}-xspeq--BQR2*XCW3gBaAV-#Smt+HkkdwZcn&byYXYGs8t5O~Gl zx;6i#A^hYW6dXTuCKv${%al(6h4)U5j!yCTxk6&;`0Q*p(OOy-0?PKTr~Uw}Lp6j2fC#S)?LK%1(X(giU({A;0SjxC z?`kD11eHY!z%0#wykx&xwmcCpm~OfF#yjajRipR)0=ag z<(rR3RJO2C0_2-_QW6pplCU1fKpi{@<)@Il!^TW(7eK1MI5qQ`snOR=54oa_srJD*#7xH5?fRE81QZFqjVTXMDK+Lu? z4?Bb~Bm&Os1^_TlqC2;TG)?6s#1Evu!i}grjEAj^90? zk>2nbOgV;bE-a?5b-|s4=^X~}IEX<@x7-~5t{!jR`OO7|>qn5t^(HHU`s|usJj8#Z zy=+0J-w-4JoFrYd;G#ep zp`bWP2Vn%TE+C+~0&K(S>ZS-9G{hsM6hP>>{khwglZ><^%$jnbm8gB?jjG;VyKLV5 z<*bNE;y{i8F2v+dYT2V;c#%)O`^XelLVt_Xby)~vCFmy8tTb4E)Yh^eo@9Cy)+G+E z$=k?fYnMEL`V0Z%6>PE!AT(i%lUrMsfh-gmT%%gpu>wSFiT42y+}$lczITM8ltEP| zLru7A`li^;V?iINC(IU3KU?qW*YaYuMOSdvD$qL;JCBUS_?3BP+p$$ zCvRyRxx1Txh4qD zLGHpp51?(d;s7;Co9pdu)MyL!Wv^McNb_LpUsDmkjx%-2!hl(82KWYK3Er>DxgYOwERI$*E?Xl2S!y0p)j|gE{Hja} zi@%?)po6Rpq-Q2|frR|dcmD#fKma^I8jpa4k3CsS@}TsZ@@}|9O#GlPYhRW9`yAmxl1P6TG`22ykhkA3k)=QFSP9 zF>O^K#OL136%ZKI(IByF{FBOrb~m)b5vo-GSl8f4(rMk1CFdG zO;-B!@GS5mHJ&RM3+KFpX10Ib?dWdF;Fl7{RY#y(T^JOXuc5mGJ2^)s6R!be9I_!$ zD9OEJ6B$K=F#~Dcmu8k_Dgm?vuu`cmzOPIrR^guR`|qO`2R6ozIG#7<)dBD8vBR%F za%#UVE^qE?B(mrIcL1Xp)Jm|~Ptx-V+|7KaB(z3Vtte*NUWo*2so6=;mPXQ!L7}3M ztqTMr-O*(O3ua*PqnR{_d3kw3%_O6*Kkm!3$B4lLK;(1eEUciQz-6(1_YaL9+2pGW z0xI)?Tv`yU!4?t&x`+c@91N^HYlRrXEH!g+$k?!^)Wvl&JYoG)-P1#+f*L*|T=aNa zG~)2gUl%XV04|VQVSAVXMQ#w(qdq>KZx64!OAMD@eH9gTi&>M4{`Tz`;Czz{+1PYf zb>uoX<6=HS5BlsVju*7sK;oKA%`Q9(eP58Zb9b4H{iny1r1DR5M!?7eABd*^eQkgI zn~6uZ?9WSzF2ej&zdqZg%a=E}jHKVx8#X%?>HMby)35x`^>QCxt_Gm<`}gm3a@psQ z=GZ8lQ*!^k67o~Z@ShEcpdDm{O$64+f56Hoeyz@kCjn>!0%*C-k@)}bN1F2fZMkVY zKdbLw;|HxEb1ny9;Q&G(snFM0@i;=dgob=@r+J+*hsmH;&VQHt@ZaT%fj)`pELD!W zp&kJtAr@FBDDs61j*3GiZoyGP9YaM*8ry%5b7K6z3l#gG1+G?|b=y1Mkqr8VFFfB)t}M@L88#$3by9_r1TC+q~Eu~J+i`{!cB zTaY0jeRK(4`1{71Uw{rU+}~f!Bfvmy@Ra7iU;5|g?Wjkm{`lzs`R)Ioesio0k@k7G zV&f*kt@{cJVT(gp$%vaTl@CWj5(?ac-7F@#u{W~{S`SZw-;n_)hXfY>7WnlG)Q8jw zJVz~%+3)CW8S;?#4;{b z*7t*KY=^}!5NZm$PrUr5s9l6u{pV5FG3Hs2K1!{y5l>G~H)ZJv-q9|8^m=Ro6psfh zgeGA42+vL9tKAXJpojDoLK+%H9bmSj{9ySW=+0^hMZE$70veIK0f`3YcR#vT*>K>*cC)m3Cg@;dL9wCUih#H>FOkScS|eqyDL@j#-3Y~{+Y=Z|Xm;I< z)ZqJ!3>qnR0jm##8h0ydbAW*b5%`G3XgT?q_l_{kLvTWI{|vesO5ao%N?lhJV4KLe zehUK-)EMh4cjwe~`*=4|uu2>>1RyeyZq)sJ5v~DfKqE(A$s-_gHfg4EFt>MmagCO3 zwHed;V$fr61`isK8#`n8$;Cd1ok~*y>kIOE0pt#VRw%O3u^7xh3&L6xP=_LM6ewdv z$297(zzl80`kx!>VTzV?KJpp7$LAzy2 z=ci8Tm9|vb8i4NXK8NbB2x&S3iD)##MR$fBPoI824Xpee(Y7*djWlpj&~-!-9JJz| zwrU>iFYHz{?#I;(Gq{Hk3UNy`bL8biWR)8SXKeE`r#HTg3+j-Cb z59QAQ(F3@y!3&!9W{S+ZYI ze7HdR(a$J)l4NI$gPyg9<#`e&xxB(smnECnQt@B@z4=Xf-yy%lF~(L`SG)D4LMyLf zL|7118Il5$yH-jN2ac6VShTAFwjglu(n075v@SZ~fE{MrU?@UzZ@u*^p!NYxAK<*8 zQqe&QXK(~?42t99V^E3zs`AwX)`HGOFd9s7L6^72NBno01y#$ zT{d1GcE7i%lK{{<7>e|X`S})LeMLzu4XtqzWl_=S{DW*=G>rfVr`f!A*`Sm*2EYdW zcy{Z-3rN>V^tkvrv|EU;IK@xY26TeApg_3%1KMW42Mex2wgDAyf|{;7IX%6Zm$!Eu zpfJeDJoT2X>RFlvuc2!IRS7AL58rDT9iZob1#Ca8WgK8O5C_U!eZX7Hg7))W`|NG( zj~I738 zSE}@K2i@LfZOd?v&2l*#X1d#G=L%rk5Dhp`V;R=MgD1$q5_02MYyilYsj2IciXQ6z zk3a$B1GNddhcy@-*Z@psg948E$oVf>G?fzU71nZ~D zC#W`OpZWvzEu>V z5}=u_{+`W43*{8dxV=YeRA>NNy6frb&G&9KKoJUTB8UOyps+(Eo={wWQq3HM?r<#N zz%^iwa)7P1^aL$dx@7qB71m{>dx8j3p{Y`pvef>+I`h*aB5nFUt2P_NX z_i!+*M;Yqu;3E&9m59`!aF?bXhp*u-a-8Of5kdy4>e5 zib;2Bt7WJM-I5Gb&z2`nIIlI!UQ!}uV|9Joe-cFUNGqH8l3gTJRE{t*VBDCcRg~48 zxgZZs_{=^7ke4yT@tby~wV&O-I=BLu zA@CV@eyaa!Ba1Qi|bx^dR&Y zg3x$(l?Wn06eRn`N!bi*KTsr$FD$e|3mH+6&$i8hIwQ(uNf!$@0s;rBygi(cAB*_h zIo?1Z!&%+K>g52L3+30tk@0|Rj~LIA9%+9iKM$0z-Qn(80KtgH54sPjUV9FZ7GS=#8G_RFF5x@b`PNzeg!+1XVGOU5kjT_qn6M5tByqX zj8DN_?kDd9-re6Rok*3hX}dCNs|;NkiO~H(AQR|zi##D?m2du%g-JX_=>YqR07ahM<62p>Sz8iQ*54VWR6w`p)Vfj0zRt%$l~+r7ab^(V!_jz{Si|F z<8*{mjlOc2B-_iNooS2fg7_jGI`S*E736c{1%0}>Ag{53R#9*Kp?7{Z3ZyU}V73ir zkb+=J6OCw+`*Il)1A4yu&c0Qn&^SQ|uxq?FH@P-R1YH(CnYL>}SOkPp4_xqN9m{5E zBrVY*nTYSncU72@WVg~K8F0M3?z`3BjEA1~bwmUK~zBW0|kj_QmmUjkE7 zaG@JqSg8tgf*_&UgV`l(&A$UmPC#_X%S`O0Iyy$ExmzKA?=F|EDEBl**O^)#Nhgjq zOV4|GMvk!;X3dFsdnm)fB1athuaXn&>PfZ)3N zX~Ftd-^SO>Ou%04UNA;p7G{loe!c_y44+!R_EqSWt5?^sq9>_OLL$P1jhpmg82@y& zG<~Afg9p&Y+wuMK^{YW|1}W-Gb~gTp4?~4K7>{Q+|@-;47Z$tGoLOcL1tr*uir5)S2;aWyOh#o#RGMjF7mgDIMyT#06|x zg3Se6BaS#SQ6FtL>li6(BH0TOgExB%p;B?}&$qY?Mm7$!ZuM*-Iw=CTupQWqwQ5{K zhx)6robKzL?>*+f4!e#mcNTgke}`tm$?=Mr(C)YxjM0Yu`t=xw;-g4>FU-jaatTg_ zSX?0Ih^{(XT0t?X=b+NNYJPWqv{qJDM#hLXX1zz|(dfWl(y|9ySVDM!08K8CNOu_- z-S=ifCzBbONE?CgvJhV|w9_wQXCC!7TMVav)_f1FoQRaMf|Y=N`Jpc@r}7)I3{ z_YODmAZ_4+v0U+K4~rJ_8#`Kd(ef7tT>xlblfg_viERTzzSIRVY}L`32SKW zfVT6ZUc$qt!Rm&A-6ZJ38NrqER;Za6?%th*9j{}%pujh=`=w-K&r4{%DdljI^rTTq zlB$^@{rwLyLgC@%5=|x&pZzXb_sz~m7`_)aF)?XNJg^8}$mz%z%j;4U4TWbOyoxQ@ zpG~l}=0?I4EYzlT-?J+fl$3~u zifplfH}zqJ2LDf@JFgd}whas)|9BrKKk@7?%dr!NIES9vxTh&ps8g-ze= z-QbLq({>#x;7e4Xb{oI{!EtFMqTg`D_jBK%)J@^B(n(=Din5Uo$ljN?wj>{xCiTI{ zI{(0&)1;F?ZIOG$Xc7UdY3J)|jL5;T2&wOV<$J+R_nGL^MM;8F>(6{?oHE#&=boOq z_;iCEM9^+R)RYAHmr9%Bp++}qq?UR7n8U*2Yh7a_?#JLGsN~jQbYl&?-W{9hs$TBa zoMnX7hrVZ#%@MUH@6Cp?9rVUJ-WoXY(msDUw`C=vqVSAEiQmYX@nSwfts>Pzbl;od z;b+inT&~!mzHsYGK+jX!8#itszMb0GwNF?V;ZR%@5^}+3JFj(jWW@$sJQX&T?R&%t z6eyvZ-jOBr6|Rihz}Q~@(zcyY#NekC_Is^`zvZ+%Dnvj>bYE9jSFS`0Jt-)%XUsw< zvWxQF?-GuF`?kO{i=Y3P~cT3fT97&>?E+z(7uX{i86#yaYP0+b&; zsz%I-xoEiMi_)z)n48b8*cJeh=j2)=4ZdOP%BcNPxOQNm(+dMK=CqOvc>ZJ(Irz7T zBPze>RZ%jL$Obzr3sV!0`Buf;_jefbk^{dAw){9feOp0b?sh|%aCBYWZMfkdDFVpT z1E2*O$GyN$!nFHpXVjBb=%sm;&k-niGW4pj@tF~#Wh>r`a2+V5f8OaG9@d5#WJxwM z4`!=M0!wpq)Pp{Qp`s$u1f>mZMcHy`UOiSgy^CDA96g(xiK#+P034sCVw3)GE!-8z zgDc?eHy|zI_-uV=)PTOMTUmLz(4vbcUy_t{XD8VQ6D_AS<_cXJ!0)%A5=tBP-WidZ z?Kq0|T&V~+ST0LmUtf2@6zEl1Q4&Z7FO*g|!}dRpEZLve+vqp)J*yW0VQdzoS3TV< ziA>jr3Cx?rK~pY!QeDktb$O|tReSDSA&NP>w$N&T*yp&XuP+a#-3UYv+29vHh$LS~ z=z}Q#c}keTYu>R#UU=F7pP~>^e5p{(VWBHkZ(>QHc)s>ME37LFeR04OB3LSS=_e+e zDwB-z`vvk&D)cOG1#NmQyxw_d9)Cj69nnWaG$-)N;Dx9s&goOnMBH@-wNay<0>9NI z|7v`2MxTh^d|-(O8diuGCA{ie2<3}R-K+gxJ24K~Y7*4r*b=O%(&NWJQo@&yCG4(W z8ZTYQ!mid58V%-K47=)?FGWItWcISk8yKGK@8qJn_8~1TCO_ZlI@cB^N&H4EBTh0V zpl<}(-IX?rd?tEi~t0+^?upm5vl4A3aF z;?FK$f9h6t_Hsj!`eok7T25TAOxvwH2MRa$4&5q7-#Hd}AGoxcI(`rCs=)NjIN*NV z%Gf@<<$W7HtKSfEO}*eLHwc@o?GJ#$DOqn}bX#vu|IsDts>t7g1GXW1l{tcbxlICvNSo)`ky`*ltKo z=$-7xANf#`Fa&rqva*^rgv3G00p*ixS#MHB_)M$X>i&Z5i=u0Q5rVnxZxx738krYn z_ZjGB6DDIw9hafK_iVr_?d#W>FP|zf+&6Bh9IpI!R%hZbUM{uN$eTS-?bAs1-X@RL zJ3gP}KC6)?$Nvh-PocD_LfV*0KC_MkW}C9(eew)=EI?PImDc7$&aB0Us$&lkXhH&{ zYD?JRPEYbUN8@{V_GsT^W5ktFaHR3m%%=f(ZbBUQcSqk3W5xED)pWKN?r4d3h-YAn zVCi@SludislRM=k9vRsFfqsFqlZqhd6?SxvCUc_45mAJ{XpQ99h7_#RXiXWWGotz{F+trlo zD^E9HbK!|a&*sD#kg2FFR=TAJi`xgE28T8L94(Qwl#Occ zp2V^nNkk>}uG(+Q?q@>YVwBTJ) zQBfgf3^blW?tdD&_3mz!4$%`yNSqKi`6pgn&QWgz;-kf7r{~b!7Goj3bA@@dW0s?r zl85txe@n~At+-^*W%mw&1g(&vs6{zu9bx|@X}qsUQaN7<7v9FIhU*otYUSqU zl5v?|0=fKoR?#l<&izk)3zlf=HEDYJrB{=-R+V7p@JDRZt#BBqv_-uW^~DLGbodKd z4ot#K?i~DM>ri!S&fMUIPR$Ivar2-N=A3@ij{zx?14PfNvxVd_$zTGM$%}RqMU25!A5P^2Qr02C zaiwCX1O&$9!X8`<3=G=7-sVnoz&2dlS~D@3Db{B3*-Y7xl9ry0R!-ya+Fe~e3JMB( z?>H8w z!0feFPpaf-1_k@k_DQ2GYde(Gx*8s%mC zyD8@U0EmX+aTr2-)5_7&(fJiYEt*N&t z4^L%cU}T({4E9Av;QfB`Cc=PL^Gxi?%|rZm9oy*2=<`JUt0mej^YimRQoI2cKttdY z)W-ERb>a^{S(uxTAMIe-j2bS$?6tIv%bAFJD8$n_X#%3LYK;0E?iNPaWPU2DJseymlu7K#v*)k1mObjBGQB)#?4X8w)!A zX8PoZaH-bVY)B@g#Y=@+dyS&U9U$*VsteP0kEA?|Nbr!&fli(G-jR>4_ZBw-Zr<_P z--Jo4wl+CX#gcbC`Rpc`&}z%_%?MvXb-d3F)o%Pvk)pO{1QU&TPapu>FHbuekM5ZLj z#C^ruXAu$l$&F2rI(qs5m;!6?q6WF3 zZ2^hg=fXe|F(yNcRtPu_Kv6*P35euOSGr#C*b4ql>@rw!2%xqeHN7F{3`wLU2MR8n zfrZ7!aU=m;Fr1vzW}N24?Nma03p^5Spx9p7?P+v{cx#PCdZG-F4Sm{<@@_K3eqc&e z=Sg0?eh>660}xC!t|QK_-Ch;P``3m&7Hpf_+h^dZESAo``IY?%FtaGcQatVjvVN+{ zB|tpW5Kbl&Y#;nToV|5aRBiM&j0H-HNJ#ofDM(4TL5HL?3?bd!Gbjp53P`tf4h>R+ zB8+tB5Yin(4orQ=Cw}kqyx;qNe|%@HS+iKm;hg)v_ukj8YeW2|lVv88J^pw=)UcjP zes&@1UJXR?yKSFftP4)?{vl!eM z59EcyfaAPYc_C5mAF%_&tU>$JP8qXJXHra>WiwCtRh&SvCy zK+7wZq4NOI0Vw7+JOSpKh9+|KQ6Dr#+*1K%A1}B;eOJHZ`w%MzhG>VUoxFf`GAKX+n0V2TFW)nMxn@{?+jY2+{mji1)mI zSK25k?zIgTemLRLkUnK|Z9Ap$Tn`!^%k}#B`~I!R6hGjSez=Pb;N()Rt*xExB&#bZ zC`88P5=8=8B6q+>dGzsAz<&AXY^iTZn0F8WN+ZT|P@Hf3ra9%7Oi4)z2EFL&wp~)x zm_iSdyNn47BP^f1=Gedc##`jU1Ip<=k)tC*Edx4qV@qI5LVH(02vJ>l7T4; zARe~4rP-_|FMngo)c0CZrVqVscCd)Qf2fkWdI1n$IcPWT0dzTe^G?<5EIsZxyD?BA z3j0Mga|Z|{SpXy+z|BsofPAfvLDP#a5are@T_8yl+70jvsyzFabj?NOW+%TN;7a%d zA7Ew8mVhGX?XBBq=?h+!Kuy{cU?KH1!v8GrZR-aPhT5}lG(enf0$A8bo%|_+w&VZ^ z*xJ$zcn@t)n;QFLSF~-*L_=)pH!)IX7Ws?SoP_jrRhRvzKxN2F2L~=7L3{%U2^eXF zvp*PpOWg6mLlekv0t{bo9ieS8=KRxROCK0D2}w7HfJ9Wl1k3NbJ-Pf1pe6rzDL{X! z=(dDfek2*t8*b4V_B-wxC~-F~G^THDxuy@?%j7%v(%$|N9(y2(Ie^fo{PCbIa`Nv| zpExsh$G?irKPdT%-<~26IrjoeG5&px3InV2PkGUw-{Mb;34mIHe_b9Snpgf?!Lv$` z3!tliUq7$>_Kp92#a(*`yz751qMMEy|J01#Q6D*E2$1-T+K5t|fW4xrvFOF72mSv> zeer+#;}#Va6?7}?XLxvcDs*CN zuZ~B5ZBPyU??1*3x>xwOj>S&U|9?FCKkpRxb>1aL7AFQoI0&YJ<}`paggP-Xk*7iO zmjXe0xy>?R6F#pVyH3572nSMXNS|@%f^^7=OUVHdZq)UgWk-$z+D;zou>JD zzph%&x%fspqfMxPq%kTkFQravGhI)^_#Ks%7{6j}UzC<%+W4T`%1)*m)Lc)*x|ccI zn+@w$JQ5IJa!zZQ(z=*96Jc4Tx`-Z9j}mYHpcaU6s%ulO!+GcIj158QW*hb4e^w9v z{T8D!fQ=>(?;P3o3Ge*o4rhV`Y)=gB6+_e^I)x|j<%Sg=hx@-ysho)GnilmJ?+BAl!fiz+0@s;D2Y;Oe~Dpi{9;=)A_Kxl#ajzClxzh?@KHA@(2% zlXf4ythEFp%YFkZ{MejWAVfXk`>oApH?}=kz3{_K%AfhT`OHhhE-aYq|oI z;0IAhr|i)%vTEBsF75`c%<~Dz^=7-K5(Op+I=rcuJa)WJMlY&s;f{EIA<-afeL(xG z?C1w0h;9A4@ej1s7D(C}a+2MS!MxqvBWaWTvxeyJ(-L)N_{#12co`>IcG|4!s#GJ2h2NV2QYjPvk=uWy5 z+vD)X-j=mqRpt9Rr^etPV9lN7jlo9I+u17|$PZ52`pG(I(??Z~LaP)tC5F1sJG*;} zFCsaNz||>kOldJznh!YA@44Fdp{(wNq!qB|1~JW0z(27$rM|>?kC0@<9ns1TtB-17 z`3;||niXZGFn~>tFBJPU@^K`;=rh9{pA)XJtXd7ZFU?JabIZ&}SlK-pqy0kV$4P zvc#Ab)#aRbRO`GH{9}Wi(&>cC>BP#tD?v;ky}LsAHb1b1p zJKPBKuXg)Uzp*4Zs$8h-(jHBFb}?kK$9Dfp4) zGaWAhoXpMYI++yWxqn0w%3`OC7E_L zoAVd$v91k)4)QFlsFB!tgEuILaFoYXR`?rkELrcR$38-PLm$~a(4=*+ zN|<@)X;AEz;w2}7Gvc!ePi%VQf+=H#3NY5=8J`Y8&yE_Qdkg|N-k;h3((p>IBi1*} zr*iEHK0}&sK;9GGq;4Z{QbtbK-nluBE>3BoR{P{X$iG}eRmIWup2RSW@99?HMJ{Ek zEd`mna=JMypb}iHA5G?qF}{euDTFF9o%9%YX*x_@aj5OEQjO%)Jjio-bq|@yyL;~? zX2PtMeS}1JU5q4EmVRM9nBBD;eyGT+Z8V>2F%_rKImtV(Jj~E>NL1bAl%FkdJurzX zVdiccy}v4%Q^_|0Fc_}{m?ckpP6&FoJ#P6R0`nIa@n1`RLc_#MAWG|f&M59s zW5_RAe!cCD8`5{k`SJenYzD5^^!<}-L9dvX8F?S?&yP*hUtyJEa?crwW&aj7WBK6n zdY2a6FX)-2Y7DE4XdLQI?p4hkk%sTlOC;J@XDx(pH~-s)3(Vuev#E!|UB*gTDev2| z1*H;s-bIuR#5NZEh&u^&x_P}ML!XX!|EiEn`FC-tdy19mg_(M4HdI-rd15W;6VwuX zwe!!9KI3@y1ra;~2k_S-ar_e0owPq%YTwLK7hLy^4v4v&raPK49JxHmy78-fAkmoy z^2bRK|IIt3gaVcf)1-0-vccGk`l7R=sf0bm-o>8;3V+M>)^jE<0z~|ZlDG^l|3kc& zqu*-!63J~RV)K|LT}hIS&sr=qu4 z?3*Oo7Z*O5SL*rdOz*5?M{;wQw+X6Jip>m0zYLS-zD_nn01IvZlEY9Fj0;9eU8g1j1eBMFnyQ)87HXQT-{$v-8<>c^%_5*_B%4B!63|cThbi`mIF++) zeE=H}vM#>}Nv~&9E_LzPP;i;``Wy4;tAxrG=c)Yh8;rpxR66=h&Y5yuwq-Is7ZnNB)t* z?Pbq-RGIXWR@QBLEW;GvA19uzH>OhR4rNs_`y*9ZJoR+#_c_vUX*`1~K4xgY|BA%t z7xdV2$ivdzz>1m7q^&rm`PG`az!GE9+Zp>mG#Bf;PS2JjlVCQ{7Zc;qvUfReScjt7 zM`F4tgUJ@mMj)&w*=CfWcN@u+H?@JC%;5|XUwDpfCQg&!m%y6a{n>~74~(g_{0)(} zyG;;d?q1xHV+!4=p(VzKhJ5Nnaq-v#Mq+i5?w9OB8yy^`~lJBe{aMm{*btX zPP4I8s!n0=%FmnhpIaL$i#k7rPF%tL=RLMm$s-=+{HF~>j+~I|Z%^tU8DzY#OR#@G zVM1+=fOfqp6e#9wFN8K~fMP-adV0Blq*67eRp#s%PdToIctS3j`B^LX*Jk`zYX{VN z9C!HNcFP>_q2sk~-_*ujUh;mZL>nZmC`n0C8JC7WO<>Ugdq>vW2CAa#dg|aqdqRfL znE&3s4Xa1+Qj(p**eIxv0sZuwyKDOq$6Iyqp9|Cb?2)w!l~=lttpldsj4?Dpz)*dhm#;to7NAe6Hy3Ve zG35337NE$5oQl8K))R)5lPFl$>}7RM)fR-6UPyk2rSH+0P(ArDqUGH{=y%o6t=5P0)SYx3k_!K;HlQiLD{M6)?{|!b{Tm5#Iru%^6M}|w)edRiYY78tfN+qr3=4~} zN;$h)frSCvaGf4lVm@{xkzo_(>t9f$WlXJhixcNJ1z8TqVmF%PmGAl!aybpRfCIVu z2s?8z$Z?ZSER4-U)qPl3JDf*63x(tD*Yk|@)?GP{zbSHgOXRY;X<0cVMSW$G{w5t8 z;4TFET#ZgIyD4Va>O6GH!U9^cAW~d|`=c{h=x27z`leYG%=p8u1JB5KjZ* zkFr0%AKnw%EAHB@vK(^LrhV+_&!s0=BFhM9K$QtvgcUDNVGcsOTI8-uMpHx$|1>NT zZTx(1)TA|LTIT8Ki1@T1<9Nm6&aAy3Sxfoup43_2FH;7ghR4a*0j#C=jOtCZ<{N2k z{P7gpZrVUT2qFp?zM)C_t7N$8m4kKMgo)a3cDsMBIBXs&v3|bcfSfjDM?cxh?;2n{ zx?cR~)rX{O-EsuPEN3 z_ht%*m42+Dcz$EPoFvSu=Dj6wx`8FX6>v?c914_GJ?3RNd(1hRKfGcRUU)?lw?%`Y z&i%5V_4NwB8b>_mhPirl(Iw(RT7(r}jKVBLf3@hp_`jjH3&~nf!BJJvMOA#R)mR#Z zQlUK~<-|GgEyM>nJ#rVwE&@ELi*Vz^S9K)Abq*T7^_rZqW7ma!50w85mQ|7U+hI6^~rr3cE>kw$~i;T|qUCs#8oJcp!2ej|| zvxBm;wX+Vf5_IoVmQBh@ zR+W9Q6^Qe(etLeV~@ly4CZ{6Ycc^%vCN$OrYMKQ z;R>H7L-S%tGen0(Ry;6Ay_uoU8sEa!T^hKZ}Wl+lmi6!(WW zi=k<#*Ey_`s-h1(Y`HiEGfs(ZQ z{dGTXnnos&KPxVc<1sk{5EKz4L|&USpiqxB!OTa_*l=L$zFlX)3MC4eUnk{PqXW23 z0>tis>WK~p33pRpYHRf0(0Qlo1?bm^yZ0=!E(94sqDd^!t!yv zTl7Fj(sBo*ZSHpZb5hXi%1PLN{8OdH;icXAmp}^Gy|jp(lFI-hx?F^#{%6bUf5=Ql z!)8M{VI1ozQ6bOve66ozpE15jaEnpAJZVmUDJx(%s}~8N9Cu3& zz_M^`Z8DAy(LKjJ6;-&eG!TF1ZPxJS@ufGlkaKMC>%ej>Q~;KC>7966$PI{RvCUSC zS)#r?=lf3x<--YCA&`U5bLp?ImhEu*{<`t@393b)ye-CFIKg}A=G$nnPkWm?j7guI zl%^;;N{~pW@rh8{+-p%QRbTTVFKvAAjru;{Gq-pDM4J77Wuz@B)R;) zA9aLPN}%%wQk9LddL-iVIRLiv2%D90gM-u0l%<(^pK;pJVZdix z#C`NBVp8uvUzhB;bw-0YSwFj2Y?MSDBX;~y5w{p+gbD_n13WQ*E##;S^_){t){6%u3v;=T!HH=lb z81h`k(*ItSxU-VKb2mg4piGg^xmXh(7w!s|W!hLCpt~LUJz8VcTEl5nVjKaAR@j+E@PrCmmvBR`jVyhQFwTSvt(JC-*OvS`Lv1raL6CnJ7Vac zj4$buZ*<&)VB)(ApNg$h!L2>e?BVYqQ2WE<6QcAKSi@Z^j>0JAjfk#2se&fh$-zN&HZO9N;jX1n~D;3mr-9s>g61_-UrsBUjaA1ED785uo z*w+b<6~^uFgZ|q18rT0hRFlUnuHAd2>WIMErI{#}cr^uWM*S?A&#>0EtfF=Ap^1mk43YREl;o;`s5s1-k%={8zggY7xh+#?oIPb9j4FVY&jZVyvDPifkB|=jH zHD62GcDsU7Od$Go&Tv;x=!vVheWd3TQq)RcnL&F=JTf%l)^)N4!=_LsYG&rvpddAP zEE{m_aRrG^T&1jh+;7qdSzetk&>*Bv4(j_&&Ay|)^^hw09wKY*xZtN$vYPjA_H&>P zzCXtuodqzu)5I{RD)?6>M73To*tv;I#e%hEWA0_rj9mz$5qOcR z2e1eWtjTnRO^wd-l?P|AjL&|95?^FN6_2bH{aNAB18kDwGV7?87RX=W*Atqemu*!O z&MaVz0)^bD``=#w&u5Of^R=aHIH}re%%|4`gEC)aJ(ClGx3V-riX7G4L5&DpGm;V+ zv0ura0xBFu+-F`aY8s6EJV!XvWheK$4l>3dEZ7%Za*Mxr@-Z9#JQ{d~y87*Kg=)E? zKh6F6Eu{IpSA1@xAtof-dDe zMP-)%mj<-b=7j~1D9-L~LRgW4OTV<-EWZ0oFi`ASL|&MapGA>;H!hQL=+#(QgBUpf zOn}s&r6Q?()sI(FAQn(TrEV%Y6hKk;n_Fse|IR>@m(KUm%u|2-Z{B5_X>S z;J_x(|C(ekgZR$R3_VAJ^Vj&lLQ7jbXS`<(uWs*G{4a4Co5JQy+#M$1pvsV_W+`jX z)j@2oG-Eq!7*TR=qX;RL;DZF)N=J(7j;fMew zt%<3@&7G#Bd5XTj{VBgm4z$!!{JU7Z9r|D&BWLyGnORaow7hpvwFf7toS52xsL;2S zU&2}YW!W;H+&(0=MFH)jr}5V?B00G)I)7Ws*L>0HS8&3j*>@YGOC?xb2Nnl!tvs2j zC3H$5y)VRmlr*ZT^_X+u;}py5?y){M@$xzRu{-Hja>ohev9+=NH3dnyV10RTeCl2J z*c3kGdkrCtYx!!Ac)H(N-hi6)V{V5lP>7LT=KMfcZd`uY6^2L$->wb2V}U>x+4#G@ zcD=jK*15uNp@i3?=50mzrM;qFCub(R5LcwOHnJ5(ZxgblN9@q0F>!T>3f(Yr4(wf; zahlfZi_E6sU;E@mXw4xP&G*Lwy*qwBGut4;`!xi2w0J6YxNgMgZo?~WaXUe==$d(L z7`dJq16P3lg#WGUh<$Nh#htnCS^S;l=MeH~v&m_P>uzjqe?b1yr*~5SE;sNP4p{FT zSfcz&Fg8RVhOk(fi51Z?n*ZTU8@=9waab^}3!Q48>t%nE zKHecmXv?mtHzxN&hXCl|X%#MLMcH3Ls4!T@lnkzHM8J3-NDIUODC4%Ru+@_{tb?Nu zCeKI*d<4KsPq;<&-2o+ki;j@G-i(7>!$Q;Mc_d|Sra*xCgRPJ5<$UE!QxM~~J+FtS z>jJZTcqNM%vj!AZBU5p)>}rfjD&GbrE{i0ZmgeFJO#F7AW?i@m3TByfua0c(mfL@+ zDu1(K%Xp9XD&nz?%jL*JJ=Zdu?u0(S!-0A!@aYmy;YSyZ{6AeLj@F~Ui3 zP>jS_mEq@rTYMs;M#jK%<;3tUjmH+Y-c(_fft2;l3$!|T21&7xu}Rd&7{zo$SX)>r zAq%XG5wibrt-1YFam=4$jc1zQ5&`ANvRZa5dquCHzZf(sz@O&Dsjms4re&P~l%qyf25DV>JH? zIL~@)kCR*+u84kzW&0s)=L4js(%{Of2<-b`|8fj!75k1hRWLw{a*`gbi*9QlVv<2uq9ue6@)vVbuFwq$B7_pk8ePnjwnuw^#$LAsYZ zt^8k2#A77!_a{SO?~csH=COh+M)`|S8tK5i`TJb7OXTw5803r1c{%>|9ISj{$+Er9a6 z7SlnX03-d@cKKdt4 z{g`hfITGPmA z42ggBboL&C0R%cfS%xj#rz1U@JBF-fYsmB?IT^=*B}GFHAozpwN)+1McY6={Y|#%a z7l7UFK{+~Z`)%gJj>lA?>+zA5z<`E@&7>8tjU6ktY8*C~hX}oqywE>8cEw9hOY4?M zdSZ7sM{jShvWCXa)~EbXV3O5WWT9iT^I0`eZJp< zdyEZa9!Sp5fL&Y9Kw;zz{YzAEZ-hWV+9m!?S=r@5GN>QEf^_?K!`55tbzAr){@Wg$ zHoK1p)|PMqg7mFg#cXc5&5mbgLjx{RK;E2|2iFb;x6OacT2$q0d5CFmHUDM_hQP6{ zxb<3~K#4iVsWA(Tb!gZ)g-^R%Z7#}&1Fcz6f`b{v9djD(Y_6KTR^3qq&HKkXuHyLi z%8DI0AIzjfUREY~bv0iP!9qIa8r-w6xAD^Jcp{mk2^D`#kRp1(aOY0O(%uBPdTS5L zmoBpAx$i&b#@S3zHREL3;M#^i$DQL1yy1&PtXrEqs;;q6p5Ubm%B@pJNH@$Eos^U) z;Ic~xRF0>`XBMczE*qm*AUA){bZy?pmPmqF)_#o@5qE$fr(}vu*lY9*^r^D0l(yM{ zMyU47OG2>csq4jt4#n|F0v|BsQ}sh+?Qtw8%L9PrcpJ+c8@`d{pL#*kenX8ha5oOK ze3Wh8h`1P-fUO;Y%zA$)uK+mb)9Oj8~+Lzi7{6)?f z{^rgH|cw)5<1zICsD{9@EQilP}9Kl?Sp!>I1M^$T0&)5H{ z1ps$}m|@A~?V!@_73!bTwaY z5Vq+ARGycAfI|U&6Sf0Ubas{o`m=v)z$NS;_*5Ba4g7&opz6b2c?*`MkUhD+$ZImn z1wngYfX74j?n11pji&1!Wv2>4mwGsDE$9eiWdcNb8@ab}dG!Z%W8nUBTAYH79w9+? z(2009L?l~wz7Yb`nwZ{P+A6|zCB1c?^E-C61e@d^??m7z_fA(ftzv#Hh2sXNj$v!P z+z^$ZprG(U$=PFH(4|IsEC@Al$7TqekjY-a#?I++HdEx;ovLHiyV%~2lM4)bjIDjG zDA2gzL7Of@!*4hI)Mg=wOdMMi&~R&eWr8(y^Xz~R?k0+ zEWo;ExDyqrOQwEhfqW}ezp+YA-w`cp`r_qFpK%;4uS#qi?J2a45AFoYh5;62JOp4| zkcx0vT0!wKXy1Hv`=)eUjF8r|+mu>Yw&dmIBCcxZbD8pfDSoJabM(dSt^4vfzD9(c z0JXmi2CT2{KfOdY50LnGGUjO1c^o>{eSikZg~#Qh)W7Oy0+XfSKQE#$6iEA zuK0GNg4a~yZHA0;uAyOCOW(hh>aDmCsi1AnmPJ+YYd`vUv$Mqla5L$U^pA@p7m=@F z!LtmY{=r38ua;n0&jx^ax6iVAb2ruQ{XF%y8}{DXkR415Zy|(B*#hN|Q5&1Gxg0<|ll$eHe zG*xg4B(S1{O-*HC2!;r7ffS)oF$o;~DPJTlIG;~u(Go2wfUJ2-e8XX@=4-skq72Xl zQ<{l5f5gF|F!r5QC~*C%^7@8sj2VkiDT{t=Y~$>n2rfMlu@h>u+&G0#+XK0rpICSG zUs#Oe@40&g)lEUxY{1#ZGZU@=BZqFK$vx$J8YJAh50x&COhIOzUSNxH+2+g3G)hI- z;!F#UaO!Iw^OZKo^DELQbLWW)eN{={y%u}O^XFd{?$K5rQTN)N%_-B1g1rj6on7(e zsU%)NFJN%huVDE>imBAGk%*K8c0vIM=i8a3Bi5W(Wf znus$dsI0q9Q03y-*6Vtij8V~^m*<(%$J@78>-x&{oPv)nfLpWgCB!HEIdY1#w@&PxkXtHkDx)~Xm2Id_*aL^A9+SqZp{EecDesC z4dN_Q!r$n8LH|YVy4v^=HD!FhJc+(a@8%)Eu9XLDSamS&*HpM}p;OA9Kn@j0n@b?+ z)2~O?t`ULQ=4c7<^5=EFn^VgLwQC#6poGGx`e6S?TF6uL5E+zPvqCgjKG&ka30ZDH zZNG!rON3GiE~(y&we?&67-pE1&5BeV&3&<0Vg>gJC^lmn@PlM^YtXSqhRXwVLXHwa zfv16zs3kNjy_orDl_aj*9-W1L_l;~*T*idV?kNkvGcY`U*r+R%|ACMk6${27Hy8`% zH!J2xMOsZ^Q~7ZK%XFfRur;+T0TEn9qBaDGW0VAgBO6Y)_z6Ht#YypDa7LnsFmnhSSoVn<{o}fvrT~-dxp?5tkCo2WT*PKlmfFFiBvvl41ezu6!z~m z=rW5C%oG*e?Q;E$GZv6_Kf%&od4|WQw2Jc zs+>LZaZyLMb&JLLPrI?QHkY)lXYCMOsFpmtD$UqrMN3`a6gcsrmYT1Ai5=qLPYFe& z!z4qs@LP6hH^RJw*l2pu@pZmNZW)=SLMx^U**pwv9NKcWCtb_}K`Co#!Rvq_FsJ@o zHxS_MAu!fdS!FaYR;Juk^4uQXAnkCdB+brl3R%p{JGB*(6}iMMZ(qF6TY+lfkAfO) zoza}?_qek$?erJh_Yq+I#@HH)CAhEnHJFae-k~Tn7&sVxGIQwShlLGJad1~)=zoIbYT9O zCOaOPf!aMYuF!LyX?+ol_DznBjXl~pf0f-nf6_&3{5d}33qVP^6`RM&rVKCwPh}Sk zKIGw12UbP5`uQMDi0}S%nS7+)cIK_THI`ql2pn>jYZ{Q$JCRZ zdL+0F0)|Xx?M=VTQ>2*u+*5mU`sPL9il_vH$yrtb4mGPcGtD~40Z9CNJ3C+eAx~1~ zj97oE3M-3E&Oc+C1;#+%IXbB;mha<`@L1Jn|6!ZKvM&Q+99VSBTYLpe4zs<)M^AA| zIKx&A(24n;Bec481|I;v>?h8m^nmzDz;^!TdJB`z^(2WFqihKcn~AQ6r*y|P!vjl1I^ zZ`xJ?t%bwg7|(fTTC_Is5NAx?-i8|+IUefAAj~j>53_e?!)K6PQHTq4YF-kbY2M&a zTmJ74pQy`(`^CHPYOdBwi=jVJm$wcQ`ekhXxKUwW- zL}#C2kShq}csBO3)~F(&e57->cEgeBay?Mm06MAu`?t>4;R-8#6)7OVNTzvT9D(T3 zmvU{x6?uEef_O9{TnqEt5_Tr8tW>yh#^Qhf5V5F+cZB{HxQ&~({ZJ+*mU*DqM`hGD z+j$ge#VNEL#xi8GvVRE}$0}d!w|k2Rj9+nKb6>3hvllo7HWg#Z8_c{^iU)e~0iL@n z^$w?Ug+uqG)A-7;Bep{`?P6v+t$y8^WgCMaO@KbV9z3sr$fG^-UmF|A7> z?n_45l4bVDot$?e)Yc+Tj{&#}mbIf!|_ewmGR~-4~YnaxpsL>_iH{p+=! zkcu>;cBkm=Dacb=OWsFZ+fAjqZ8#FgJk*|mPW#{|(+hES94z)2g@!RSrjq?BLZ;T8 zpBIPp0bZXmlKHGeUz!6EzgDdmEeFFtopB!8vI^M935SVpzIJVM&5-Z=T+UqJ9^A!_ z9a_K>1E`s4W{Y@+m^0bIO_khVvM#;(PICeA*mfnp9LZ z61$e&$9(dTr`;g!_4Scvg;UNR7p(1C_nGHjF!$NNBtsgI3_cJ0m#vcU@(&*#aWNTb z$DhDi;6g#024isg0VvzlAce_2m=?OtV~x*0FtAMN!v&md<6YTC@?gquiOn0VXo!e;ZLIg) zXao&O9I%-Ms_t9>N{sgcxMK^7RR8$n{hF4L9AG!b&KH^V>Pr(7-7abXRKl4lPE}FG ze+;vf@IR_6dADHA`^ zmZPm{5*)P)^8T!a&6I@v0k_C%1Cm8nP%OirR=_vrc*9DUjg74@xRRcQ$yx~ZgeMZY zrq`8?KuXFi7%M9)tEjc^qM*H=;_b7luo19rMs`dV#AbbQ#{CpCwTrUU#Wi-G*CML% z{0R_1n>XWq>mUMXEbDIN=n=B_vduSUFq_fB2FLpdfkP~yOsh1XwP1?`XT$UQCqcQ- zp9P#$6V6^v45pv0HQ@TsTpg!eG}n69ytl{iIb$7?nO(;_8e%U6^&wcI)6dDalgT`$ z-ne_wd;k^Fy+J;;p~cz~kRouVY4|RcZrw^g?Pll{boTW{^HafCr-%wNAh2HhOFBH0 zBg=nPxUGWEMtI(oYleV~L_FA*-xwAee$uve%Wta5F~~z$5vpO!UIxIVcFASRVPKlU zD4>IN3sitvyJ_coQ%jw&*B3I$8AnJY(11*KS776XVM2m+y%DvzZ*`hTuo}RV)tfw| z1HgD-zAy8u5+mjFIh|L5K9Gt#>=BPZG8$t@fYnzwH@D!VyBlTaYhJ@ zMQt75rSjB@|D5uv_@EstFox8SLZ$#jwmDD{XZ;n3l$i$(Q*e2j{IH;?h#LY1QS%w9 z(MbA=t&PuYda9@^C$eSA`aa{<4+}ZgxsBt0Fj&&9wDySshH0L2%9ShZ#yCgA&HwNkPU_qMqLP zTaJFaOckbq$5u7kI%DItaPIA~z~Zf-jdw*w@&J?S@bD5=h)3#qx~Q|K1FF)A1t7;r zv{B23#zEW5UuI?xB7eDf;nLJWALF}DVAsMw$H$jWPm7Jg8=a?2RB51-e6o06<%sC$ zvF{Z6uLItFXgl((!wX0(sG;X-J+tpn@%1^>JAha32)m5(kC{@PHC_IB>K`g1plmm; zUvf7oRekvqV?FC4)GiS!csXS%`DAtocxUQ+)u_TVt8qMnAv<~)cR&im-6;S#TVG$4 z=>Z;Z41XFZHE|y0!M!}Rhhn0m%h?t#wOjh0sG_bu_Wqa_Fv#7EaK1Hgkgy7j^+K@o zFTlgBKL{C!;Zs;U*gvps^u$|y{#y_f@XJ5XDQx@yI{(&-kQoNpz)5yYL#(42i{P_R z5Oc-&ct#_n_H=4#Mr5`+7*yyS1I(?#X#fUWL-qoCd3KPy#7gL3_Txb&8vUS14y(Mr z4SoI5lK4@;vAnZ>7DQnsGWAGU5RzO21u4QotSvq)!|UpjCzsO}hG1JaJ@ikWJQP!# zj9mlRBVDE&0!M_iuGB7_3WwsR)bJiHMMFw}7zkD_0-l3JJdWY3N05q&igxnLU||Su zG(E<91Legn77%Roq#eg0t_NwFVR9!rT$l%y0YF@kr(u2Qpsb}#cRXvJXuMV4Dtu*A zwyNlNV1ar53A3>YK38IUq&C4BvZk_j3SVG&$pf$zJ?+A^vjskH zo$Q4dzmajN&Na){#;ztaGn*`A7CIqKkkDP?M#ejnSyozevF>G zC)o4(ysk*u(Jk2_E@9zU^3REP>IGhS7?I9wBcLt6f#*coWQkg;CcqK$T+p_E=p@y{ z`anA%p!v=+)vH{~!G&ndW*&w8^e53erx~LQlwTjB&)B+ez5sVEpxAB6Tn9;X)GsT^^^>py+9m#SenZzSgMV%l4d-gMpDsCbVwRK9*jnI9U%; zCo^U%CCx)j!p*~X5H!<%?b|&r-N!{+f{U-l58zLnf!S-xXM0$Qu?B1I_1>weIaUsn z8JXVCwRt*G-gVG>3z97TeM>Rj`Hk=3QDMKWB#NVsl|Ab?oOEDSC?diX~{e!hc| z-#bgImAiDS&`IaCp5ri?DYFalSv(z+YhkcnsfiY=VrxMPi9ji~0}HH4_!i1E7}wMc z@%fy$cb=BS!{9j4t0C-g!kyZ{5Dks-P{7kOnkcb`MNf1^CZ9abOh?3O;ay#WM4qEz zg5^dRAB`G%1KRz|y5V#GOImziW0=J~a=v$Nl$5K#b>#Y4R{tD7xV@Oa3xiCrlwat$q^81D zXU;(iD~*5p;7uIt?DC2+n9@85V;@rZonzECpIFnUZPDVqw!lhAvR55$cTHAUSUrJb zP#3x|#0|Ku&z`;6p5A(-o+0)kVUdA5vgmTd=yI>?VDt1@?P&tyL;xoV4PITp%+bOqU|nBcCL^SSXR3X{VgcCh z5#-3o323Dk*<)G#x?lQu9uqu0zmiZU{*Txc;oLHT$!`V$mJ@HtZ`61P4ep=G?eiEl zX>ZwYZBMR<`*L=bm@y%f%yuW_sqr!;0ALW>-zzCKEi07pa@trQzd{g|Ur=C=s;YLF z*uEuwhU(mZ8Dsx~D0U=4qkgn=rj=o^yj4Qx#cYjzi-_w??ky_N(3yZnBe(AmbdE{Z zje8j1LWEUaKn5k_=w*?@04e_WdF5xknh=5z1m()u9w+D2qRaWE;iJB~Fx_r)H*%BJ^oNdsijOjhu%sVw%S3>XNZIwXadoL6=)1xO7u zrTmQmzo5pb0?;C(2Qa2rYW?yPm7dU+1R?@{U>M_ObaiL_{utj~rr^POV7#;!(bUP& zHi~rXB-IXk4oerd=^vJKK~BvdbNj+6=7>-5`_J(4zQCYK4r(5~)rPhvB6P7VLC4+) z`X$5ZhKcVF)*SqvcD&Ab1%lwhFK{RETd;0UNOnuYsxC7#bHK*@h*8U#AP=Zn z;i9!ww?QqXyIuS_V0q^#HUp{hyx*{I8RA{I8!rDXx0@UNu<@XNh?z!uyPjxpHURjcS?dE;Dm3OS|lsjvNB% znacM@_4eE2V*7HymTR>X8H=~I9+xs9&h0wG2GTGK21V~vladNcbk=8al_n(_Mvcxk z+tV5{3pvHbRPk()^7K!}ah@Ca9>Y_}((@=nGE^)f`F2SFh4lSLte{NEvpBXcP7a4d z?%;9MM@3f&5ou{tfOIM`I~%IA0(&)Q2&sJCfb;l1atx>50GY)n9J$M5aR{ydhrRcV ziZb1{MnPL_1JDWr5>-GXOU_^dM1dkmMo@Ata)v??k*wrgDmmvIN+d~^oGHnfB0~XH z^*trJ&))m=KIh&)cZ@s6#}Bk6bk+Ml?^<)tHP>3$IH2)iiP7gz-;X_G4;Mj!E;eAy z#cX!hHiknl56r9K1d68T&he5(pIS!ihp2%8eooCo-SPS{W=s2Gpl@W-b33b#7Qd`u z!j4b&NMX`lka6kFPfe@DSZ($&F8&T`L=efr_I=c1&8~|{xP226;bSqF4;XFWXgL_* zk*9f$RnFadpk|cbW7;!V&tv_$bZ0{Rx~V-GF3&MzSYI6gO}(hd$kg(@nrx3S#g)T} zi15_3APvj5aJ8{bw7r{~Ta}1kD7s>}pawJEfkP&Pc2NAs4KbHBaJCK*&@%VEBaVaT z!Z76w#iXIS1N91P9Zat%UQzI$4G9#@2J@`$-1x2!Li0B1ri-vV-%zez!9(1!A{h%J zUpmM%+iq5N7v}F>$!vGFBCw#K;4R5&Ob&w}6VvT#0q-C2uuur$E5fld2lzT_vgRP? z9--7YvYQ(IzG3R*279E`=?n6yT?nl?dk7$ybhl3KY^$Qh(q=BoalF>JhFZwx4p=le z4k{C?OLc7}rKYH1uwE+2)5~E$b_Bf>W3jmJ6&MiETe|sYS8~<8@KP(|H$a*S4-YSW zfwfoW=&s4rM3$iCD`L6QX@U%#^GPp{H0x3HHWW@UAUZd-YI z{h&EVL}YgOjxgKP#g8_sB!?;r^5jYOLZ|je-}4AHdHG;KXw~lCQG&RbrR=Zk(9lac zZc^r6JeQiA0_tA3q)toBgU*|KNI8vKUw`5hhr6$SxN`E%h1CVgM3;Uip_pKUR$) zuYaHJm|Sw*np2ziDFFAqb8>vv$B-oPJP%aPCoPBD7U;k~<@!GRv4bP_qXDOSI3Thz zSd*jfNuFglE5pt#z}&xuXB%V?{(;wi{c3sI#$5>&BhRL6&ZTT?A#74Y;lgR?OJB=e zm#xs~`QcPnL$FiGQnmG|%CCZu)&gvylQi-#_(#7WQ;Y4$qQE+G7gcIi6&0u5r3=#1 zbZXk#@v1xU*!OlWYx=O~%6(A)XNh#=PoC$uRpQRm70?GuC3@#B^eZrkz3f{pcgWU7 z7mEZ6-MD*?!BhA-o}zs(R4KQ$5tmB3f~Y&H>%U%xiaBx`#w*XWq*6d*i**L#x+Hhq zi*aZ=%=S$$+Az|aZxNod76Q=-58nmo)^fPpYpmWnj9Od;0tkIMCNvb{PvVsU=Fu;I za(CjN#kf&Ry$t4{r=Sx*%ww9}Y8#w4OvgD!j-v0Y-+>(mcSHGhf_{~6f=REn#|)+h zh$O&;dFv+v_kYFy37d`Bb3eR1NLMiCJG(6-FD;PuPUV-x{7G@29{}C_R*=}k|1N|p z$|V3nD7s5*paSLst%Z-+&_rZ$fD6o?2K_G?tczG8e9kObbuUf3_9@lR# zK!yoes6qLi4B*;;*^Exg&q3FtdWsBN)oS+Zw)gkd@fBpHqTEmhG2Q(ZuMubTXzt_;~FSo8KGwx6% zrW2QedzH6k*_EQctAmxZ?*Xgk)E4ahz=$xhNZHsOTXGphrA^0SVY7PyA{QBJj=C-d zk5vH0yH30^41jQpmg5i9!i5>2G%^w@)Gsv`9k(3%yGX2XL3(a<_1Q4Xy28 zkJcj{%5hSq3Jr$*$6^O{D5T=EXD1-8cg3U%{kMCPd(qY=-djCe(;1A2krw|6S~yTR z2iHkYlT;<$Bu|)6eU4S^_A%OGw#C~0(|F1D{&xGeLxP}dh`Q^MrP6Q- zQyt;jbl0uV;i(Rb+(y-8%b$*%?CBuO-^1N8AusV6B%W{eAu+wU48Z6sAUly$QWnJZ zGp5ndd*I1>M?lQ{;$jXEtMQ`u%*+fRcp_;T>BiM(yY6_wl0{R%Dy0zxeaV8=t}G?n z`yfXxa-}Fn$GVOddTtb`sdmKVDPxXEmmYL7gJ)MGD*KuO1Gh08S7Q2k9>{#RZnlIa za6G&7jnxgCkn@)&@ zN~s)!G!e=>UT-<6uWFuU4&bqnMDn;i?!Au8Vwmz#fiAjZ7agNx(vtDx!MSVXR3Clr z;p9=cI_3P<48+d*)&We&hL0Mq5+3nfThe4ahOx7oG*}6%#V3mB>gLWzFCP9_+S>o8 zhcOiYForV+nvWf$EYRBm?p3%ub-Z(F2~!9+uud8OoC9qk*_aB{grujZ9ro~H!s1Za z&0TbntK)O?1EVr+r)8ByYeE!QRQ?1K^WecnG3HPjz#ASse7iU3(6g^@WmS9e+Qo2~ z4`RyIA+mOnW+X2Z&-J3{qpx4O5+4`{W$Ylup%0tIDO7b`X!fQ4Hlba0uxkqii)<%T{N{FYFsTos1&=2mje_tV@PICxq*ls z-LG?ACsNDT)dTpgcc{R5c}s@a>RA4ObaVh-cbaIWB&WttZv%_mid1Gst#}E&^Q3^} z^Qv$oP092gB*)5dFSh@3$vl*F9xjoxBbKofRDz>ouy;R^YNezRD*J`femFa}il5OA z2kd`ktjzZ<2`a;U`pQ_XxlTIRYfPuKe%_*e_+UXywEl30y(?BypHbgSD@jypy)>vM z9AK;zJeV}-_AkE80)#F$`wasD>!a9OdpDN85WI9h7d5;)S{+jD%BsG#bpdcbf3RTZ zWzENlas@u{>mWQll+4Dp$h{gUtklm=@F-D5RfSn@vFG{s_G=7_cZjXW1%|_$bM`uV z5KqH_cu>08crA|J&YB7EX9GwhplUu9r8IC0H*Jj)mJ(uu^!0k@5c-GDp(uR?qbtLN#@&n z(crN^`NnK9k@{&pfAstJ@Uc?2ntWZkK0LAj>)!0I$^MHb*okW6cwYTxhK$T(>}1e;Tj@c_{9RXkB31-jCTphguyK= ztcIU>QQxxFn{LkJ}(dNhL+!f2{MKL2cu-f%gyrkiY0_EkTG~AbD zFhncM-I0b#E*qyr2asqR>q{LD3S#;(jUbBL@9kld$|~V%V4`*3I4Zcayo;FBNV z+ZQiyYpe=(&BgWfJ5qsd7k++G(J?NP89-kP?}j5jX292?&Bs#@TCzRSyl>}s{zu8H zaW({4QI$Yc7|b$+`n`=(={E_p+*zQDlbmhe-rC>p`|-((Sq?Dka+sKETKhEt`zD>h zs=rdkNGA&FG43P#d&|eK)9Zy~Yn<0qld2Cazkk%z#`GT1wr+DSQq%ffAJP`3*)ipVzv|kq)$pOU8;! zO&!jxoctp1F1Nx83k!Ffvd_|8G+I4eLsog@Jd%a?XMUJ1eKGCm!g9{#BOdEhdN?Tq zNXuj_9_ThPuBuj}uHdvZ(%qHC2S&3fOeQmA0s<>S-o5kuNQ{=qKJ@79drEv>2SoM> zsZi$XM(Sy*Bp@;WXT_xkK8eA~ngHQ(Rj7)XPPA!bF+)Di;`o5h?Yylz*x8bQ8zR24 zVf2F}GC4UP>^uWSCPh${H#avw!c?RL22^E^Ic+$(Y}+$QO6s8$k~!tFf6D=tzvY0` zH-Ai}ta1%7_|)E+Y@B5M^-8eMw>!_+4(Dr09bLaW1Bz)bQ*?P{mQwxOX_k>a9Z!ur z-k7f?Fp^Viw@2j73;y`Vs7XNdkN@-$fBk>I_aRqywwtQk`7MF1)uuSfZPi z0hf!T7kB92mwvdy#^2Plyd@IRY|tC1_8~lVSf*}usPyodQYmjtK&|!>PenST7$D#3 zfFM%Yql>j& z2dLbqlvT)(kP7^It}>fRnLXDchw@`F7n_3lj^S!6!o%gGwA~R2A!J=F&<)S+#8{&0 z3z*$9@B3D-Or=EB*3~h~#c<=&uwJre8?^`I*NYwxoK*wzf7ljrw4s%bv=9m(GhZ>&qY)U%;QXaR_YnX8`50EC!#9dZ)XGupO;e zqyU0cao4!-?<~M2clu4i5dFRGO53OcC7F^%#lFg|6zd?>^Q2h#*NkrEf~n_aGbxY3 z7CUX$EY@;E4X;wgahEmK!_tZ}vo;R30zK&nmabakWhtla(v7+Tu)QZPbDsSclUxu* ze(5?~Walh%6N}mOOkBLUajpS(v++gP*VN%M1GQB3+`wnR!&@T%@b3)4dHNN~t&qgN zlXKJBwT|T{C)<{Nkwxhgu&%iR(;hDHZ_#Y(9H0hHQk*u}Ia3g~&13M)TV@Y*DcO`R zcUX@dqSZq@Oe3?J_4$?~)+(SLd-}zXc%s-fWWd>SkdV@6P>CykS>9TkV2<0LIX%Qx zt*6h(g6{6QYn&9fNJ+!2qzsER$yJy%7rUD)nTVkU!k=4Zt@~$F%FV|z5RbzF|1Y1b`0MeiJELy$H{GeMkBXum|*`ZJ(JcYiTTbm#6_h*`ColwMq(O?2^&$MwsmD5ks6tGg@eYZA_0Ts*EL{i# z{`3(TotA69829z58W$NM4n0De4V1M7U8F=;T3iS;*pjwfkxM0uR+l1%$+?s57H6t< zD4!J@*^Te7(JNpUv-rSDTlwf?;6z{t7CF8Q*(X9A-sg#kmh5MY(df9EAU-jZJ_aC9 zh?Gb@)77oT7k+mK_?elFIqH)n$T;!u>4M#w-2l2aAC`27TvrJMgK`AwZ$*ftXerrI zb_Pb7dEo9^FN0gwOYHiVTaDc$B@O!A^N77t>3&8kN~X$d*BwLTB;eBhpt+@m6`qkn zf;%jc38rRZHC;!lg?axxF+Y&OKcQ5m{NoR|s$a}>Pn|k|nwZVMtu$=9H$QpPys7SY3tbqSwIgA*hO$w zx)ZH5G-m{i=hEHJbJG{elSY{t{$fR803 zHZ~|cwdu_xuY>h45k<>?p{&CF%ThF6idn2#;i)@|TxRouVb(8+h=?*lY}2l^O#_Y@ z$LSMk5G{CG%}f4Z>dYgwQw zlr38k+&VM_%M5kjT{Aj5l%{3b6Osd1-%7*hXDonWP2Cmb?h_@*h8uNplB2;S+?`Kw%I~3x*Wp@lmy^)b6OeE z0sg3g($U#5&qlH5fG7VaD@JTJ*+?(AD9P2^vwU2CvZM0%wa@d@c$D z#Y$oZL9jGL;uA2D3-{M^CX`$5MAOCJ-Ewksr9!Y%xrF|M{UIoc{-#Wik1lhaQp2EU z&#mRtX1L`+NevSB?eaA)PmG%8Lw}N@V>DRJJ%;UK{0K7P54>6k4W`R?f_r~jT5>2~ z&**uyioi|g@)*7?K1^s$&B$1tYwHJIsE9!eHL#n?8@HZmV8E5_oa8YH%GdZ0AMuc% z)#oqsXU?AGcN!Zn*38FY3xQ22{%W!t%{CDz3)byynB_e`j>B7GJoWV0bZ*NVB56N; z|E{v3NdNP<+)F-%TQ&i?6z*ClT{S?_8~pxqpTYPSOIw@`r1|-sKR?Y~;UM_0iuXPG zhM#%p*VpH+)Smr`ts?%Hs`M`MKynoL2A)^ht9g6ZD@u7U{ot`UaAX&7t;|npbG*)n zM%dnz@g+Ku1zji4>ip>|{jwbLuA^kLU)a8{c3%Ink@`5?*$SCLOe1Ky?O~X7b!t8O z^oDO=rSSQAiIFfDO;Ces-?IBh9l2*~3-(-L@I*m*%;E5Lc&>T&voZb2gh?%Q1y(ZdDw(~5dka%%yz7B!xL4Pxn&>?+_^cD=G7!htyS?3>0oFOLeD#Z^ zWq{ts06#0WXfC`xnz%#Z>S@t!n|1@wfqitW$E$VFwcA&gT;WfM zj#nO>sJXdo&P{(PLP(dGmf7~o<57Sd-Um_+9nZ0w#WAwZUTN#n<{#2XaJd7@MZiOy z2jFp*QQJ)VJ_|FmdBe&4SPjZjjvm;Y@~aP)m_Txp>znU@WxA!B(lQ*qSPT*b zS{;X5w>#Vivo-YS^WMDacp@)9;6Ud-x?T3Olzfbsm464h>*T5ZBycK0Tt}bbQ0Bj& z=S@R>?RZJOoj9=8Hhpf+D2iQQ<}6Wt=2t&oh2e(e=f`_5>drgP7qE}j9Df`u?Kn9^ z{XK-v+C_-YaMDRRtAvwNG87a+L!SYW>1ntkJYeV}VSz<+yBjc4hcl(6r)TXr)a_w| zUDR?lmHoF}4mY{Ai>&g*cih<6Rx;mSUe0*;&JP5O;ejk@hZJas#k$GlsC#EO`ookx zJzr<5=j1j#)KgJXYN1=&UqQCEE~X0)=N&%)-i5p^vQ~Vts&RLb{{&WT&n#DB%K7~j z28lO~spZ(Yq+?9lxj|*7U25kPso95#{XC8o0T-^jxM25&uQ3B***t+Sl68SwjvCv5 zGvCXPXh*GQJs{K*xoFsEs&Db5qps9`&{OS>zB=$V@z=qII?q+RZiP!~Zr0yg^k3~< zHk0nuid;T?^X5?8L6TDR>S`>SWj_E- zuT(`vRXzicl�xqqq}T*FgCpOSg)541r5n-SYB!Y~21f3y=WCHspWoU1OI6`|AH2 zDDNbfao)p066zS{Ex- z18`w(*<3j&T45L1e?XqxklvVwFieyY+uF{e0Xaj= zM-YLBCS5cJKVL87>5L}- zT?#byj9J)_ICcW2AW@eUM!CSCr{9w$&JSb{l7**AL6Vd~+iGHLLMJd0=76RJHlU@V z#U-2$Tj2r*12^Ql+mL0m;#e%I#To>f{|@CRYb&h^u!w)h;tjMJQbOyipfUJoSIFa` zutw0I7OAB$_E8%A@xh^LcpL2qY zzFrZ)eEi(awBk9T=J)pSA4M(#9h}}tXZI-H`M+_(KL(GFX3H7Oei}3LEK}({)AB!R zb_w0Sr$sP%*pyc5@iFE5e=66hP1KzBX#jAj#w>SyfPuihx2voPD_yt#0`c_$s886t zA2GN62e5q|&Bts0^|FAOCZsD~f{S&wuEFDgzEhcs1(G}jv|dNxSXYP_R>dcc;o;%= zWp2&PkHBse8S5(2!C)L4fRx9W#GF2-Kd9&liNW=$D7?+G>bZNa4E0?mNmL>auW>Uo z53;^&ZPFi%WtPh9+4WuFoBc@CiIssRDm%2aa(bUvY z?Y@_Ni;*J7F5+$6MtqkI0(37@3s`)7C&JJkpU43&+_$Qr!O~^J?QLEh{R}8f+>zu_ z24+(>Iedu?sB0B-hoAHJ1kzntJ56K zH*2*kl7^j3?t#~QjAEDUwxi0^5mGZVW0lza;+3sc62Bm=3zIEj?`Qa1ce*^B2Q;Z+ z#yS{&p0#fhzYv70hnxB4_xf-qyh99;^vU*P*wlvh!wwf(je+G3JYKG=3CeWYvESa= zie&l^^&9!=Kh9J3y?bfs1p2Ziloq}B)`uH+vDmX77C<#x>E)a)YTRSUPNA0&o4>;V z+_%67UD1LNQwYba5h++c&70C)lhSM7wvuWj89{$q2o*LxJdbtz;6OFOxl678QLTpy z=pj2$*QobQF1H5K#L5QSwKCHKLIG&6W*EQcV^-FzcnbPY;3A$QLgzRom~7WhQ_1e= z{|o3}EIoD@-&SE&<&jj>kmE5yAtg=DHRA)g@H;<0K+{Ei;9sN@F zbNBrGd(<*Q>#k9&KT$}K$nO%WRUN(K&9`UCefr$qI1vB(^=ltl4vFKP_j$7nd{`wC ze(UXEytJJ45-6lUhMqoss#>vl-U2l4lKAZat1XHD2i1R=bct6MEKUzQ!FDJtUtbXX z@WW~5;SEz41n$FiHr>Ocupj;;bOr}Is_F7&n1oZjpaM4i;cakoD!@ZOP;Jep+Um8v zMId@Oe<%NdF z^wuMVTEL>c8o6mw%aHZD;Xb}>uKM+qUsS67ECu;I#kT-+>WQ+qwo0I%v5zC*s6MII zR-BW7+K!O4dC&BvC&lSmD&{aJsEVQ**dj|CU84(HE{F9zG_LHVSI@heQhkS}f~*2K zQlXTmpoaiTmc+cVo+b*d;+DHV5n~kXn zB1Idb+O&wU)J7eHuCtASxQO?$OGC4CP7Cs-wYA&J?NK?^fVD^ml9#W0tgdSSaA@Ln zgMk~mw>CNWyzP`veN#PsTL9f3;npVIToo3_TDPFiGDRhP7jAvNul&qhAwws%buqeJdu61AYprp&?gHjDR-Tla;8hDu!2uB0l|4z z|K)PANF>Mn*?V*QlWS9D@PLS^b2gHqo9MgYMuv2amOYaaA^sH_e(FsJq5( zGiqy6wtp6;ylgzG?ajHQsHjMwTjw6)auoWa;j{T0F5PYoeY%Fx(E#)%U%8qOEgx|- z3ijK8$*Q>KIY3vP@^nl7@@;s(TE>zPVI5duWOSF)mENC(zQM@e@bDe>ncG@#f+58v znj!1kw+twkmzSSqQr*K5LTA?3jdzeQkjVvyt^H0_-s_^r2lVOI9^>>l>!8(P)i@!S z3+EA{uPZ7j2Q9^;ttX<7NWGojM6nNFp&|3H@ee4s*7=_8R6@HrUb!W;c4WW}rZ9Zc zEb{PRsyLmU_3oSR`(vEQ&k2dFOss7}E*W|?&PfBS!zFa=e-fqz6I{j}>I`iJq{l`` zB-@0d+l!2M1c^@cp+j%D7kUHd<3x=^yJj60Ii|rXMbFwf{Wn}B(=&cl6y5sKoRT6U zexxjKm9}~b7C2+U)SMJMXXlo#xt3OVr@_F|{ll0YLnqhFJ(hw6?iUUx+l6JdhX#z% z97)o^zoS`tr&E*yyDZ}@OLxO);g4uec*ks*2D9r*Vq$ttj)Vv4b{K5n6Slt$ZQt|C zvw6X|J26a7kfns*ilm{xHQbe%b=*~Nk3sT;^CsUyzgVVNjuf%+OjECsk;$C!@V<4; zyOHXM0ESSN9gSa$Hn6(`KX({@xV1I{Uz#?Yx=YyZ5pAWN#oOti$QgDucGtfxB9mtm=aXCX(3OaGTO_Mdm-J< znwpvhUUT>aK_;IpN_{X&MOyHtr|PDe_b-}_W?$dfxZQpj%~rTtAw9g2^Pn^!JX^)gqn0Rv7*D%%59%2~7u;=0qw+4}KgO7;Fb)o*GkT2t{oB?sd zA|l2f6SlxfOZYA%s6z5K^^Z8By^ZOWi*J2r^Ki7AyW2M;B?s2Q09vCJs2h1Y2v?+&el?YEL-+aGhHJ-txx8}=UL&dx*zTrEH5m+g>cr{kF4y(1SC_&J^~^_$fg`TYz<>#7gQ zGEGB|MV)OlUQ{6@yw0iG$tK@HHIU6qEJ>{1&EQV|E5`9Gj6s8zF6ps6zYdZxaUZ#` zIH?;LaQ5RfIkm$7Ua_RJLfg85Hm@az{|1KYYc>EN?!gV_t zoY20sj*dsN`ZpLEOYYgT#6s_yphG0%YYZ&hGmwEO8O^)dESCRSFH-M6G>*h4C?GAvJbeP`s z5wK-@a~!&usa+Phr`e?)=CC#**SX*UHN$}@IlD!zrd91o92ZA1Gs{(CZuqEv1v*@R zuA_0te!GdAfa&(_qTPtWQTpD=bOnbK3?@_OK+a+)xBIi-%U^-+-N~r=`Jbys$uF?T z$`X;TU-C&xO4@U{zbPof_sN4;T{>xg*DuG&*m!H#koivwWkL;84}zmzucYh!3neTo z1bZt|&0k*CX&o=8Te;QNieKlk;)vna|725_u{2%~(>05_>>AV@r13px@g4!SxN4Xl z)TwRQsj?=4O8D+OIfkx3zGS?iOqrq!hRBUOUqM1V!J}^L(=Ii`_yvD;U%A&M zi?O>XY@L6m)~X#X?OI+d2pZEx*-;}E2RDE}b{@kMA+kSvK?OSTg+vVOtmSqs9-6H^ zXt ztn0@RyVtOIy?$%E^h0PM2N$UzfZv{r{~$kC1AL^$Y0qeE_2rxu-RphYtFqim6s^XK z%I`Vgzw4VXSQWoCYDn;sePK!?nSOGr3Pp<+06htME8pWyX@(FPcE9JgySbh~amC=r z52Cp16cM_uA+4sf>t3r*Pk>n5ws!)OuBMdbue-wrHlLUNa3gq@rTa>7{fu?su~j35 zB3$zPJ^O8pl;EMB(dZ*|Jc1W zcl-1~e`J4G!sx44$nO9E-swYS${VMXUw?l-_mKGCR?zyt`8^~4_xF%q_!T+6YjRMe zpX({XT1`>CbXYT`Dc76@^%ISEYFIa8V`IB@ZHrG$q;%qMLB^VL*m!-F0e&w=Us(^_X!eu8Rt=uIsSrqxtwaIkbucCu{a!n3{fE zzSPs3vdB(#`6x8!*9AYRTgHXr|NiYE>s$$$XFbk^c71*Gz;50j8LnBmM_F7PSj{^Q zrr401pI|OtT(P-6c@)QWpEU1 zM~m5@Vpx3tJNwL`WW-LR^W*igs{aYvy)bogs5Sn<&hcE(KRnHo;N3)h0(cL6c6~Kp zx@4Koms_<{ihUOLI=Vbh&)V_*CFYK5I@rqz7E^O*)dphDlMp)dTWP|D+?^WQ+AjFy zXcTNIBCl|0@`rr)>p-vMFba!_@xWf*H5clm+J&XaDb@q8#>E1s4ePr6C`*ckst$qC zL@U)=W=3E0h887y`+7&`fYd>PiNM7I@+~ zpG0p@$C<8Ny;g>FUxPEYIPA;w6UdqH8McO`>R~c+XRlw6>x9?5Y4i^ZfB)X6Io!{` zCLl(vV!oM@W(S`4p;Moq4kU4c%k(FLXU_+Jwj7SJbK;C%L>?RC=xjDqxZsst$dUz<;wHTtS7iwp$wUEC0bT8-Iotj#jrZ--G zwjQS_wV32+MGM-Tkk-_Xbb!{7JL;Xx`ena@#J5K2-?t}~2|1DEzCPxpQlQ`CoO|Ps zNZvDdPiCQ7H7xN4iu3!Fy0{Ll=IX2-C={%pN1H8 z=K0K+)9zNLga|EaLH{4ihH|yxkp-*W?2-k05}t=T4@^2UT$EhYVbtwS0V!?YI=|WF zgISPQikw);q-fI?3VuCinWxLErDscsb*}R+NjR^MJvEw(xeUSxXy2jK#2)D9$gDp} z)~!u!KgmOU7dT|E7p9IVprsS5Xzb~s^zb+fN}#hDvn+Cg8FM1Z(a|zD7*&^@qQZI2OyO;6>jp7f{p^cJh}O-w38wk>+tqBQ;o7&;C^j*d^yt0x|z zHS(W7Efc|gmhNyvrzP*M@eri+K3*3svkC<>%{ah(i?85$0#zmYyl2TJrZz({w&;r+ z>0-$FwT+h`{}(HuS8*wVJwk32V!p4Ib|~+x>c54D!q43JI|M5@Z1dDpZ?#6ylWfx6 zFDfLOP@(xt?G38L4z#HT+DNY_5c&&(D7$EIf78LkQs}1!BOf zgplP_CO!ufKLall!56$$b^`Nk`T6DRyu=q*6ce0ZfxJPoCMA#3^R>c?NvYM`&QZ@# z0alSF7TxLH<&tN4>K6z|U0gav)+j(HU`yj{cZ;oJcz92UW?EJl~a7njNQ*B;=0B1q(rntl;!1p1H?3X+}Ypv^v zz+}_TfHpz6zS5SAp*HmSzkCJzp%a|1;?mM>|v zwYSeKw?6r6;PaXM?tOyYGX9|E8zvJwk|?EwlDsQkc_|J1G&I0Qzu3R4=-Q8 zRS!SRpA?&3S^4^IdiF7-F*8DObr}s!YcVGy#C-H2Al=+#VXmvl?6;<$~l70 z1*6-FvGuTu-$pXP>p@d8996* zSu}xNBb-Xi<`2j~uHK`l-=Lmc;x{&@x=bXAEImm!&|mpJcH}m)If1ysN=gaNp5%!c z|LQQ*WH8W4SXfj=@*^z(U0;y6ZkIoWEGB$pLTJeEr~L-|Fv-(M`EKro`?oj8)G^0o zN>lJpjRS|j_cXR~*U|p-wpnn|Z+<#uCcjcT9x^}pPrO3KjDzA*|La99m>@K`P-GvZ z{aib~^d9ymwxAYJ#vLg+coAwy5#%>4dazK|U1vVHEpV#>#-5Ouijk`V5kRzP^MW=VdJhbGyIXzaJ}aGuq8Z9i&>LsPN=Cnk`avu7ES+ zm{IBgYIC6Bse1-hcW~trWmjc}>#w7XVE@h1lq-rUZMcMRqgGI;M$;g9?#GWCrRJ26 zt>kHgC^$_4m7wCUgp@nz>grO7=IUQjy90AR@PN%xz!Sq&mUe!f1o7bCf?k>Bq8=?R zo!Q7HgY@$!p|8)dtMm@Lwr&BT{`+d4jnyP|+q5qdWN9AHN3d-1{W}B*ocD)K0r;s7 zVU(>_@b$+{=Z!2j z;hhT?E@*OSxmG5)RypX1I$Je@sXlQ+2V^94;&`2xyLOY~JgL{y&C()*(#xKHX&nnn z#gU{0AI4ouz&5TkFzmVW z9_Q&VY-Z>jC_c-6*05mtD=(l+`u>?jOuGCg5opm6*97?Mn>cc@#2;el5Q)oZTegO>`oTznPI;9CNI`M$C4*qk>P1;s5L<>p9c?XTN?*)!o$toqC{ag zQGF*c&eCDQQ@$9jj%{L>L$|{7 zRq?hrNHPmhJ+yTd0(h2Nd~h{xFbVAi1Zmlx$vif%vZTA>PQcH=3$u-Khk z5ju#IwN8d^b2t;7?Oo(n8bVx_qLYf^tT$YAK18DppwN%TIVQAHSiUxs@%SG!L(u6E zA-%?R1Zr)4)1F-sbHYBHpEl-c4Ibdd`55=Ul2(FX8OE~Jfnt&U&bX3+EY(W@C4!{t zTB_pKom}OH0R(;R#DElYez#??j0)gJYQbEngUvTD3}_J@xsINfSgc?M)ap4+eSnRG zsA`Igz}ni{ChM|7jWg#)>}p3QhC`?PEh$S_%aZe=;&x3NJFad*AG~byI>g0eEQf^~ z2J^(#!*(V^)e{81K0aQ&@OGyCn9{iISMWb4bBYQ--0H>bb(eZ%orCUCzo~1uw-g=+ zu1-*OuGkxr+=N-+b)=<*VjRZ$JA8rLZxsbF`a;j~8^8g=)U)rYr~3^I)CiekI-(k0 z+_*wPj0k#d)Lo(=zfofps5=%APd<0dNO?XPKQj626+w@6-q#P{3Sjqoo}=_x7UWA@y}39m@am3yCZ0mmNnPGE(x7fcHF z;o`M3_DPFNLQ;QKNn8{YJIh09xVgPsSk+RyjYMJ8Vh>D}OXg9-pA2aNX@W}Z=S96G zyxvyNnw0+b09F@`KI*@l~td zU`k?;?e~NA`F|AIGdrpqXRHyttS2n1knO$QT3?^1JVi#>1NT#e-AK2%cDzl%%BlfK zlcC*`eY~J$q_qZ*4>V52(`%E4mzP&gzIHEp%-Q;EoP^~SF#7RT0t$lNSPDCE3;}51 zIheZ|ZXaboe_|CF@NT57A2MtL>S~u6M0}n^v1_+Z$jexWD~F^#9ey8Yep7OTH$hOw zCrLTon2O9S$;civq7|WnyV=tbJRNIro z9s=F^@ORBECiWm?0ik?zr>!5z?aH~am(n^SzuJ!5y!?#=c-Maw;-=@8zJXR;!orf0 z5^rpYh%4{Saa7$|_8;mTsLFcG)C+*$a*2_T@A*irk5?FVuQiW$XX&P0Wt0tOxheTd z)Cv9>iqLbJTOKIlBxC9Rs1qX@$+_B^%8Hs^T6+Aq0{p`47zMXpTXCT+!I?8B*QM+t zyTmMLguLIrd?@*1zI_j9f#6j=LSGEqRK)244UNZsjvyTZmyOZz-Q-?ujVOcvs7Yzf z-2xNyx`WMk@O2aSJ-ERSkdZKs5nZc7;QJpupwQeABDMM&N~bKS@1eKlmWOf(NxsR* z7!h~#lAOH!*EyS1h4Fi9bZbxr`|)9G->D5b_{W>HqKCww5D{hSI>b+v5{y8Vu!o1Bv0Vfc0F4IyqrG}ZD`JwtXNEt=al9B3?Tm<4>vitUhuU{qS zzoKHkb#|t{2@^tvTn|UBlAT_BG+#0B?CPpNDI+|HGFnVsyp)-k5HGaaR8D7ki66L0 z-@Oxe1!pf=g6OYugAK3w<=*wA&9M(V#^ay32c;A1uSiSkhg;Vdgbpp&Z>J3}$E;Rv9a`1aLA@@xFMXdt zREh)ncGdLP!FXUSm=H(dxl2=gB7;(aB~UzniwN@##U&-PjbSn+-WU`0 zJWYS!6vp|p?L5vCf1Y5hJ^X*xAMn?Ooa7gLWkP}!!si&f9)bo$;(Ml@?}OaOHbN3c z50qnm*BYdolm^b>f9k(J9ychR(xlT63qJo3B!ZP6+ytOD=Sn3A$&An*kG|y`s=VI6 zc+DURrT`2%FMFnZMMA;sFo4Pu0BuND>lg;Q_;CY;OTyvdAJw&E{%4O#DYb~p-;LFU zO*ghsXr18VF$GG|+@#!vp3XTE)V|5caUw87=sRs}{*3CydmU-xUB;Aks zsd|yeF#X!v<=V{$5A{%JHX`4fv9w_*Srv7KAbOI?pBWVIR8QHb1aX;Eo0B8%4zK>@(H3bF0!Q6 z8P&hpRQ4(9xWf9#iQQv*!F~rmxxeo$t*g)g>LGaL4=C(KX1(^&p0$DZeUFXAe$_;- zDf_|nFHv}Yg(8v-7dpefwHM!APKon8*oI`x=^ z^Ok3+e)y8U-Ej@cPQ=a&r7I>Wcz!jmc0nZ;h=RI4#KMA&POQAjVXfn=qj#DFJ^hAA zizLqT;ag(F&J^qF;!|Du6S~&fS+3dDMN14ON@JYQzDal=CDz18yfU9-^N*1H>ug_C zR(c`Lc0`R5mIUz8y>2ihPlr*&bCX8?w$R(KB}h~&{kb3iq5PmQM9V|Twbk=sRs_>= zo!+epezWmdxuqqk9Gqq1RY9^VML_^Nu|2!|5$7YcwZGqfAP~(l-^kZ9z|3ZW*c*|O z4PFGUQ-bhLla^2#xw<2WRx2MbZ=96N1%FZo=9@POgP1Rox{z_Z*x*5WrSrPz&Vu*X zPrkC7`};F~l*#iyK0z-e3sU$*zbYtn9Hj*n7{VL&$2QKWv7RP5)L5zaa6twybmJ@PLxz&!kX@9F%~F)GYyv`dyi6iQ`+$+#X2* z5*Z;rD$qKNj*f1brCkiB;_>bj4T~L7>j|p+1wu6o!ut~rkP@3olPUL{ zgf28edo*Wb)e@30R>Y7HM7Cf$sqm4?KTg^syPa)@^Xat8Eb4w&Dz_^>yPU4*BmWF> z2H)bu5?j3S2NDw>Yt3QQawZ-z5(|ceE7!SSqu{()5}FiNe=m>4@~#V(Ts^NItvO-* zbl4~eS#ogWEYZG+!l411`sEw0+Z+k27jce^H7O*2@j9I8i0+x+YR$PS?xa{>z|GDc z6dS8~i!sHlZ%%7-&bj`B#z4Z8XYak%uPy(S z6)WQ51Zle9@^nhrpmSZcdwud)c7qtJ=OJ(U4>C>JiSAlI>=;0zjO&!g54EGbC``W*gaTeWRgH0 zIb$`bm|DTZVbRgiIpX`Pv=#=1(Pnx#%E{#rfo*!NudXN+W2)q#+!D0KMiP8JF zYfZ^!yE7VwT-8gQxI#dYz%OKCn6we%?=MNYtZDV_&_xlBB*4+;7PR3zF>n=?8`fJ) zK>5~U?}4Fz^uu;=cndck%1O3_!?BBqB!Qp(4EroJ-Jb$HnT{2L4`HqbY)tZL{rbkK z;fei8kFHM-lrO$lot_Odu^yrx8F86Q9#7I|0bq;{R24{xpeW8O>~XH(Q_mN?p85q< zDL(gC)Xos2?`|EwCpjAGO%U2|#gG+{xY{fT8A!I-XKVJgPf2;IJ=rbaqUN6e{K&gZ zsR`I+o+p`44ujwCBoKQkY)+8uyP?h2JG^>9fK{FWfXG?fwk_g6l_eRfd+MW-u5b*O zrigQODg~S_w%`SH*oN%+Ml8(0ggB&nUdn~)Q=s`!p`rU^Yn0WpYo3Xz5&r}cH3!5! zoyCZU7bbps+pgSBVL~68*X)hq);x`0k7xaExe2c5+fdbNQGdCQj8w;XApLsR_e&ps zwY`#KLiPFVME?z|)f=x~DU*pg&tKWzV6B?N3kcYgMP8eU@T?|dIHy>2=Wodyz08_t zA8QKMw0`ghIbytEb8Jvs4A8;(`%`d7=hW5IBt3|@uY(Sqo0)k4?mBRVu>(rZPEMBv zI;}h@>;|km96>WLB_+Mmo*e(1g#MN0Dds+r}Sa%-DRG7>N;wjJE9hvPrPEFijX{Ba)wUP>^=vXectkS zt;*35$8}HFUAYvfJ@dqz#FgWPTZ=N!R~IXBRmp5p1C;*?2WsH#z@8K;U*@#f9+l&w zihQ2;6yP(g;97BMF2au)t6aY9yt3L=$N-QZkWckdZSq~OfM`V61=YvKGrxpQ%(UNz zmbZTirtKfw*=fu5Y_iEVP@VM0h8B2vt~VY3cRRa<$JSk6#6c`mch>e5VM`73&RTw} zQ8R}W0O|ln;wD(xKzk)5WCG&*DaK}JUpE*T?m|fw0t#O57%mY_e1^NagJ)AK0&`SdI^969UZco{`Egi zCs*c-^~uaU!R8mrCViber8BB%LFb_mE3(LCSs~ijm;BVJ5yWOdiWTY+5YP%yF|Hf} ztx;2dKTwcywraZgK(;n&2L}Vg*eLitx1`D{F8DyjWs|<|96>Wnn+0RLHeQ-3#Uqlw zk^^hMqB4s;*2aCiv4 zzDo$wy~<4}tEL(>%vwbz!j>{SK_I#eOml1Zp^-Cf6EGw=Q5}=4P;}|~bVpJ9hm{3x zi}2}5<`TQ)&nK*U9Ex&O$C)Me6&YK&I)1cgHNqJi@ud=3wq#9Xi5eLnKBSwS4=qR76g zl^(VpI681<12j_LPCe;8F)?vFQE_?C;fZDs^m1mfJ8W3M_&^xk} z09u`j+05M-_GtXr4U3_Vr?|swAD;<+Dh%CQP!m`M(!KrOcmLB{d4KTj`$f$EVC(7o zzivG>|Jz#6{!|)shv}cRfced<3;9*W?wX=WpY&BLvAg^6K;aCWv9fGw){^f~?q#d% zZ881@rgPVrn3-|H$cYC9E*1BmfYibl(!Dd2`ZwJ3Xm5GyQ;f{V;VQNlk7#6TAFrN$ z!uT}L=sGVP-g{nDlufa2YDonaRslBEg@M9m5kxN(dl3l8h0Ih^gKW9JJ$8+Jp^jNu zoW8a3sbikSY@o2bJzoc}CU|w8VsYk1+p8NWBS=Sf6i+KX}yYpMwS#ykjj>z=G z?d|Oir*pmTJsw5t00eAT;Y3Dp(xYSBn1ilr(U@#V$z~qE;$3*C0)o{9?Z^kL_+BIV z?PZdcu>s3R9QVTVk{v=|9gkrYwAxJExUVH^FlID{6=PwAFc`=h%wcmX29mieIy@vT>9Go~#o#y0`KIlZPKT4WPEu~cKrA2)Pf=CkY*7WXsWXedk` zh@r(nHDcx{2t3(_OYe*AA);E2^|~eXvT`AqRX%f<30kNh6;_Uc5i+37b57*@AP_(B z0tHUSLji9hIjVkekQP)J&NX&CQ_3 zVrP=^i1hI}!V}FSHKbfIRC*mW&!Yo{bM5ill)IJBeiKl0o*v<6iTs-6dm z<*z_3P0u$)qJ=Yx-v2q*ku4q~y)o4FU8gv%7&RN6lVI8wJ*vBkC2LQ}^RBPI(ip<} zz)JV9m#KoeWU(bO1Hr7Z&%v) z?kxH7B&oKkvRc{U=Ebk%KJ5wB`IC;WgynBXR1+hxb8Vn|;^2sGZoc~AhHI~z$4d8x z7BfpL;F3X>1ym0BpJ@}MAxRC2w~y-x&}br!loDln%Oacrw3~Y6z}?|6HZkMT6NTLB zh{FlAOzGh1a;{8rLx01lqjl)hg2pnHZ^{a`x*l?5Lj7CzS*5&Aima?17K`gR#sI8*gW+}ljY;d?hLfNqGB78>0prx z9iS(j#N^X`&J&jhLv$x}hYGEX^~zsP1bpZbxDbdldPWCc&Xzfo6gnVkd;zpY4Jg#jHjNH3FM@e^2bV}sq2bOsl*_pA5OEuh`qLirVL=%L z-9+P76s2c?O8b6F*uYDRekS*+b}i7K`2sHj3Z*ZlodLM$p$a0{%$&XQ{J}t&@mXyu zP!Cfx_7;na0zL&eL+67CD@nM)T~Z*M8hy8zB{2i|6r7!%HzQZ8#uJO}aI|f0(OIK) zEhz$J!=({YqM#^CnX|O*S}QE>KbIFl1N56Uoi%RM2-p(O_B!)eJR2Gpnr?~V7QJbu zBn9s8XXZe$-jSG?@O)rZ9IW8*@l812=jZ66@-<8ot18(D2BT0V^a?aVa76Gga!3TD4ac~2B?%cG6;Shj+dWIt*+veG z$b0er&YQ}X6TuE16uAak1|Va0V$jwrH@h=bHtC{_%po)ri@gp1`P?DXjURg`b2B<@ zA0C3<{(HgPk{%*7V9&=gPkyX9_h~+};DtbAC!v*^lCUF@8!)UkxY~b_>TT<>QhG=N zlPgHfdYb3KeDdVF(uW%ZTVA@Vs;Ya&6*r_=eo6{mK!Dktq<5E%WzwC+In3xxm^pE1 zVz(WR=3S`k7s5E`Wlm3@P@-W#q$%bpB?|fjC!M{0X|dc*bcZB4y0(*uA{P8ize_oW z>vILoq{=vkF&`JmXAQT!ajvPOqj~fW`eC}t_59|sW+ZxaWcvL$?{rDY5rxgoiT4lo zN}37mgMED;*WJM4oz~Q)n#Kd_-TTW?z)XlhFx zyhIxA-kqPANw*ZTSiR^M%5)P%$z~bbaIgs{eQo@eX4%~S;}k0-PMMJ{zhV2oY6j;+ z8Q|7C09Iwmc_fS^l;-VH-PV6^djV#$ftG>4GrpYp0ZK%`g?p;zffv+guK);zQa0wL zSK9SQ^gG8^N?Ujqw$zUplp!xck73OiaS^zlwBTgmXOf1{g z=jBMQIL-D&2hf@S%u-}+jb1bU6}e&O&e^Q!-6*|}0OHP;_V>QPP-N&J`s<7J#r2cx#T=yzb7i+|H&Ru9 zo;t})@B?gKekr`R_uCWoTBHa3Yv-Fqw~&|^PU;Jww5Yn%AA%hy1msEqt4^ugAzohI z&8S___C)U&TWvSDkWlDJ0~|w^w&--E95)=!X14KSMG>^ zX*3v8c=Ut%?)}Rm1!g-TSjfSlxKXvTI=v3}#Jrb{`KO}2ZAo}2;B~-^;Zsu9^W@o& z9YA8vELA45W8_o} z7TeoKtmk$GdoV9@P0g6Yp{zi*3C7eb*O||uZQ~B#T|hNOUza?dBvN#9rGuz>l|_Hi z35zPdxe{=&<@@h#`D$PPfASDXFa3tZc7J`M%IfjwyT7IDRbLP1*N6>LNJ=F$P>d6n zbWWcrCf?T2c%*4oJ`ovtIT&Ve>((&dYTRL}BmS_nGY{V7Nmwu2{t>;_e^RR<;PnS_*tEaMh zTUJW+hd2PPB?T4`o(JURtUsA)%iB^c#)FHJ;kE3rHdXH}SW8SCM3cU_n zb=9Ls3U};yU>8a;G}|&8J4d!$-oH->$qb~VkFlJ*{-Lt^m+9yx#pa?_A!9JyWRM?Kx2d17&K zku3F#{wF95*o(vxo8L~(qoL;u`V;ZApZdBMsJ1ZAfo(jm-^yvRThd&Y8+rGc;N%XI zxq`#HPcCw199?%X_Dj`<-!=IBLR}IsY-m(!Oxfiwz6tHiH>F@;U;tZh7?PqQC?_Yh zYok`SI<$$!-KVdwQXD-gN_wP-#*B9eh$9EBG9r16O&eN|`nBTbp!zslk$4<)S5M}P z_V{uiKnsu0&E3fYQqyaqf>4TeR_q1K-p^RcDbtBUtjHRn+LHiJE&!^5C&wOrcczG!P6nhi{HIu z!ztpga5zWC{s zEME;$e4^#b!$QG?%E(4SN7M9$vSd|Q5ae6P$;5%vXCAX?YSIBldkP3?!^5rGiid29 z=?bn#d+oRziNteh{dxoP>6ez91We2b(GvyzYA?E{9{{jVY37&4;Wmmh1DnCq1C?wq z=Xz?5xb8)mA?#M_XA<&Xvf6w$N5EZnHWDirl5Ja5<)BloQ8((=_;gDWFoSC!n-s}& zpYlkpF72pn2netJF^)=Tm36C-Pm~a*j$xc3Ex=*JHsCeg!b%CC%8Y4vNZQ{(CvubzWL49| zP?aCnK)&imUE5=F*`bApCtA%{0|qOm_*A)wnKL*Yfbx7-DR(;djHl1+E&U_gt`d3% zcJdcFA_68N&s2ODD?tOL)FCnGrAwID=iW^$IZ(zFZYK62uWigt z23A`jxAL|O=uAyDsWq1icLb)LLbosr#fGk~KHhE_&$2EMOu}@MVtbwCGbHDh%CNbA zT}@;QRxMpcS-aTAsAMRPh=voAB^g7@%Oy>BY`mC?!q%bEp3xvfda54|ayIX@_deCt zRjN-8Zb}a-?(#glw`9U#EbIq1^q~@8Ftg}=lA(Fq)OiUf3!UdLp|lVNOD@q_jg>Dp z?6z(QnY%+;jP&8ZDA#fI9UY1F^%Dt@p97?LLPeD0GCkSxrH!^?skSiRVH?cK4?W&ccW3YO1P78l%IV1OUgQj=`~73|V|3F$GpO~-q!1bE z=0J^fEwYS{BVEWDFS?rO`L@dOZQ&dsBc47BDnbi>VQ}h%X$#iyt8E3iO-<9d@(c> z1oJPs?YnYi(27ai;f%Lp44u6g6I9+~P^1HBdM|-7$cM8llku5+FyW!~HSmE;I?vab zDY9R=a=aNzge=HZ7iHyDh}%5P78ljlkKG|jXZ8ewkp#cV7*lci61#w4#v-vEuR+X* z0x65@vP3(oT^J_1s#pR^I|{On*u})&%CWctRhUH{|K&gS_suZEewBTO5+W)J4C)l# zfq{x}Oznsz7$xe?tkIBkaZ%|SPz*@9*=lXw$O%gCKkHq!L#%0^!S$al58a|>G1(QF zc|wh|-8?*6`HlL-c=gKK+16N7(Uz4|F&nH)e*~V(`luQDicE_5)YGlcHB3cfki-a8>HB78=L%{s1Gh)5_P}j0d6n6et9t-;*oD{{i*l2LJ2D6 zUw11s=Gdu1Bb!&Vt{FO{!^qL^K-T^%+mk!#dEZD;>p|g`y1D5|%m?3o^c&y!C~}3T zm?`c}4%U`tnTA>73S!Q@JvI)fwI@`c2vJZ`F>{-iBC~j$qcm64LDc|qZUah6BFIux zhluWWCoT`BJ)7KrpO-H~Z|tq@i4cJ}C4&3-%HTWD!##mx4C`=Sm(}+7yt{X^A07T} zk(uBy^9kmASb9)B%0J>i`;y|%r~Up*s@>H+LhR31QB(S-k3fRzMxVJ&S0unfMXUsv z%DslaB$UJnO%2!R1Bbe~x#Oh50CNB+D|tW!V#z{TV0>SqJLnX2KlO-WhUKIALU|+0 zzycaaeYo#IaIF)#=L%&$Ns%bI_Qf$B^~0(&E;dWjHZ$qwZ_N(o|@PzJ|dm4sS^^L)oPR@9B*9;q(r_aHi|pXSvyX^(wymnT8%%JR{*E>+nAF^8kF%-g>CrfneEMhNV?hpQ(73dCHN z_{|C!=yk7ZqLPy;%N`c%XL+=NS&h#J%Os`UUye4w7s#H{7Di9DrS$)BJ!@lQb59T@ zY1%r`-<|Eq3cOzThR@zn;-Je+VLNnQ$l{kZQvLlJ<5L;$3&P(7JqL7A=cT8Mw)eGr zpflXBaH4B_N!73o;=B0F6E$8*9yrC*wDF-~iF)1ALc9%T+`l$oi)|S*kuEHsM2J{4 zubjo57jnLefhTeb32|dS01sLohZ`+;(V0~vNfeXh(eh!NuS0FK4cUnGN!@by4KIbL z6~g$jO$t1ZGarB>oTh)4eJx+{=bqm39BshJD<~LYliv7tv+bWGOwZB_`>J)>4QcA zfs!iC(r3?0%nKhTZ)3OyGHi@nK0lcshMcndWLS4>b-w2)1Og#bQ_uMcLmxlKlb~W! z@34-Q4eFnV6W%_YpK(66<9Gw4nDtqSfK0+fAz+ce%pbSn6(Yng-te%3O(I zORH3$Ay}K@3Sy@mYom#v{i2oBrHvyPtwq)m|M*O94ox~>CTp-!Z5k}UVUD;Udl^65 zb7QJQZ&ydZytej9BhlS4ijjc?{;5^ELJn^-`1qqZO2qm@SN@an_@ZLNx~BO}C((%X zkTjDAG<7>exrQey^%vdhHt<<|-kU<0O=L6Vt)z2;`y!=T^Ahqn59_n5yE#W%hO|u5 zZb7HB*-O%m_=n3NsoI6DeuD@_eVnDq&IP?>Ff;yH&o>&^`Qcv?Zmlgwi$=gh=1;Sb6m8R}hR(M9KqLW4L&N z|KN0L6g$nwIe86XO90~-Z}qj~R~Neyd$4kXhPBTH3j}QkdV5u!b4_v197;@+k5@K8 zu2_PU{fFIB;-Q&|=fZ%e_aX9}EX1`Z*G@~A7L9HKoON{6&`hga3Z*0B4=|@OjoOZk zbzC@-a6t=pA8EcIv9a8x3Y!?v;@#UG)rPqCcFTre|Mx2ED?k3avhGb7j)O*m<*Zh$ ze;t&lH(sn$ajGW3xpX#0Io#IggJOD4c&{UhC(=`7!G248(AyS6*V5ix-&CxWgv`=j zKBOlJQ$WIVR$8JLloF&qoVH=7iW*#SPV!o(1QCPI-cjks!&JF0{bZ}_5!@S~AZ0$8EV_Lyw zy;j?1VL!VA!@wKI7Z+8uVDYTx)+Pj4l8oYH>sg!?1`LF|WM;|3%gZr_07ml0K|9t% zO#NE1O1z>TtkA`Z(X};MIM{xfwbfhI-bHcCj$lg0yR$R{p{p)o3Q@OhGI!?AI(dpS z+6urPl9VvT9M@|6& zaSU_yI*CKpc2bvclS^%nqN?ZKp>aS4-F4LP*D=_ z@HA}D;YrJN(Q=tv2oo=tNi-CUE7?F<%)c9!OQ%nElt3};P*lB+&vYbxU*;WG24V$J z6`xbgU~CNzg6WFOsxZmAc8$l8Fx?qK?Z@5y9WihrYYQV?zIlQkUh^WJPBf4LtE#qeTX; zY222*-!WaW2w;H6wD-Gk1SPkWI%24ihIgOg$q0mC&YsZ$mCnB_=qJiJV9Ujx+2U;} zy({B1O?$3)_e@9TS3_WQ!Rt3~TqeROzG~*2JHhtZ&M&6twW=2D*jNZ=3pKqpfa@<@ zdr>^7b*6MEW3BXg=N}=&^K#D4BJ7H84|(~?x}cCjiGr#&iepPq3K>uw1lHH1Q3X~e zp1@KVoQ+74ZKOT(SO%@vuwU^t1RR|6_>&YdF*PzDp{FG(Yq1FmfoxJ}Bn(1lbsG zThyCIrl!oE?_c6+;kg(6-@@`*_C-Wo9J;q}-_Exdra1F8d?6y4H+50B>xaFZOn*_) z!{6rnFxjNK(Oq2Rt+qh%&9#c|EaB^S*8vo1t3IpLAlR*$-{?*)Vft?Em_w$lq)2##NTcfc zw2A%L8N%d#69EZoF&RTM#-KH<%Ga|TWI&kK23^?YhPa;DhIXxS(8%Boc&ZB_cM!+B z683QBW`l^+dqM(H0X8d-SFQK{h&UYw|I2?z5f2E4{ag57v@HA20J(z)ZOwn$#GI%d zwfkABZVL)f<+gK)0sUnK@>|QY;PAA!Rc4bRdW5Z~Tr`3#8gAy=L!Y{v!y9q%!LB@W zlQRmQzQZJO?EQQXSEo-soO^G@L6qxJH}kG%Ic#iUE~kpkqCq!(e4x-a3Wra#*+~q6 zg(o$9&a$7!^S-)K{$}u(UK! zMS7(QlTiH_O5ycqt0ItzcvDhx<|Q`hL0{Gi<3ZW}vxwB9fHWN(7^N7*t{ zQWonF4*MU(+&N+*C(hhi4ry2V;twAmc~$o`OP`8$SweU*%Pp&Y^w}^4Yo2M_NdOvt z)lIvcL*v3=V37QJ=v>{(2N-b&=yE2rrB>}KE#*#`n}bROUW_JIk2O#FR7!T*NivF9 zeL}YBIeP9cLvjNWeI}9zV0qNu-+3tXBsWHf#UqX%UrK+>rJA(0J^oZt$CmpLVJ^o! z0PK5J+lzAH5$a#HGLOCSuTYrzg>?Q3^5vrSbDWHEQ|C`IZxZ~r9oH-_;HG$YQz16D zwjO4}EByvV1gq?^LG7hg8d(>lR)6jgHplaHDW7>~>q+KX^!TfAdgJC`VQY|QBtwmC zf;VoN{_28t8nAo*uOjU z)i{0&`G29Sf!x){X=b^d^}^%D$b{}-R>0~%w8_3G2E@oBLydrq1;*DK!j@Ai7RRD# zJ~~Xk13Zlwru+_tY`Rg}sq~Nlc-xG+c?s#21jrtUdXky55UyY3q?~P8YsUNWMFb4J zO7!vP;{dAev`3>p@bhwGlUM4j(eO=pQZVP0l3Hps5&#dfJF6$0%ZKf@?{6iew1OIhoV-GYf$NPRyUYOwTi?K(uKi@3as;}SC-&c0V? z%{T+~nzOZDyBCz?jKDMA?8EB zEfn`YZ_7$_7U?5@7L;J; z5P#U%6c2!r5p5$0F0Qzx{xDrqtqH)LAM0F8N@{d|gj$sJ9!Df19Eo6*b!{WHjzb9X z`l!Zh126FhLx?5-+zQn#h~*dX$Jr2XPd71&JgXt30V4iUZBG=oYUdSg$}tJ)Xq^WN z3TDKL4FD^Bu_$acF&CFtY)jOI_3Vg4L|6do)4G)q#WQ=L1sEpU58ZEq%=htu7{%}h z5vY>MMMuyjj-M;HwdH_;-V*vR`>uWXb3*-l9<8$CH(s6tneApU^Qo)@ zol2aDZ~|b9U0kO3dMsL6F5-U&sbcysNR$V>=VC4;ol&J<2T4;LtqLAtg{?Dk;~AG+3+*9Nc4!q$-M zrBghi+HIk-Oym~@1On2qS@4(jZgaiLlqXM)g~_pe)UVX$33_{bAKlKmd2sZVl$Q#) zJeMT{RtX0g`eO!}am&MVx!2Z@*qf`hz~NJ!?rp!)T}`Drkf(h-w|Cyebzydyt2IWc z=HIEpUW<_Kt<6Zo`doY6GN(AXSy_1)(v5)^D?0-GncBBB>haf8^%EJ!c8-p15F#KlYMWT0t$oFQV|FIEUQzYuy+e4&YLQj{+i79a zZlr{0+^5&zz_Z-!KD&JkGipZ7Ho) zI7Z%k-}a+X!?Ps!?4zTO=NJ|@n#0JCpU}A(L%)5B>Ga($Geqt}8D#5%ede;MnVA5f zs{jojCpa^lFuV*#uYcFAyr{|CY1s>~S+?{6;a?;|FZv(SMU})u-g7LFMVtlA);dFN z4SBLUtmci%#(vV_gv}Tjry6dZDgNr#w)hnc3Iio3`N#Wftv_rA+aC^Xy^Mbw zC`ox!p>+X9QG78U*YaXglNSmSIs%FtsWBY_yTzZxZ_*;Iyf&_28AQqto|iR@4Bz*f zT!sqnZK=)7#)5I18;z62XF*_JX2PyQY6m1?-U7vJdx9`aL`x*8;qXn+Jec{sfbZSY zcZorfztcBpO9L1T$4@-wh0wDLCZ>xY1VlLgg7qCRXM5lBj&hIV+W!II%Rhf)A_N@u zN;HWvxQ$M~>wd<_oWV#sQOCv#$-Xw(ocVbxAY(0OTLuXRrpQI|V!h&_{%xE&UAZF- zxcJ;Zwx%i=TUI7Jebr(B;W(tz^)^)2R_x1FBnggYYO~_-ZMlM0-r5|7frhMTGE~}mX zhhnAd&r#5@G+K#_zdkY9y4_WvNk7}0dGw}6^qbuvgYhmsK$4YUt1MRCO1Y!1`JQQc zYBOa6lWl1DOZyjC#_b1+1R!ZS%x~76I+&e)agH=`26f=SfI}tMC<#cjT$(4la;+&-J?X@O5XMb}E*}9Ivpm=RpuPw`arl8c~AQ0lTfZ z?NxHT`bP}w#h*=Yd?uW9^^{6Vbh&Li=Z{A7o@kP-rtt=ZVt=VI9~}qmkgnwABCWvt z9%a{~!S3%fr2gttojx0XZo;P(@S@Q?+6^dAibM1lM67Ni^z{IyOmPv2Q+Bq4pVia? zK>&|A2p)>d3JS%sNQAzj zIB8tyCg4#&*IiOPTPW!N<|V5~JA8})4GJ=pPX32t)P?AR zlwvUZ^l{Fk$!LL4nJs>>R2|L7DN8wH$D35u#!=snP*5CN>PE97iihVWA<&f%(1QL1 z%#M&C>}Y^2Zcf~filwc!=LFc0$+MX^w$(O>N{QJ|CoVberG>C0jO!FXx0|PeJCzHiqch-;d_dK9#9{=;7e-2cY4_+*Z%C>nhr* zHashPiIoTD2kPSFn2IT6^OlessoZ&?`r8xMmA>&cYQ#QCO~4;S>nr}hs_+`31`+bW zplhbGf_$4OxdD*B`_G=ak|kNMUIA0&%?#@WPoL(!kd1M}55sgiWG6gHLG8A;#<-7A zs8wp8b;fQjFc+ddJdXf{gg7ON(5U%-o8lBpRV)ArqVN0CNf(1?Ip7#h{})vdLl(o?C9z#|9IXXiwz~nE975C zJCqNrv==?yTEhnPTYUkv=hpgMO%rR=`?zJD)9q%8X^&Dk0mvkfrl{lS%u0L&T(k0E zN+M`;I#c_F09z-KZZ+&5Kiyv-q_4q8HXw@*PYxlrM{&Qv_r`}O+|E!-5+_MpP95F_ z<1i|fpvi#ay?Wl!Ob_6nlsS7vsTNt8k4qQ=&iCwC44ruj|y+bAd@FT2vh!qy%7* z2is2!$$Ac!O}alt+Q303njh7{(D_v;!qSea$4(CfjE_;n5(gfCKQj+L4X)-LM-G&U z;HYM=KMrGyy>w!a8N)mN+Mk@lgLij7H8AH@MPv_~9!~Fbzw+uqr`%VTiF>)p=ESoE zmA_G|eful5TF-%64ap1tEakQaZPD-+ltG!8n2;e)r#yJj;)4kBnG3fBG{T3s-%J6Q4Gy{GPpVj&Wo z=rss=uFR##wF@&PkB6uO(v;Xae(&0oKe(25_@=Rl{#~*Ww2=jSb;iWve=oUqA#oI= zc}^CVKi^-8o`Gzjl)$R-qneY$&~E{XB0(q}-i#&cA!Mr&KX=M|27g$hUzukVIzBRy1nXX5D?YnEa<;4#*;hAKI9Bf|TZ_fN@S+Hz^z`LE@z!gCW*# zame+HGPSwa-K#7tjiKJ_+&a-d@$=1>?tuWssh10rny zZY*`*?-l>2_&6=6f5^vudJ0Hkgnl9jIF+v2BrpmJ3F4RijZ@PZ-4S1bY6-wv9zP6E z#PD*4CnVek;z@v8Nk$Pmu*Bhao!gL1{PpJq(v24@H{92mM!K>jU#ymoC*v5wftlPh zdk+W!@|Yf!dxA0W`{<=HucDssu6+i;~jfl4_QFbs;bi5r-H^)xpSNg zSC9SwpEXQhu$bKWW`AjdpQS;~aZth)e?YW(W0No>GylJ8X{Ipv3^Lx`FWiWrO-@o8Xp)8CEz-HerRZj3cTyy(9UhvN;7FN zGC@$Jf1mL|+JPwEmZBb}haVM+RK3dHmpLkTH8SH(1_y3u{kG%~T~7MHVd0~`Xwfiq zxCf_JOgx%hrv)09G`(#Nsm|OQ>nO#}YrGrgz$0rSnLpE+>6ncTY}@>!^+ZC28+Lv9 z?6HWJdYy4U+*B-A_xC4>-ejcDi@)?n0Y46g3|R*y^DDIMIbOnyT^-71tldt!{!zv)%ndoLS@$F!`9fGF)W~| zx<9>Y@2iGRfUqsEb182uCjOBo^ZQq%r7d+opjNy8&qR>RK%gV=V(hyPYP%=1H_xa+ zTdI930b5bQ1LRf|BT@Qq(PP?gbYw$(T z@u+~^l@>Bj|3x|ePHlstZ&gYU2x|RQM|3N1+Xn|yH`Vqet?#-+7RtH6e&R<>()hZz zxIKEAtV`d^`z9;R>F_T5mjaVlpgK3)U!}FxF4HWLUo@LjabE0e^J%dqLx#xP^nRvI zwh3hCT@<^t@%Y(vE)wiV=3k{iJ|TOcMjixxtJ>F(n(-%Sv4K|Rki2r`ZITz;(_A}= zFxs}=F%V`2l0f8=mp;g_iU8{VRHK6a>7fh=yhI~Apc>qe;_Lr=Oka9wNy)=PZPWWf z=c|8SvhA$$g@L4D%(o^nZ~IO0Vath zJT(qLpbfjp-%itty!sVy>%xK|FSrp93osZf3%7v&M_r#F>ECta*dN?{k`p`7kl#ST z;whch!r=@M@X5X~+oOv<>-X-^IWj*!;_Je4l}EpZqXhMBRC@AeQdG|`AyD4~^zZgu zi;5Ky9?@ZENCpS z!s}Wft3rRi7!|c#qW(OyY!|p*SN{X$VkR`dL++dwX^qjCaD81?D|!k z0j{rLOq?=yq8>)18#GEjk@Huvru^etXl7=>x$BI$oiNr14~iyrY>Ed1`p+rCh|=I9 zMe@wtLVHPXZjPT96!KF1)o+!ti>&}_&ljf`+^?WZ?$3e9=5IQ!WgUl?|G3lg?Io_O z8oOpIZzR9~QE>?|S6Qj(C@?BWW_(|DtWeV*v*kY5kx`n>H%)L}=pBv5ITAK0S-dvA zz?#_=##$6AB#7DYh+F>y`s^Z77~etqw@f@4%ln?;=HUnb`zD@AsLvAisQ(co!8m!9 z2Q_VO#+Mhi9OR5NRE(COtx-6Nkv)Fw5Q}>S(K;yGF4As$A(`CNqoSp`nJk-K1%N!1 z$;>Tld`d;ymGXsYV>4P}lsq$Lj-q_Bk;wl-_lXSaE#H<=np|y@ds9K3f6B; z-=bKTKv>#)yJbOe9snjn_|96=Ukp77MPYc-+N#zy&FolnNdqq;wSbPXy}bY}RD>m2 z5aqYEFBiEC5&b_k^pFkaC@v1Smu{XQJF>YwsgpF>Tx(P8h<7+`+8TLmgn&O`V*UoC zWQ_`xl$S>RgG)xIl!s$jUBztlU(mhqHpvuKb{Ne&vsCM}%^Y>8)HYLXoXw`6g# zD0ejUX29uer6j3=rR}iFHkSxs$L)xA>+)v_YKQ*&W}Y(;iM=$mcRtc=Z%!;z0>;jd zS1)N-VQ6Azc2;4SKB9}EoWQsRm${1;_jmC`8G6Z>*C8#YP|vu@YHD6t4HrdZYIcEG zQ#Oyy(Jzd%z{f%pe?1C>fL><%{JeWJL%004?Gqc3c-QrQzq{>F8sHiNMR@bkM<6%w zhK)@l`i}0W+pgi(A|XuTe^jTn1&A?)$X=>@UO z;d37dQ?Lcc4lS0#XjZ7$v72B-iy~SlaqjJMZ&v(iCZ>9}P^p`hZiP?J)B5fgB4?oc z!DaVqhdY96?ra|dG5z7VQvOZtjv%bQ|C2nB10m;yX63}U&WmS}WrCJSq_V9O%!W2!-6WVXQ465l=^o>YWTCPe z$v^~RZ@~nR8t`p25u6=|t8>27q@d0NAzq$d{m}p{u>K__u>_qbiZHAz@tji~kU}Q7 zsjQM}eUBvBBH56Tt}me}3Q_OKfSw@G)17AOnn9`CEjVw_GPb?i#G$76J0A$z{^%Nb zU*(6F`OXeDRKTiP=esWc5;$fTZ5AJY^Pt%=z?weiT>5*q$6Ciq#nlT(noti70|(#t z+~XiwJU6=NXPL5)u3`zXxA|r0_PT2!h37Wa=*TNX2709C_J^bU#~5D7;r~WSzPz48 zi(PU6LDiWW3?Sv}%&r2oj;1H2D-dzk9=p$Bq~`9(K>L5#Hy!SYSzfYqjn)ZiuR(4U z`)}@sh+)=*%-Y-W-}lt}4sz_AoI=u0EM&spG{3wYH1iwl%^AF_JM@4|P0fZRU2-&R zK`qyTYu9Be{&JQI1h@7rI0HJV$!p6O+@i~NJKKR}?)%b$75enOYIaBfpvX{LT`!%y zg@NK$yp)v-O7-qLLKr}?V#QwEVD;Lh&D8L?NhvCN?Fp6P&>-9KlP9muE*==a4HEuV zk(cI@`IIa6Q{C#b*ZcP;0U!IWjC}uf_rIu#?Xy4oR~^vH9RK8zd(=0>4zvKYVUmrY zt&w)phoj{fi_V9Dnp$0DsGR|~_>7NKv0F|%urGw{PQr}VE2kN99`_I?T#8hjYf-8m?kP&-@+hqp@H46AJ&jo^Hw=%@MHLwA`i7>~wb} zi(^%UG0F77N+8Rd4-HwVjja6PX)an9;SKieyd zo&#zTpxKgcjiR}_DsyIXGcd4XF*FQc8L_bu`gMOS<%G@S14EMor_aBksUwDtjE>g) zG|U+eE5rINllmuLLWA0|i4`#D-Gjs$2-`uWvCvl)55)gqP>rpxHnxhD_b~s$B*pR7 z<%^fZnKkLvRaT1c70^E|<6#0CeljDFdW#9EREqVmmK)y{p04kXS=W8sLuy*4C#QPr zeRa(5)L688#tV%ii$%*>WMTIWXm+ViT$-ttO{}EFmy#)s4+aH4x`)MtRd?X5RT51^%H;b_i*V2VB~J`=^ZKqV;ePpYsBn;0jaC#$ywpt$qCU zAw2sx9mA*7_NlIkjQ)d&wbE{tb0HX*;&jcYN0xc?1tZc%GsPSxETyD^c>YL!h4+gK z<5RgJE`Ah%K%q9|S>xITGE8qSZBXE^aIz0RL5GLcv@@I#cV4vwbjx{Buk$_FtZ+TE zyj;^R&iSEIe{e|L=Jw~e+Qflz#SSLg-}Nt$j$sgbM=lC*1+=JhfP`gFOIMKA5_paW zv$<>=5EuxCPSoc3@FAQmRbzM;9D>mdq(9ih@lWG`Xhoo)Nq2dk_<;E5VZy69(+pZ0d zf!wHwh!RpNB_JRoT_#8?NXJNbBSR00N`rKRh=52*H-pmM9Yc55P&4yg$Z$V<@8^E@ z`+e`{578gMnzhz-UB`KzN0|2(8M@9ea&!BCycBM`-Ovv%=nxQ{3-^#haIqHPNxc41)JT~Um)jQLQ1C9 zx`pCNlhe8Tt7vSlHTJm=g(maDRD3E=A$*gf8cNl0K-df{46k(~<1AtO@AsR%0}*wH z%}Cva-1}J?0e4(Fo2Yg|LS)Lhx&4|HpQsc&2q>h=)KpiWqlMCrVc)QG$WckWw7Rv# zbFToC6Ly5RoP}V~p_3CulQ%23Br9bu-c!A%%{II~p|rayC_NC%t6E}m26!gFsoUN! zT4INeMPZ`3Q#zW$ESQmnZ<*#eG;{_x@8414sA*0o?>3Za3yLjMIBFwD2yaEi6qpu{ zFD(3O^7`oV(}pHau;|z5%Itw{-OgJ*^q>z%(SFX{!B8~RIm>cjxi;V7U}L!L@4IY( zIYLRuN*X+`aWQC#mi}E(Lp4)4i`v`3UgL2lJ_ntzE+vWpL1E7xJm9hz?w?Co(Vh$7 zh&t$Nmc-zaOEpjSGZ6FM8BXm-v(EzBhi_1;*+BSrEx~28q8QpZ-@Hc-bX;Dy1&uWPR|r&79P`T01U+I#}m=(fRn4jDR+Tz zwK+9mm5~l@5L~Y(nW&m4ZakX8&mg%cpZFQ6JCrV`493A9i(uCA7W?#MZP46sb1)ln zLpzO6EuDKbXE}F(-Ok!>$ zAS}L)a~hG3+6qHP1Uje0H1p z*Y_+f`JNX%qJ4k1d~-zWr4=5Cew1iz+}j(A_S)AiVYuBLt#Sb3?DQdC0p0~7DP-e` zo|H4Xc5M^0Zd(cGfKymdeEbzXrW6vAkVsJ&Gu%ZxH#?h%IpunJ`9(J}9rT4Al`;bo zBGDsAj&;A6pOBn?wBm{nQVaHj_AU3&>V8>Bo957~kupBhKC{XG2HRs{&)k81s(kYJ z*^){}O?XMIa>sH7a3MmxK^~L>aQgNBP5;m<#ptQhl_(^VUGw(ChxGk!z0CX++O_dvJ9tP7yg=#y2K1RxYCdByr^;I<5**bW-?mQtjp8Ji9L^zUb^sQ%G=>C*JU>|68&Yh z;(ZZN2~i;QzaJ%UahQC=dA3NS1k#pJ_NX);A*R#_k2>Y%wnM=>GAZo|;N? zLW{g}r*Hu`0HbeNSg3hc+m)>D?6m9h3E-WOprD1&!l_`3`^LSf%hArUkGBH2%qIGe zoO(YXvhh(D_Rn6u{`y@zY=r@AscUkvI$)|&l%EGjYD=eF2N5&By4L@S=7!&^&-yJG z=Ir3G-lHHx0Np+_yKM)^t5Rg*9baonipz$vG?P9znnQq2bg{=6GMUZrCP(FBvYRIe zW8{SqSX*yA+C7Xy9PZ+m9oqM_#X-BiNu1M)=L@zb`J_;wHX~FNgxnbB0_Q_}u6{Z{ z+Lj*ZNkDq8M9D>c&LeH!@24wl_in?IdIyy%w6anJ#8!CRVC@S5fjhwI+*} z?1}sZDK0Kn5f$_Rx5sift$N6H&bvGoxap9}tg^LU%6zi1X@&QuV|*@B*p~PZMV<0$ z>HuU4eDX|}o!6_!VVMM%m~P)DMqV4sH$`4k=IxJ(JDOM;;y8jrFI3!* z))hx2f_oubdhuk>vMI6wM838My;Q3&>7cUCza=L0Ogqb26IO=idw7l;m*3VwX3Bwx zV60c~B`TA-!Lg^*7IfKle$mfT+sqxm8Be){`RX1cjdr4Cncg3X5WsZd+Gz#W=`RB< zMg>W&?VigmwR5ULPkv#rGQ161Xf)QFxZ6c}rLkb-ut(?!ZRytN)Dj6oZG^#1`a9I- zEC{u#X`vWgyOF8I7HUteHl(zYj37S74Vjg2j(lkURmRKy2Js`uRPh1fXEof3m^xXjrGGyAk+hVl+1+sfMzsh zkPQs@!2%Burd-D@rVPyr0$v!5<>PrS$0mpO@6Y-Q2<^voA6)xW3z5JZ|55MZ<|HpsxdE&kmS7zL~1N%49*y=SL z5%^t>$~|CS+Ho)!k}0^7b=Voc(&v50?Hrc1Q`+?IqD;fL)z*~kXH!MPwRAquJJ!6yYiW80=QlC8IpLPd(Nb#x~k zc6y5lpbyGkEDBDh(zdVfs@raIqhKVKYm*O+R03^gn#be~+K;A{5MQQnytyC*(+S9* z&bq$aDd)1G`S6CMBM67BMH!M1AU7`AYJCs%m3k#^n4lMu_F~C*N=R6kg3G?aym>h) zVPsM8>Ej~Y$$RIc9FF}j?_4S59#cjs6C*O;V^4&=j$gGsx0d>ZSxzU!Vti5OCnH?F z=!lF}sV!A{5CQ>5Rz!vK70$OqtdDl-g4GKx>d4A=i76Zd#+_Ln7%X39d41PV*%7FI znj?t|Vk>e3sivIzdQehTRRVN)Z&!Cm@Ym(00i8-mm-AtqI!jjP)+o|~@ z2C(Ls?!|Ve!hf~)`a3^)5119Kis_%uu6OP6^1)Z{K)fX+q~jmF6Z2m>x7#Xbv9sd+ zZ@@kK7`PwX{&v2f0kMT_U>$(y&AqVonJ9~ug|6hd)}@5#=(^u{&HPT&rr^R}0T~T{ z(~2s*Br{NSxO7Kh*Tvf1cun>zKzuP9xU%~wZULw1r8jR zSXGR|HrJuam<{ZbzXdwS4-H@%4KKs3Z!PiOOOl+Cl!n79M`te0axjTtZmw~_1HnwB z*oNO{{W+^`kDM&eMqkf;7G(nMiOB`&I38jUk?rE*>VWkqDHpP)fBfrR#_-RcgLQ%hv8|+m7kY{J`tU()0k4QOK+^mz=k* z0kl{S?L0Eh^^YPMhgQrAsj~ERT-B=wyH#(N4oi|$H7YGVwD<-5}nI;xv6g(0>gSOKRY#t*3Rx4AR)I?ggh=R%HAwO{Bxs`_Ctn z*aXRiHx+>@zG`(8%4yI;FS0*vn_7TX3ygmFT`ib(!}#9N#-$1astI4qq|F{NAP@%= zBhZU~2Xv)dQ)vkjvJVFD@y4_+j}-)hrrgBD#N!;D_~U7%oam^G*g-3OSYzmuJcGcl zQW;yx(Dmdew-KHN$_ZHjEnWgETxfJtf;|?Y$2OenD2P7TsjBbCcW-OYpJ-Wtf>gcM z3AdFJk$(i!_WebkbX&?p$iM_Qp=$3n+2Ht~#}$VrB=XD+6Q;UE&h^N2ScG6(wy;e) z9MC~-@{|)Lb$TJ-!23J1X5V(V064Yn{}XyIz}TqM!ngkeRNsUp|J{w>(%=O=uv&Kv zoi#VPN~z10v9a2?J~s+cIfHFPAc-N#fYtaNPubed-A@XyFKyqxTgBSGf`_ME_2&NP z&{)5~wM_4CqT%Y-9%OpwAc0Wog8O`=%98G~=K_^c!OtGPKxGxU{mlDSp_4p1X^wpg;a`b(q)eVoY*Tcc%;ZaM~( z?wD+7AVEj*dhA@~R@2x9(%5Y!OdWkdxR_(#$_R!RXLP2rz~&WylRJH=ceR)G71*UD z@kcP$le-y+k-mDMZ26Bc9kcrX7pR{bFr(wQ|Gx$5@0Ly26nLs;cbJvgfkK7-Wu8HS zOK*F6d0PU=R8d?q$Tt~1j&I;GAE+}88Po1v{AiDgLhdtrZOkbG>f+JSkxGRLiG@X$ zbPP}DD(dNB*ft%ImxM6S6haSv8lqWo#P_%1_(IyL8CS!o z+-bC_wyYczBTG2@=umgw9G6D9LzJJNbZS0zgq?>(dZl5<$kgR%SgiIX#B6^$eEz`Tr7!9Y! zn@H|`CAy!Q3N!+$rF`#6+2X54M=_|Y${8y0^da70zd3gm&UVJPR{rjeV7_G|uuu2$ z^~KLcwjYfai4KgyES5_|s09jj6l=uXFbg1-^4c}kjb8#F%s$rMl{yNc->hVNF!urg zgu6;DAvQOfboQz(6rHDGZn4~M;>#oZ3^LIV$7j6hD0sDj&`+_jEDIEmAb|Ewt?}XN?&^_tOz6=w@I6QMia!O_dB!y2!5W=B@) z0rtS?v~e#d`+4Bl*6{tBDkg)R*BGDc%;LJU1Oa=M}4|HlSNjp1V zft*Q}+Sp`G`s=PPl3h-G5w+NT1EhD<1;#un$uR^odtM<_LrX%^%oTSq%g5t7 ztH+{zCCAZ~#&!8NkgZ0Mkjg%Ja;K7^9LUufvMdI-=KPkH8jgAq2zFv(TATf%bTGt` z7pO{5X7X=Szy`Dga@DPQ0ilRjP7)L>(pCota?Vo9wChepAR4;Jlxh6~K{df8b?Nq1 zj;q(HUigN~<(l@3n3iPj7@E35_$ybBeq(p^J1!-Go*v~*?yR+M>9~4lvws>)P-69e z@Q);lRmSdq2>IRE_tjBZWVoO+&HT7+^J3yS!`{`N4OSPk|9JEq?$3W#fi7 zUv$^)vqT_xS`+?vXpRG)ZK$~;ecxG4Y7>r%iO(9%6>_cBNbGDxO}$?u+IJ41j>5kK zLS9NpNVLQTp|yGzd!HEhCnjs|hz#Y%H8fm;&@;_}=6*m!?#45y`pjm3%6@YKNDBdj zd}(lFfgcs{g61%6yGBMPS1pFR@zw$N>#+`}IF}QIhm@NuRxa8vj`STkE*Br?)8F}c z90*u67T+-2n()(p2Ndz6Y1gl{OfydrT)cR1as3p-Zs7XzCH|_~mIANY;pWkz^&ehe z)M-(1<$Q`m|Je;Md(PnA+tV+$xWn{sEN&C=@`S2^`_VuW!;a(Byr2Q~kdqzd3(%r9 z?k>eI6Gf`)<P>`B8S~vzI>;v#|qjz=T>R1>_-eAjge=5n1 z8=5_d(=U6rIG%G99X@^4i6&u(dQbW@b8)7o&Xp0KlolhnOR~XtmV34}-R$hJIU`JU zz}e?C0wNO#q9Nb8m*-@mkF8gljt*4gm&1 z+T+1o{fv5n!nNvT--t|6Xyvt9um(s?G@idj7rB;)7z*snM9I1ye;jjlR6gctAz&15 z4BK&=WT14XTRmWH55+DP=u%L1h#z@ScTeE${8N<0x%r+6)w%$;I4(msw$MuKUEGIZ z3FVO5SyTihy~pqJPXPL)67H5CDNzU3shzQMPvXee?^*D}05)S}4g=&8X?dV$SxFQz zQ}2dP<#29<65WFuSqlvp3C0N&OdRYy6ggbJG#2~7*Q@tq%zE<*=GJp(?2$4mK$=|i zBjBZNbX5Ca%;zv1-kVSY7F*BI7vA~`oaD!poI9yao>^1}A-3+-7O{1Hk3jbG@CXV)A8cFuL1`lkGLd*tI!x>NiY`VmZG zNyLD8wse_V2xK`J}X%6-~!eqTd0?L zevc{+E`C^~4+b3D@JcdZ0N(6&-a?5K#4Uj7oHr^W>&|jT5tSP44OXS`-?hj;sqq8^ z-qLQmXFEBq&nw-B72rW^lx z@?cKS6ysoOLLQd>;iuJxU5ads7f6VMtVzRL56FAL(7>i=Yhz~uO*Ff?Src>~4_~ca zy+}Ka+wsiLe;7g>&c>|Gk5$kSaU0i^qr&g6#7U5ATm}_fW^2xHH$r#{Rx`w{z78ai z`ZZ5PRQLni;)cgpXD!%}|2VyHT$}>=-T==(16@sw{06@+2DG78zX1VsEP96ul4p2t z9Bfu+_$VC%SbB3w#se|}t9)j>z1zmlxXiX>$L`N4gOlFN-#_`u8~e@OuG;1i7@V5) zo>N4@((f}WB|5^88y+k#GfnibEF<;<`awhU&KJ8hAtv}4=|)Boi~~LZ8KA9boQFkz zY}JzXO{GPw{MRSPRXK8`u$lmX%6LmuUSz95S#~S*ajnlx6!q=}{j-u>E(5Gb>(z44 zse}LQ@JBdvAg__SiG)eWcZ%iaf>1jVk!?x<>tC^TR8pG(3&K?G>O;`mJ|0y`_DaTz z!o0tHc?8<0Wv~_9ouTJ_A8Nk*a&@dk{skI_o-xk5&~BNO>(SiKq4hLya!`8N?k@JG zvH~CBv*Qclau4*&F5bx8N9s%7nTzhE6P%(diIw|jk0zv;rb&RXI6 zlYsW{ky*s8!)ztCp6L6fPTZe>r|Lb@OF=<%Kn+=1y#z#ILYGnL_rz`fMLYc)Z(>1 zZO%gb=_E)-kyW(--81@UzvdmcFT{sX?0X!V}z)`@%bGtdG`NaIsC;YURX1!Ox zO#K1h&2Qq%Tm0&P($nvVQJW7=;vIpIlri)ZyF_@HU8@>n3CaLV-dKdrn5 zsLC#1+U)!Sm;M%ZZ${k7>7eyDmVsLngF?~<^0RPEwP*jQiN_nzOb7f6Fh?;C;N1UH zn&k-oGHz_Dc~gw$2G9gRVvG2gpeS9`8|@my=D1nJ7jXMCAArSAFTnEYnp*+kEEd#YPo6%7>`izkfJKtzvY!h9&I36{6Zppl z1j#0MRijA3kRikTq06`kDch0zr8y4ytS9^d)aXx-vHiSqQ_IIS>sym*u=^|IiE*E8 z8gcO~&cxhC0jba`$1Y35?g|Ce0waNv^Z4b?z*U`=BjdUhc9snrH)v)smkJ$M|LqAH zTtn0+?1Ae-xWf)JZKJ_LrjvDUeIpT>QQxk?LmcAZ;eL&VSB~e=r`pXq#^c68LIQ2N zvia}U)-dC4Ml}*r@Jhbni7ByX3kqhCf?TDheyT)z{`{VxIThIUzkSe4Ett!|#BsX8 zU~~io5`8d83^vl`wqRP+|e+?aN(w4ylsMejMCz2hMamY;|I>cYb-aojW}w zHuyJ~cr}Evz5*&kK&+YQI9T6h4+sndhk)zT>Q5+;X$i&72?89Mu?CI9v3i<8b|O3e5P~t& z9f5-q6?5@FY9hMh$D57iI%eN3*V|$+M)SxpkUNg0LVp4v15|aH^{~~WU`Cmn>)<%> zi@6{BqnAa!8KlfOET*M=;5dNTIthT>zGPoIqs~bJz+kiv{~vRn5}DlOFC%GiLqE~G z-_Itj4M~B!j!fXIooyYq4dqZTjP4c{4-N}^4jMn9=`TsEY3;|~8X-6T^rM~I$ceWZ__3{~mk-&WU8Qg~25Xoaa2|Jx&`uEq4VQir$+$ zWIa?3991s4jumnHSS?!DRcVd4w2`Gb#PxR3%n3#Htk&n9v@=Q(+*pv?B{m=c{*N=Nyzu^iDmn|p|3g(9kRuagV5WlQ1gwRqDThF$Ab^T+b_IRDA>m zf&|posdQ(f9Rl`e-IYZd;1&_~`SUFSd$ZOf$7Y-Si5W#d{FsO1;~3qZY-oSbL{|}< zNbE|&3(tVezwRmwWnpY}moSre#fOVvF4*nExlcQ^ohjyAsRha^PoBJw6ts+180$ZA zBgLx*?%#aPuWgH;`n&M25HNiV$$Y+*Kq4Cr5|=FSfz#9w&^KYfgZ`W#>s zd(dQ`j?r%as%!bys%bIb0($XJ#V7w06N@nGEKh&BlFUmLY+8a!bKE;b9$j0E>V zr!;7dHb-a7ZT|rie8=S-FhyqsQ?|T8X)qc<#PUmNXb2Hfjw<&|c#}|EzMz#L6p%>^ zmwf#iR5x_8+f#w5`#LDMQcU{O|8YId%3t?T2pFev7dh_j{PyTHT<8qQ!@dIye|{(X zA|_^6Ws^U@8G38<_~pwtuRIy?s&sPR+@C)zfO+_BUq0BS{^Z%&(4l?qJF+ULUX<*^ z`%1+19d!ElLypU(h9@WpeH3R8S^9V)rop@QP5!EG-7JR85oj@jY*ALWFx{Q#+UL|_ zSr-p5@;#{Rud9gr^YOR;+#);)?BucC+rR#lNd3PmnomR~FaAuh;@@s*M)#(F?8@9hg8^ zE&i>QIzkPrU)9xrRe^-Q;{oa?4AMaWW-h)p8+hiY<=SHr4J_v+QW?;5U}$&6F=V_N zL_JR6iH&kPg~oduH=4eL3Ly>O`_X1Af`#8)>zCs?{#BPYTDMg_ZhrtKGE4v)dZfSe z@*)KMl1Dhr5V(nrVW&UNd3NGa{C9Ow!Dgysd~dMMDWsS|AfT(zJSOZJDXSTujFFIg&p3?pzjb=lJK$S*uIW}`OC^_f^a-Cn#8&~|;ec%$je=YK3g zgMi(8H+EkO%;bo`nW3kz{qW{Kz#$Is1(x4q&c{nBTqfbC?HmorizLXT`u%gFeQFJQ z@qepa8yIX(-wNvwiQmutc};?bowZ-m_GOR$_FNLt8Lu(~&*+dTf%B;LT?X=}A4?c3 zC!k35<>GBckN{|jia^wN>1%70P(9!K8U}ftsT7rmh(Gsc*AK6pQ=buHNZ0-oQ%+-@1W{Z(kneB`H%FUKY5m9k$VS4Nby)i|*LtL)<2q z05on^H4n*!@@at!3n)w)K_Rfe-94)J&G2QW+Fe9^1qApJa30w&Tm*Tz4Gp%hGlSzG zWB)u18AAMzS<1dTOa~_t@ba?7u|#Z?52pm((-pVbCXF^{0h2KlfLUofs$;?FNLAAr^&N2nEO^|7NlT5sRyltTP;@|9 z8O|Q?<+(2tFwp>!dSPG2AAuR$3;*ce@9IU2_(W94$Q@ki*^We4DPXDo1e^ho6g?88 z>!C;YDJSgCo&BT}w!sk{;mso|C@nU;vi-``RyzuGuB(RQ`wOI;zv@oSkJ?^UKt{?O zB-mdZW@#r<$n^%Kz>aJ<$5PHaj&1GUCL#%m5(}6hs3G-^1{#I>4ex?E6@}0@M1k4- z`kCvfjTAVNczX?X2a}AZ5Xw&|C_g^v4_8pHGf5>g)li$&sMPOj+&yYzfNU52(6|9G zR~!maFF}X=cUXGol>nRv`?F_MGyfO8=Q`0=(CwV>DPaqIXs*$dc-mS$T0!%}2SMxF zb4bdPkG|h#O<`*tg(|`b8MMr6v?p+z7VhMYo%oUCu`n}xM{$v!eOaM)cSRsSKYdJG zy^w^$@yv1f^jY6Ei|`rwp3|X&*9&chlP2v&zh7_0r_9qg6^j(+069sweg?#zoo-6R zjVUKjph43}X&~k~^(oui>-w3eukdjl_tW>j?SD!q8#JGsa0~wac|m0#PBw4(H=y1I zUATPAzTT2-*%CO>$_mDoRWDt--}LncC8c=#st2FTyu&L`8})!X<(?g15E7pV_yF7U zJfC?ooqrx~RTK#yr@(;<-^QF(bgHNbj{7wwf+aWJLXNM>1}3;MDgl_I4Yn1ax34BD zI_zu|zo_L`(q@|%D9_drQCoczvaYko>jGyFdP@@hvag-1ePg<|Fc6k&Gvfs5vKXnt zBLggB$Yt_@7^Tb@)%KlTc3F>4J9m{U&EC~*8=gA|q`%Ectn9%G3ia+3nLH|Me%8vb zSqZeq5er8Bo*+!At3sR!q9ksn*ZYIe44Fd;gP}B*C@)_Cedvo-ARNZEe(PU6ki&qv zX~U!4-L=crpQNjd>b4^hM81xTdd$mo^?p3{`1F_6A zJtDy}j4^Z9XAXXX<)I9y zUu0GIQsuC~F`N?h;e2PAobnGPLcCnsq_ThHS4(1#VJR3?p8XsdS~JZ|V$@SC0{K65 zU}b&V!6<`8@hOF<8=tNiLUyjroLOsxwSNzc0CZ}8MyM3IdB5c~Gf4TUC9E}G;b^H) zXcnchWGpKDioJX*;li&$TbLoP!TV0|ALzv zL$M9RklI-33_>NxMg&yukmHvSn9_Qsb!+7e+S|xKpo7M~0M0i+Ye6%5L9MwjgQ)(^ z{Q)X0Sl&j1r4-QRA#uHF{`YHX4UsudzrRe3)ts`eJK!(I{zb&&=-#uGMkQc1a**ta zq!!-f26V1FC#{iuOQa8vdCah)1*?eqQoRxm+BJ+yWhLrJ@a( ztCxT_C&N$0>(}^i0Q)9a?YBBBk1@czbK=JsK%`Q{Y%D(xP&T&O%Ch;Bv9J`Rnc+Z3 zgatDeG({u14++6j{_MEMrrj6I>o}8~cFb#$6WaIco=TVJz7F`l8X%qsWSY#}gnr!A zCmK&M|c<%tB-y9CJ{1oLB${g~*q|7nWKf4v@-a_=dp) zZnIsTW6kUMFIUx@G{1rwh+Cr3pEUdFxY_){bnpWjN)4#+VJU$;69_DNEUUB-H22mq z__q6NaD?H;S0anUzl_wU@QMEuGfYMg#B|WB=m>2~DzSZch z5~MX$pp<5sX6|NbO67-Jj{Ni=Yqw95jf)1v(W@`(60T6I4Qigeia|RJCws#t>@48&OQzNaL&QUYn@+xc5lBHBnuaTdaZb1Ew2-b*-Ej|Ip zz5{YSUQ`TrgjuyI5a>S2rp^J*b>Ov1k-a@9K{OiCMtWTX#CblFw|O8^S+e#O23Dxm z`d>Mm*4vBxe}VWdJ#b4UhYRJ-tq&)&e)#lhB*)ESX1B)B6wQ~5BT7E|UnwERyjA_@ z$jFMX+Qv~XM1i|%#Up^H>~SgCMF^C1myg*R;HU`#gs~LDVF&Xr!3?#EtAH%zaOkjg z5J~`V)bzk3MlK}Kl>iV1gfsa|E`U1_D7<^s8AStx7#^Oqu2+-F(18Q3D+j~!peXtW z6p0w+9MGNjI#XOWcF@lFuLk24o}Q1KtpieQX9xzA{1X+VHp=8HkANl|8kdWrDHpqgkv2FJ;f9E1?WO@Vwq@C(O9 zeh06isa$(}yA>dn0RBK+S7B#s)l+Us-L{_{&ajg{%%f> z9NkeXbo3F1(!4>AdE}dIJc~cbe~Y%A0zxVUIc^4*PW;I3Zm_!UX;MA*C6ybG_oUAk zJ|4tAic$}0di?mto=d-$yh8ULriPiva?EY`V1_kxMpPVe8{qd3A8M%dZ?J|l0h9Zl zv~EZZRH`AzbTs@QxGTbQeF(axV=kUyyVe&m|HVFCAg^d`3=s_p4UOwq?Z~;2eurms zOAZ0~{P}Z(4^f{gZKrwOh?q-ZUm)F*6mAB;CVT11{Ilf!L3Tx9iK282#$ zuX;dUJcuIawz=9G?{)hRh!@Z7eiTuJb>2%D~I{Kg)#U=}ovM z6Py2)l><AODE<3&2rRa1zfN*^TLhpc@F+3nnZE9+X@G~91a!;^K%GWDr z{;*6c(5u_Oy|VAlrn=UL=dVRpguwqx@ezwUSep8-nR+1z5vzZ&?O0P++np-M3;CbL zMpzOCjF*7b0C@5q0|R`L5xecS8PFBMX8@#YmgbigwA8JkNaP;sFBhxN6@sa+&Q~2v zei}*=x&5lmz65M<1szG9+yv@3iy+(c|1@dTjr^u1t{{jn`3R0H7 z9P1n#{kXD)whQ7O9lPe){ z8NQ`1VG)ewkEc22is#~Qi3L(V`G{SiFHpH>Hn>e72wS}k{17^tZeOOv2w^bEkQWjq zZ5_X+RLk`krK**!ryKeKtxfg1$hkn#Wwr8(?b+3TJ_SNTAehd1i9* zN!X7>KU&dP&Lk+<<_n8UZ(70L){dKucO|n%w(_oTvHl8U?c`si8G(Pmj}dybxbqrp zz7{e9bG10u%5eDT0%{nH%UG=@CM_YrBq8#{hi8lk&Uf>Xd}moho2@NQ7$SKmbtA}N z`EG_ypgi7-g5cj%qlN20I970IXzW}m(q)T!v%Wmhj9{AL-vq8-xC=$==8UkP50A>Z(A=g8iD9fW-<8QX2BT@!w=M-_ zCY1m9yHFx<@5z67JkbO*{74|1=-TdN{Fa1fZbk-43vGz~tu zH;AnpEfhi^kYM&rLK0VTZm%T@%D>f|4faf8N=m7ZRj~x)puo49ZBcD8u#KN0JK5tK zIdJ>?YE`w-H=q_B%r$n7UkYa^&RvP2C z?I#qtV=sAlxX~SG$Ok$xU0e^ogK_W^7Q7p3E&PJr6i#im)CiK`tW*s%sJg=#^!o|7Q<{BV*a~IQz#*z z&(0Li6uKc0M>@3B!>An0ahDzfrtflgcK3jA&FZaf^0x$9G7gP9JdBud>Z%JK@7%kR zfgG|~;dFNl=%D-Z=SZ>XOjgIoBvc3zrT-Yh@?k{qDW37&^1AEA+@=s76|SE&n=zwk zX>8vupFPz}br7Yh6wV*B70Pd_7CpSha_Kjm!ZB&`HfH!ZddqaypH318owFgDVw(d^Fc zUrpO-Te-bCD&;`6T-V)QZGBHvy;)4tudcPVH7D-jqo@bTXS^z}KkXot$T%`WCK57~ zOa3$)G&~}8i*{b>n}l+R$^ov=+6x!F1yQDcXo4uRNd4+ZYUIx5131jabh5aZPcp2O zGz2OEb-6d@pk8j_gk8%TJ@hKME)~&M@keinT1Dqup#3H=%TW}OtwnCLw@`D3X@K#v zL0$E0rU@|m+KD^^iEpZ%aR(OpAAzrG)9mMGKqE20b#c!32JZY3!$F&1-(Ut;hsnk| zm%~X1(f=rSY)Hn7SG~up(r6C)dYqA>1askIXvA}@738}z7yis{htG0J$R#Xb+S`FHXR^^N)&)v^!C|X9~ zxWBjeL8um(c&TPcNPPGhcYfSPLugktxKZq|UB;rn-fNf|nK|OU6hnqU+edFq8Fnlr zv`;Lk3aP8U9DHmeRO7%u8TpErW#;09h-Iy;2F=oUaVXA$H}k|b-Wm*;=mQ^G zRDl2o2soY+p)o8j<@>zsOv#-SomT_m0t}HHj?V3dy(Wqdt^;K?bv5$XUrrzwbo5nU zvZ*-WA(HcKQ+JsyH~@y@?jUIo+c(ag6)X$5DanV;=O7N>UY#pN43&IV^K?QXnwhgfClth%Ok!vGw&(5c4ag)H3@6yrgXKd+~fWB-&_Rz z`6F^n-L12dhm@elOHmoY$b)Z$zjlaBKMXyqWRS;>zG#_fk}E ziV7!Lea>X9@OQ1Gi-0J{J96bwx{{1a!R94!+Br1aTDVkV^MUYtceT;YY|v-W!NK9G z2D?`tF&UKvR9#IiU|2L;tt4D2)4v`!u#+d3o@tI~m;Nz`c;xRQINSK-B9l~J3iZBI z^WqcpKSq<&L-0_u21^3*%w1ApQogF z_M8+l$c^Kf*ai}7F;UT39d>jQYab*id(!DFF$b(dXa+PmyR^N)a%E3Q&uA6tb)C56 zhHq_*gu+<084of?#G~oL1MAJWl|P>t@XzIN9Qj0NlfA;PR~}D?Up&0Qv)QkF-CG4m z9eDQb`#gvIzpA8Th;lls!0lMz%RlK%o$&u;b&l}JjZrog;oQ$|3s04tJqY=xpR-$3yTOe>^Cdt4Eq}rGr_Hc z1N#ytsa*+$z+BGX-Lr|+c0o_~%H zl2~zei9A?y0&~QzW#=+0&KMO`Us7>dX$Nffwp+l@v+Fe#$&O1O)ga|1d*se;0nDL$ zbnFaU3vGF!8$0700?AvWvN0DPUv??H`E-_c!b&9M`!BazM^x?55ta6gq{#)yyGoi2 z2ekHsid-wro3AM9FrNL%8eSWWq=X;QYH z)7h&a<*aJsV&)XJC7cyL0sXw26bFk%0eCSD%8=M+7zK7QM5iwPMUHt+QZASp* zPN5q|wp2*2nmT2R?3Z<1{FBTTM7iwCT3uP3 zQKKiEVDw;#B>bJZ zc+%zGA_vkW@h!=RKWH8?MlE0T)yAY)1_ zkzoIw;!q~bdqNi<*=hWkDsT4Y>7(V7s~3NISKpecF8Xf@$*3%=`=7(%VuKTdzpyrE zr1VSv`JsywKe-4VX3VyGD5Ci{)m;h&3Rw4aHtc5RY?T5Mawg|i#KLYh`^zA^e!_dk zbB4|=6ZmRZzk4BhokJ|n+2wLCchsBLb3?~hnaA0vQS1^{Na*2^Zrf+yLpCYDw^`+Q zhXacDras3=WYjTS4i?YAMwDJmskBwC_r3yAGpph@|C;KI$ZAjaq_~(^+lsyE%_gws zx12l|dkA;%ZarXUW#8Y&u?0BFRx@nDni-rAAsr}F`pw{d`=a(M|6UDeQ?-`~o z|70Zl?P?7}>Y1zKQ>!nY(4d9iF|85OGq8CmkX_4*!&>cRuq9NtsWGem4l|_0>;9f1 z%G9n-xHG3Hw1jOcD=EEd&+{pHST@9@`Tpx7c?yoz^w+_@*2)wQQTWn~D#Ay^i^>~G zPu>v7#>q|h>_2PTcfK|ajpaZ=8~O#^r6RT=?C zxqdc(uvZ0J%H^32Q>4E2KeeZfpgSY0*e#B;E+ZwO<42kWodjvyd-Jz`B@3lNOXw@qUQdP@J{jD6dc6})|5yQ|LcF}T z5}vubKag?W#3(9b<%bO8cMTracTTr=hfGGNdop0?a8^YjD62b`$`et`Ru6-e-Qdbw zi%B5{7i=U9-{d%$g^`*I6IPAy_8RZJQKiXRZ9hPh*{t+8 zCy{UA3%&5;4x{}&^+Kj7B#J;;f$OK1gl#NLKp=`Wjp8*?&bn5=z_(#8N5et64^ZjJ zdgNppQ-59|SOyGB(m@ z2MR17y&0m3jR0M7Ag2pM2Q{?FNUB|+wZ!;?V}NZDzq9fczihnR^p2dHUB z^xu5&eHN55i-dcvfACnAp26?o=75}y!xvA^Nxc;La-Q;v#`x%s`o?mLve|4R@y)?3 z+i$7E#x~GfGiQYeev-o@EFAQ%rLxm+Xe7NE@fkgk3&hg+)x<|_iF6XC?{|YuJAf9Z z%(dGh{q>ss{iyLG-^L@-s1J?jHyU~5yR-a})vX^5X)n%3{AF5Lg9{1w0@GjIo@m1f zj7E8>^Ry(U(k6y2ROeE;2zbNbm4{RbYa;kn7@pLR3V0*cISsUf=7{xVDtN4sM!_? zb;6Sn-a^Cle#_ZJjo}plb_$WUr8MV;UMGGlqNIz!gtpKBy`#C|=wrhcjUAp4l|QiP zexc;<&Xr!{-AAf(Oij=sIm2s_ITP38ZhDT-&D70&i$UVK2!cJs@rw_wLVw(J#9vF) z3pgP6Y&!eVDGsbWWR4EEgsQum6wQ^lTFNs1K|fG+M-wTo-7(euCDpt+D)3~g!YvTG zu61e(3j*3m>QhMgub;$hhYWDc{r>dy%q8AW&|_bOoMMWTuivcxKM*z>%mJ8=dT3oZ zg)&imYTTVwl2{6~7q^6t?m-N=%6W?0&Am6W?{RSfM<@l5u6B3(zysfZdJvwUvXMl1 z>83$q-tdZ@VlCK+t%YNJ(1xR9*!a7Hw?HJyfi;4b!97-x;71*04a_#4t08WcvyCkuS!7S4G|P5hef?mV zxT~$_0w{!+X}UZC`<=`Ld3w~q6>&wiPfwvG>{Uwvxj&`o>+dV8Xa-Vu>+BJ^&k6Vd zV`@knNT+0MhPPrHe?RYLrY46}^|cNUR+w7T zl_HR>x2_872O|HXzI~(Dn(wLR%+hC_d&4t_5pH{C`Ze)jciZmRDD)lL^QTHVg+K;v z?+@iRmb_urt-%Php?rVPD1O4K_yvkp26Qj3huAE3_DIc^SV1!H)^Hy{Q3(p_46rfW zf_7}6kw0PlwgP9}&!>mIS}Q*$CS9(*QUav~0h?_JfqK179DHs^=ao$FaGNUXc9>%8 z>g&(yjna-nU4#57i)^g06EU;ApbMbNMQXJ^POPJ=n={O{v6HhNC~LmZk?7yO4o#7NHYZye3jfI#g)>IK5BCcG+^YG<#sT zl^v*3B{nAahr3SMaekdAXqNe!gtuxs@BhQzdjK`rcI(2ZuktD?HbA7kDxh@fQe#J? zDJY0krAdwSp4d?l5RhJ?A|Snkp+!Y{2^~TUHK7L*AcVC4j^*9^?C;zE?0wFhfBrM) z%M1=PLh^7w_p|P`)^)AxDlkW0Yw63ugmXRL^T?8IFMX5UnDF}D=Ohl18VkTPd@h5^ zV8R0mJ^F&gO|qLl7xnCmL&w)XY>k`R^!uvSmIO&?;<(Y4PKN zSS+44Tb~0{zJsmPVuYgm&3gee=7ye#e&&&m>Cv5bBOf?Wz7J8YwJq z0EdoL8^+^iNlVqk2I$GxNoQZ}7Z_q|z(FcDlM^)QHiz-BuhIS+jvZ zrwy?Jb8HWDzu&v?IA+cWKQSoJ2ZF`VzVD#8J}Rnq^LF)WxgXaS70%(bM(wCbd~Rli zc>0G#i9HNVio^hE&gQm4Z0R?OF^S_k;Zdce)dp{bI`J>rBKM~ui1%08#m|9Nf{@Ym zO{al-$!V@PB%iS>TM)Q(RA=WNN@G`xNgbobpEJmgfZNHEZ4I4&lp(kNqbbPlDPz}Ath zsnec#3yLwg^nItOJ}fSjCk?27sg~egH`ZM=JNFBpA94GA^gIO)5;`|nHp8(UDa`ZW zx4L`e_oOe~b{0Pw$DcOP`(!QA=M7Arb9K00m?pIhl4rI9hYo$mNeu&QkY_epb(=KO zKIO6!&qhnuNSL0U-B@zHg5#E7`K^=+aS+Y%bD4bd4*jPFySoc9E^SFQ@%HVhU^zDf zU$n+Zger=?EG&#>GY=1KgoWDPzdHaOC|Bvp=nzrbkbAQZ!wE&uY(EnqJ7e|WG*)rdekSX_hC!t%vkC;EC8-^fNGTPlXm!3!kjhZ%c0lz6N3j^oqdponSHw_gx0H{eTeS%QpjK+u_M-Fk|)f z=^v!=CuE}vHz{0)>FZx)gAv%T^yKol_P&_&PXc#X%O&t5IG&*DQ^=M5cPZiWPpz(s zf@CQqC+>qo=yml`3%1!ls+Dfyna~uqjqZ1MHFoWt{W@vxD!Y(zFaF>pN;rn@GpY+F-qeyEwp(1B_SYSQ+|jV54&5 z{e?f#n!{0ZJvhF6=J&&4M~Oqjl4ct!%gb(`k(D=O*93tlUO2$@ToKFxD*}4DxnzzO zWB`Dko?MEN3#Pd4#&RyRQ4o45AYhk~DEhdj=C;rX9@inI#`O5sGW~pNr8^pq(k)0W zaJOdO1rrS>SJQrFuaU{|Epal4gqtPsN5?2WZPCw;=*QaYsS(#>`<@MD{OFDO1I_z5 zt9kTfy8NEr4jq{dsRw-iihi9}Q=PGiT71fFCdjL94Cr80b@$Ze?3=rHesR*XT?>r< z#5`xAs=;(QsP)^KK)F&Dj>&aw++G%Ej55fdvAo-}LPl+fkiR~$YF($jJbX$;(|PC5 z?x?YGPR#BphwWf={j}?lqVi4XQsO=khIWE*bbpML=qP?0c@_Bm$iIB?`CL%YcBQ0! zpiy0r2|EQg@H*cJYdq}dmTAKN(jxa51|hZfHcHsa8{v!{TJ>-0WJ#V7eSy^jkTE+L z#Gu4UT5a*xn_Fbvey^T3#(U-J)dx31eqTP^^JiQ5#kBS0G8PYVTo;e3U$S!f zy`0}1{%1>ppS1ZAC9v4w&sK%inpZSG7%g*FIyAy$#j7(ag! zgLH=14oiRfHj8>k@{|C-#-qAE{!Cxvejjlsh{Be$!6Ra_vPE97TAn~Mf7^Psz_72p zSJNl8!@aK82UiWK_XTxgbH(45yD0IcjR|VqyjeP-dYKZ$-A}iAgC||EJwm9J-v(fV z+WDW+i&h}y#raL7|ERvAlssN#8wOPl_G=k0c6 zf${Le4ejG$2X~MJp8UA^O-%8h$WUa}f1!r3M>C!tMkU{CdCM8EFlQ0Y@Jo(>SXzDx z26F5SX)pqORQ)3-An(b{UuT?R9sOcrVqUYI8J{j9r}k_GF-Q~#Ac|>?Uf%KS$WQCp zCg%QMpMRokY1rb3r_>cd7w2-S3WUGhX+0uAK0J*-|5}lYCz>}}ET`Iz1TTKj<#ml6 zP{qATK45w3W@Vvlre=joP|Q&-p7b{7BD3? zsZ^Dez#PR$JtqA*h9r9E`jF7W^9A%ISa3*BdMjjQO>+Oa-e;jS>;mBsW5ID5dFtuF z^Nr)P5i!}Ix&U!%V!GK%3}^T{Q2go2y@BwXS%;kN2jS6L#ZEIk{1Di7@ZO1K*|M`` z@Tn4_=l_pZa-fN0n|w+RmLbNQ)xLd=?51Qm*~H3rsv(bm1iqO;ZN$*Hy%@>$(2w+LbEfw~?Y&{;s-yBg!u6qn zM_!W@}aLS0EUNE<-u#O;@~VyJ`(I$Ha6{Azmq zY{`6lAuM8_x4#NFvx=D%*sB>I75^h(+nKc~q1!d`k0;%v->&(z8}aeg(h?a=AYOip zlx3cEVyZ!3*<90RYrC{n_|()+Az3KH_JqtOS%>})F6%(0+7G^FD}F2gOB-hDsD}L? zARh++J!LaGYp;;(n>WO-xwjS3t2VYAc4_Nzl9uiAX!VZA&y2Ee@n<>d8 z%W!7{{A}Ni@*vDHb@dU`Qa@3IWa1T|qxnDz=j0cJKOy}9OZSeKb(RDY@7t`qzQu%Z za0=2M0i(dKGzSyoAk~PjG%Wx&x!n)inIhz{%1T5OO1D!t1S+`+kLqrIx;<7DR#-w^ zU{zE&;Zno;q%FjMEQhSm)XB3QDHj-rD!gOTw;yJi&%_l}6gf^+yoSO3 zQln4+&z@OK@a)YsUC)p^$<&bjazQu_m!pWTEHKg0HsND!!rLtm6J z<*qzt6|hDSpee-GcHoR;Ceu6pNL&OcCV$HfLv`EHG;383#bhc08APoI4@}L?+e%&f z-XiMs-`v;pxZ_rW8LYHpuc7+XjgF=OtQTZ1cC}kh5_66NQM$l(6&Ow)1y+Iejg6DH zM}0HB=7u5xPXo}F|3E9iUzIp?sI>G4)jo0?wfr(Xx$hhmUOL&^j0J-z^^R3>GK~{9 zNQy-(w-xlBjf@%$<<9u_O(F;?P%V9Ye|ibkuNB$mLLDA|vD^pl^5_ z8(#kaL|6fNV-D@go*#;BTCeIO-Ter2LmQkY2%RLE-Cl3Emj{SC;NvLv=KnnNZM(QT zS$D`#7|ekmUP^WTnls8ic~40m`W2|PUpsP#njWli6ie3(X9%LREe-;hXX44NyEpf3 zhzfs^M=w2l%lQpx8|&$QANKG(F}vjv7_mvfLJ$d+$n`-I-X!^~pLkS574va(n_WmH=9Nvepy1iageGOFD^}~ef)XZBI@azW$fA()) z>PQLwXpYQveTPogZ0yU)`Xi2`qe#DZAIVuM3D`#O28L!ps^o~KSy^hu60@{`1Xdqb3TFP7eU}Txr#(q5;v#9 z25W$@J4qp6BU)3D%eza|8B`Qc~3!I}D3=N4=D6a`gO$U!8?BfnZ$!1t7bb3qc zk~zo}eUW~$iS9O;^3EN8U@#E6P2hU=5m}X7^LF%lXv@@(h4!CEv5}d1spx7;rcT^F zF;H5gs(yx;DEZ&5gl}NXJpI#AmXS8TgZbBq$?bD4`JE@o0Hy~>7YiRuT$7Vq{BB;^w&k5ffw8LxpryrfTVV!JC@y#)2=Htmq(;X@S8`pbO%uMpjEbudh0J`T zgo0u3>*8M!%38JdenG%w-iFeR212j`Tp)#8ERjmkbgfNWwuw+a0UE?&(RfhLKdf&I z1(S$VhHWBdLIP@~c5AcR)q_Z$nUeIKJ8K$JnL61?jkg)G@d=9@tS;@ve3+6(t^e71{)02aD<{yN| zgB9dW4KdD3EChKZNOaB&yPq78sleBn20NgE*>2`vWVjnTvugq6G;;@#>hHM}|4hZc zf4x~7L}~k&NayHns<27*f*A@bI1fb@ROCwkl;@xB06lBiCy{&T&8x z#tjC{XejoIzvNP;`IBQpo0v-fK#~Fzxo{4i6KR_Sc*V-t?8I+3n(5y{8~?mA2E-w| zB0qV@mOxNA<4ne{yd|> zzyB}u-X6oirVM}{bDI>8QPDc@fqcyPCuPUUw&9~QRwnlLq9*wtM`Bq(m@Ph^PI0R$ z{QmuWH-%ye=4%T;A34Kb?5*mr20LD67h=*KwFej(IMb zscE9t?qSsjP*I6rY3&n>nt~T!wd?nJSb8}hyVia{Cv{j{TsH2WKdGHFUf$O*vKL+O z_U)0Ovbk#&%PIbhXLz>Tm41q#uD(~-FatKXYR5WJ{OgYO1u#=LE)dq0@OrNr2e+id zu(DcFdissDD_o#rcn0vP9V<@cN8e8gkKgk-4$Lp^lsR1pjn#W~*6BCH>~|U4``?Db z@OpTEKff~oozl9uUNBkGx1d6!N4yqba)ar$Owf(roLDJx!B^8at{dkkHhy`eVOq6; zy3-~8<1U{#0pI<*xEv9%Y$1c)p+LtV0zb}=0Oc=@s! zxL>SZ$P^5BmoigfFHs>A+<`>ezP^ggO!>Ps-TjM@r}dNVop4$72(EGSP;40d}C7zRi#JQp-afZ7T=D+3C9H>kcc< z(4N}h1^&aMD$TC{S($4itH1ylZ@h}lHejUk4S*AOytDaU%^7se^C2N1aS7|sqrP&* zF5YV!j0D(9WajN(Wu&m$?>AW+D>yo&28k+yf;Q`zGV60UPta_%={@) z(N)H5^UZAYM+qg3vH@*>f@T6h5=g73jCeKg{dIii3BYqxry5O(JZH?|&69oVq_!!^ z+h=2d^uMg{@;xm~w!uJAaHFNDfq%qh9sBE5KKp5FIvavj;yqeB?`noKa#T{VSVo zT&O-go_NU~n3|Es&9MHT@Sj^@>1K7$Uhc|)vK>Q`=6C6AS4z%w#zBh^- z6{vi_)v?TZ5=dkwp~f0A!mx5%^uHtJizC~APDy;c3Z^~&w*zH={3B5I;2(gpfvC8D z3Y0~8}HfY51bfQsYryR zkq}9sJhN$ND*_7aFIoNGx6Pi#7S!a;t*rxmhfPjT{nKhtPMra*fN250J<#w8k2BeQ zOxf-JY3a1}lu@B8`9fcO)iw(6cU>D}KKR3O%<{R-PsQJZk!C&r3kBLBcg+BYn9x&< zOG9G(i;5ZCrM~r(BJ zvE7Es>*+Tf_dePI&L;G5TIvbhYH#WGBP?Hak{5(G*>`*s?DFC@Tbj7qqR zE`Pjbpc2NsXL|$`G;jBu;nuNS0MGMlbJRbSVJ6kTqHfeLyBkB>{sQc!#&B0{y%Nx` zFb3Jb$$eMm0l|>;rxIl2HCCmy79a3trAb&?S9jr?*FG6+R*=h^1eMpmuOxV3`8{>K7I1cw*QL}4>^6VBCG!npls-|}= z-28Mjion>mwcn4+B(9o?xsjpE=Ae&Aw{Oq(&o@&&^X@v1Y4G){oxkfH@zU9>tgN@F%;fhUr%{m1r;|N=01i~7f`sDf0hSVI`-+MqigR*7# ztYqs7>#>so^~~W@sMh+>uGf5ODW;wcQxor{eh$@Ky0quoPY~^DB$!!n7ZZQJKV_&J zNrJop-T#1qEALAMob~Lcqiog^1KwEn?C*vGCURB$QveFhALnKR5qYrMP7SaU9xUEJ zih$QnWcUD=cOBro2rjlQsU%)hPY7M^v|&8Ms`2PvHn_H}Kg{kN17)dy2~SThCK`;p z!8zT5J6w^d$U$zOLl0ZsR|l6i)C^G;!#Ld})wsdI)^q*T1h_gedZLzjQjwb~n-44f1O%~lbjH&cz2kYsn%V2B_J2pfh&S%0kLoyv! zFYsc2plp7E(mznPV;7e$^j{nKhni+jS#1fmu4&!5CxaI>57alu>AiVop-4~eU*jYv zc4q#}CJlv0%q_swl`@-Xbn_~(1P1h}0%w6(!r6+?hp8t0p)+z_l)iY>a)!H zkGo3-kv1}5eAVX5L&02zS8%xxjdo4WrRg-Q_xN?!IxxYhnjqHFt?U@IbT7C+8(V~e z(>bz<8s3o~K>Yx6YmVp4U^{TVC1L`)S1$Rw)K$8)h1w*rVk~@5cw?{A2&T)+(dF;l zQm?#$M`(<_SX3hgh;CU^-1NNab*p>9I04BneajGN@hz$SjY1N{O>vG#ghUI<35mR3E5S zj8JlmFZf%?4H-BRk81q;89TMxJk959%7DvEtm;le193uk0|>|^>vfT z>1%nqD!=WMvEo~$r9{5$3ZM2}-|Emn7$=@d9_PN4k!3yeD};dYgT$YMl>Z6x_4m)2 z{$_xF{oYLA`t+NBbLr#h_|TtDRln0v;K!Y>P`O3o1AdHx(7Ho|N#;y2Za`8FDWUr96;o2X)#~4s=XhH2 zqNKlD&ZqC@uV&hnA=Vuu+LsMce_cVzKN1S3O>b&wXzTzq7tn=YS*1|TpD-`8qnMAf zjBrJ^jvF$TphAQI-Rg@efzDu7wQZ=3IrD{4H|SEnebh!MJmK1ssv;OJOu<)LT8j9+ zgIRzX#5P}MMQ9|RP;d+mhNQA@<+MF@m-n;z{sky({`^CTSAP*1CG;$*99Y^>JwQ|2 zwk;L#^B<}3!R@Z01LAcr3#7>Y;=_#v1zs)3Hio4IVy3>mZf3{!` zZ~ubyj1(3*WmID9%?7IfnTwB`ucEC9W3&QGx?7{5p`qby=X<@{d0=n|DhEUm8Lr0b z&tSByG6BftA3UhGhzWjpB|_o!{DWT$&G|Q3Z_Mi6wxi@0`X3bk12%f~^|n63a5E+5 zDGnaJOfPFTHZx_Y1GQY^wIFHNo3hIuq*%R8`_}#6>I{q+sU9r8-n(~l&s&TP4R4=; zs|0QcvgszmlOi9b4Pk+#cNRpB( zFt7>hyv8*BJx@@Z7hd@19D~@O?a?3QTwbL+qczQ)-9WKpFaxnj=%}K4rXr z84}d93D_L*iZ%wz}dsb6Qpz$GqTWfM|=@Z$Nt`H9IWZ^GNt6U|cw>W}Li z4d4B-Qt6#E%eWSo6t0$pVDfE9JLqe%y|FHnn@<^V>?}-88v#&#%d|?eIz^)1{FS@G zvjdikLy_XQE(qYKmEawzSfIFk5)Jy9DWj+k2HnGlc{T|P@t*5`G}8((`5BXdh{P4b zkMX<6D#K^zWZ0g=XghLzuUsE#e+Jmk{!TV^aaKJ{h{kQClV*&>E!&a7x#z78x>jFi zTHS-H$4>&M5#8x$r*Um}-sZu8vppdqRX4qbD_$j#KdtdK@aYkNhAIoeV!|m8ek|w|p7q?|C-LaouXZQ+1#mlze7_MM^XW3Vv|l-vFo2J<|llF~xv4bavS1eAG$xn@!qZfMBY}Mw^OL ze{O(LLJ5WPV8kpuP?d#i&C6Jo@8ab21*S#Kr$RHjYJ(-rLoEUsDeZ_zJlU8W7l}uk z(vW4)rzI?il9BcAaH?OdSHrcxpU(enfYgOf0T}1~xO@R5VC(>pWkr{ic{UK2oaWMw z=bPXVQIDE2VYJ!{}a9-0qVqPVyq*Scp z$y*o~rP|Rg0s0pCOHavJ5M%F;e7;-mnY@k)=nJq6$~x>l-}~KG?%?$ygwN8Qk=}v- zP@_!lxZhbQ$ilAZ8yQi;Gk)sg*1S=q3d`=AVA)~*p7Nn?ktorh#rbsp+C@M9*hBd&>)ky3Ex;g8X;@>n+u@c zmQy*-WI8{$I}oz%{H4o(4jdWb(rwf28L2QN$SEqu^{yDZBlYxJrA*oc+%GGTJ+gFl zMWEyddOFcZWG^e3?&94QPoYl`Ql}-p{WU&b)i_o8IX}T4K8CMGQ}&w2X+rSHt48)2 zS_2!7py4EHkUF%sw|#5p_n0D~*9K8uX73laH{uoAw1b0#HE9p7#hT^YMA?pfUUhWQ z1!Vm0MH|pta?Q|B?Oz;_$*1)7wPsXa^wwumv^^~pk!S4NKDCNG+2t_4s`b&Ls-ACu z^fb-NGFGZ$NL_8QP%mn_&%_EHZJ@+7Egh{Yc&neHYB$*Ko9?|}tU?|!N_>o+JjWeMHBnVu>YoYQcjh=#_(5Yqbu!t<9Vd-9q00ed~mY_<2liAqVZmS}9DzLHD zC!F=R*RP_)@WbXjRcOU{Vzh)74q@Zmlb#{&O#Y-DwKiMiA$W{&oAo&oGN{-p0&Lu9 z*gxpIQC40`YJ$9jebe}Rj89b_u2P&((vXv#P0WM3CVVv+eY0g+VrXTBIBo)1!^6-N zCD%kI|Ku%WQ!yiFXVQ=}R9IUy_^islefzAMXrsQv$Kxyl^guuUt#DR~aZ;Qm&AEge zJo+Z7rvUpLV_oN+q~UP)HQm}A*OnSB%nNb5;6`27;80|KWfyA3B>^1RKt=Ravcg}^ z`DI(?Q=tJgl95r*@rC>feK)|#ZA<8RvwP~)Ol_c$`6oJ|wlN&(KbzuDCqPBD!QP4D zgKn;ySDYU69$4y0(;}}eRoze;d}ol)J3@1464hq5Q*knIf7z2-wejt><*Rs&5p?hS z4Aqo4Lh_BYvD5{8y9$KEW2vl;*sze+JvbO|7wZ_uWwIRGVqm^c4MnObW!;dSbgCi> zi+HAjf>vvagN;)+gs{_-g& zN2@+@ZI8`^khXdr%a}$j^%z)^YjRqIrh@Mn3~N3-H{y>>_M`D8D9qKZEmgROpCyjl zM+hm7zcCM7D&!9u@6wKv+9J1p4Oy(Z-q=-SqZh8ZRhS&uA)+0X-|E}Z4AD@sFu6Z= z>qV#>lbuRP*;pE$`F-hP5&Lt%N61#+nmz0pW#4`sL} zm798(6-<9Z@Fz=KsLFJ&DmQAM+Sl~%yom#rAP_64QRP4tC~sbwIGNbMQ_6(8^g%$! zv6=Ny5~szo=5D80fK8Kkr(%n&ues`ut%C(w?}sdYR;RX+<|<4{_m=mJR%TpWF9tef zw_>ee%h&`Gx=K=}W^JGsog6dQlZj`i3Z@Dp8P9c$Zr$ozBo&f6hH>G;uq3dR?B92w zAG=g{ZSktEZB&yBiojOak0lI=WTyI<_iR4yz;>^YePP^dgn`E}cw!)o@OeoY;~(eB z<6i@wub?9FfQ?1W?cu;LCtRl^Ju57ATPcGZ!%HcU{V2~Ox5I1?Wh^gVytrT8`rPyV zRreyxJ-@)Tq=-t~{aSnEx2s@W$$#gW?W0 zHd-bHPMrbQQFC=TscbHiBv1fbD)yQq5;zZX^81YhS*DnKb=B)xRc$}Y_E5wLqz%ZG z#}SAsN?)0AYkn1>GuI!5#6Nm{Y+XOfcBoW@#NWQt(fl0gcIR20%!Y=B*SsXqf=-Rv z+S*c3P?&CqSH}R(sa+3@uY8TjNV{olj9Zh}Z$j(S?E=WA5~G!Qo!vSonga>&H(0ux zUx)gF?J5r&+qlkEwq0!2FSBxU+n|Ihif8*MNv%pDIGDDZ#JnUd`TEtXSINO0Vo2&T z)pMb+3OKnYc|iv`g(dH=0VK~;62P&LMaz=s|_p66`BIKv$3t% ziLmWp`=V~HFxz(dn8RGZbDtouUo|JUq}0=f^a%1F;0jt>;a%Intpu+l7r@2*c%6RA zw)f|E*Z)NSxI)MK_nYc@EJoMpCIfe*s+Manmf#q;RCYP1UN!BgmX_(ykC^^wXIB@O zw%5G!0H3I;*MyH3W!gm$mZPLlwU`gm(e>8fZ49QtQ-y!bfxay{T(xL&u{+DNaS_GT z47CdcANVCM1bpz9L+eDH0{huQ3GRR~ut(D8fcvqU(|bHz!Bam!`2NjQKkp0k^L)kq z;(aRjtNOaDwAwE~bJBr7oNQ)lfc`EkPxsI^t0N9{+PCVJNvotiZQ{zsh?ycSM4Ks|q0rn| zadqr&rYk9P;v~40!vOWTCAYG9vV_;MH!g#c{dvt!KX1ETQ zYXg!wq~(TIa(qq!^326u*yK!vU_y3wIqPiCLYN|K7}U4G(N&|Ytc=3Y3{esokgC|H zZ)Z0X6cIes>)4)%Jrn37T|M3c;#gB}uUFIVQw(7%i@>mm#$(4k&=zDoIPltYO-qW0 z=4*)2+aTBCq*HjdYWy}5@T7`?v8KKO^T97W7+mRM~t(CsMJHWPGYW!>Ca?a3k5=yMEn&8ObB>KKUIyz!^r3i9x=I z(c%HiD(RI?p3>T#39@O@CYJ8Osr~`Bm0oknnhSXv1Ad+zOnHNdHmQn{7y3 zTq^`hNYbR*J>;=K<{K^M8cKy+-Ba8qM?$6hvF=TC2HyE@^%EY~Gz< zT9s#mjJ{JqFhbL+T{Ouhe!vQD#CrT5rhlO^(FfVsB#)Q%Xx6H(r$yL*bZdBxgq5FN zn>S7oR+*^e@co((nZATwomFsPj)K8BHl5Jq@(MI;w*E;H_+)$jUJ^ z0({T~grnVS4(C^uwM+0=^|g*5>af|&7N3kB>!KA0_PQRcn%W2?4Zg^fn~jq9>}fZ# z9rYe)-if!;EEsJcSep}?>DDP|c5f8w%008Cjqrv=O^?bF?CQe$Te(b9;>_S0XSF?g z!YbMo;hps+4pb=IGquqdTf=F;rbfUGEwI~xAkb4wms4SWkZ0W-CF&-=qIM^eMqH3{ zoi80ZyGAsB(8ZfTn|O<49iRBrz~fJ(YsEtZ%i*wK>YHTmeleh8xS>v7?-UYhe}aMw zJEQ~?w@(q6b66BK)qmre!6KnP$Tbv(plp$`%)c}ObAg8!I78TUVt+hio9!AXw0mQ6 zqLvUHt4>=%$gd3ccrfXlQsqm`MQTk+b7JcZUE3QuPln>ydDK%NHJe{AxY`s1`xuay zCDL!@v@DjAd?>@t1r9^KLp#F<1_tmG6DK{s-(XMA&K7T#?2!%;a<^uzgoMZk0-;t5 z*QGU=swL&(o{r&ko5n;|K(F_qwXZ&*lRLKf*0jCXizyrXho@7z2ufk2p5z@Wv+oF* z7xhAfn|*7mJQ=GE5K@={qo)s3ro0ZB9jE3443=?$@@&-F?XvCHETp}g(czU&6^(= z_E;2Cm(^fSR&c>pTer%&@??6ek>Tk3O49|MC{~h(VlD}RF~qdCWEFXM;8K`N@#-#8 z&=Sb~B3q1OFb~qalWKy&eX(wK=eWL$PG(x;OW$0=5jI#XuUqg>~xoI`(wG$Mqavik{jR zjaPV9&n%gIB_XWc5QgaVN}3g(gCKIm<4XHo8Fa*sR`2DvrX}_wl(R1S337^YR;t2? zAZrhB0P7Z{q;4!F`Et#XPFjJdbk1&nq<*`l4OoJ?QHQ+h6`F+OaJk;r znrUDvftm!ZjaewD62T{vdh9Jy_Jf6#^xqD$aVcCaRj_R=kPw?tGEW*O2z6Le7i$O0 z>EAwa8kM*t<)WN2Szr7CbDq5GFxLiygakp0;Y_`x@&t8%rylu-3y_2Cc)LaRsc7L* zYjF%8JfZq^^fL!@#v>EniRR zfx`>~@`k27mjWAJSDMS+f3QmsehEk`TAr~yif0mwU66e^%-s;zC}CEJPYog>?a_p> z;B>F~Ua>m2T8TWfiWVe3wfo2-Pva`uAah1F9h_ZdWj%Px`w-kByq>Gder=fW(U*XR z;g;iN2{RwgqnZYRm@NpE`0TGdlD&aN{>)ll6=Hg+||Jc(NQgS?Y_IUK%;zN|UUbC*m~Y z>SO|?AxIBfhDy1Tg;RF1Peh`yc7&h={{2LR9r#4E2KtUh3w2O(6D=Swj zsM)v@W!oJR-P75*@~jOTTp{3rNguGC{I0F73DX3lHHt!my=P# zTwGjwRxZ;nsmzgk- zG6eI?pH=nAZSK+%iuFVzg|)lADIF@5%*Kp`J?&Mm{BGcw*324Krkm{d4K1Go^U2N` zcBs(sX{$zTSF8A^1tfQC-v=#z|Lbl7J|@f6Q)6rMeJ2AKi;SdEi$nHkCQg}(-<;A% zgCLp$aw*5*3|Cgxr!s$vJrigkhI-N3-Mh7S$ldSk!;k|*(t%S61{UHf7qs~zV|C{e zlh14&y@>Jj08U5X(Y)rQYV0sD>T@-xKaWupZ4fjbcJp$7@NWUhce|6;;A0MgJ6V~Uo9t7m>Jz5J27$bO4F|gfM|NpM& z6ZrAO{ins8?T=#43f^^@M>%+jwOL|60TEH$H;#PWs37mWzT$;tH3jD}=(7J`X;*5Z zg%gyPNDy}48w$pfL;rLHFbJ&x^HfK(Und zdUt>o+F7r2$N%+)tabR;8nRa5KdJHmwQa!C{Od{iu|xj#r2Jfle+Aed>u~+ge+7qs z1&4p7BCLnuU)k4>knr!(f&a^cgJf4|EhrJRwJ)8&`D1PVkH{kKRIF{+Yz6Sx=-Rqa z2p02L;#YaG>2X_pN8vcDDgm#;m67eoet!G^vwGsvLM)(Q{CIuowxup@xXpD$#Gwt?#bKAD>=g}I`p&@Pkz ztSDPi>Fev8$vTB-eJf^U79hs@ac%V-ph$~VH7meMWd@^sd&maWO2;!tX)X7#2A3}z zMt+plwwbzzrNN&r6x1k^$EYDLpSvnm2(%vjF=rPSU4WL0i;ElHxY2b%LK6L=F||p> z!K!;Lm?^b213<_7U%9?^FI5E!#)D>(|2VlKLoZ25s)iq6eH`Pg68ZM_Y;pW_l|*fV ze1Cu{K9_zee6yu>u1uGde@H+c7T^Y_rM&jD8#!b#l}|VBsKdLYgc+N;y^oDx+lw@2}5v`?hx=abIs}M+F^+> zAu3Zum(hn)T0{L8@_dWI#VH2r59e)V(q-F2!x1xP!?2Usp$$F~KXv@cDk887&=H`< zRyKVu)y-RoKt8LFXio;DpwEO5!}{W}^b?6-QnEyfBf8P>Q79}9BU2qUUt*wdhu2Tg zAkMVGR{Kil`)qa>l=@$TFs(j6tKRW7Zixo|NQ5X0v_0|3Z9p}IP$lbp$?Kp5L}@c%!94X*PD_Rvxis*vNxIhuSZO$S0Oz zacx+8I#)3{R->oHnW~$pBp*?&E?pHQPFvI;C)A69g zKRv_;i?JIjYvm+E#7pe^@IV1e58u&h7cshle$DHx=UGc>m+~NrH41A3H<~%~pb4%7 zSsVMPX%_O|v#?BA(lhnJY;c&@;mp=rpYfPSo=Oc;3K6Wd0*&B`+u^S6W{2<)ZGhv- zFXGflvuOs?02^Z=P92e20&m?$xoMAHR903l)-EvShM{RJRFMVN?UC?mf5_BSs0l3S z%caM`82HyP<-GnoQhCOvD%#!ltp)9hjFA!<;ON490}nrz0^TFHYD|^)%=jBGi@TH& zIN=Eka3al_?Ea`0Av>=8<4Y zGT_Wwm02xnMr8m-m;{GM*)9&c^Lnk`@}FA20RWBl?re_%Nn;WQ4hauJ$hcpdpG-VHG^kt$=RPFUI+Kf>P zrO6O#K{aG<>bCl1j60IP>gz$BBG~pXJ{NBG_ho2QXez0yku6k|`KwH8%YEXQ1vP;Y z9VOYMos9M}$DxmLYq6F_doDy8FZug<+I!wh$so{o z2zLlccnlRofI8UO)}nHy+Obn|PS+(h-W(L6YG>-U$3cco)xZM5)23F(e< z*J00AWw-VeNTr0UYm(=x2mw9YHQH)t_26ruoI^*TXG_+p>g-_pQ+E|YH9#mJn%czW z9{rYN6+%1s(z`tKWdob6j?`2hrOkoKAyE^n^=2TdG4d{`;_I4yA4J*&?76)ynKB2e{wAvKoMUX z6_duOJ+Rh1vFhs8TaAP}g{Nh5@3(=y8(B0pPwadWVb9<}MG33WUmF=38z)*Zh!d$K z0(?C3ZCu>UXyD^%WmR~+C9~Gc+j~YHk$~6Rt<`e;)D2qlrlL*oddT70KxI z5jld9Rny@JnbtvfZ_G%!imU66Vc59x*q(KEHnuI?Zg33v33RCCYYtfY-Hy#4dYc6M ztnLi2sTo*>Dhj2B&d1?9C-RWi-&&G2GQ^nW6K~Wj9nG%{+{ERcY;+)PSWqVDGWI6# zB@Szu%zWZalXImN=H5Z&p`AHD(nTB;g|!*Pg$O(aC#4-lERH%-Sy}wSU&mMh+B{u3 zuba!4C*&Cvm@eF!wG0TS1Illl5%S}{ zX5xa?0gx785$wfi4955~qf*gzZkv8B00PtYU{D+iY zbkN6Vm{Qus)9)LJtyIDJKEl@!{U_~nth!7 z;G1lMZygpVlV2^_hI#lEcU?3S%{R0~%eTv8`a zGsQy9H@&=ER7;4IuUGDjc#z0R$?X#=5CUB}7VVu3#DSEX%BARyy6hJD$_zQPA>WOsDD7frvh&_@P78~IAQX8C zD+x^V+B@g7X=|C}>2A(iNDSAGz~YSljU@tXs8O_o8sE2m{RYmT>d(Ej^4XxkNiEHg=zOW!%>q z_c5NpGEAlS!b57oX^Dk|632Z;-NkMfl`354GH*fa&6&l`>%1Gy5$T=9noG>h6c@_( zOe1OxO8eTiVgR_&>L4RgT0*9Y;o;$_N_lxnay}#RfM?+7@7Fx#KmJuq?nAlgvkS)^ zlHHx6z9ykQeqLbSxpSuyTAbki;9}G=>azTPr>2ImJV!^aZIe2z~c!G<`ckHxV_RnsJ*{!3yCfrXl55+<@j4~Ddwu1( zBT6T*w*xOF`tCK*u9Pdjr`2EETeC*Y$V*XSKDU}~Lz363Q_ah=G9v_1MybpFeK$8M zec($KOM@}xOKL{)r6=S?_YxdUG(P5$cuwp1#4zs?Z$mV#wyMy8!XP&gWa}0SA%AO8#n_uK>x{HilJXT5EFYceMc|?(C7sh(r3~J@bqg?{;&5GB zu^Gk1a;H*)%%dKmt>9zQ>+oCHz}P4^bEK-aZ@Y<7{+MTOEtbNo=wur;U5{J-3wiyt zEqS>HTSq^a4`E~Lgcz`rwFNI`iOkH)hW?;UlwdMva^IV+u3MC(;nemQuR6?Sy-mfh zaX4aRC95j0$rm}94f%kv-i^e?(_T1-h&;54mO&GiSug9q_AJctsIvP1)7+T`HFZS+ z7^{}L4O;D36s!vfK-%H#fT6QBZUYkyAn)kC00?u7@>qEs00WC8KMwF z45)}o0*S~@LZUztHVF_($le=7>RAJJ*D&jmg-ypFOPHwpO zroBN=eDC)}v3*uXbwdn=rnivxl$wnMjn+O?(v>$NGRbTOhgO&xIf~cDI}|a3CG#&k z`NLqLHnf2x$k6j(dtEBLIR&lG2lS!w-*4nPRh=$3>k?x-;nCPIbgJzEZ2T5oN*3sR z`0*_Z=a2R0xtnCBERi%*YZZ*LW3_hSIn;rg5jXsuJ@=L2^pj8Ts;AQkfJ7oUUN>GA z`8MbA1%8<_-2}3+4$YPb#|RpC*SO{_pi%2yL<&8)x#JK3yig_L#~|PeYig`mgv1Tf zi0R$T176Zj-AGS6?MfgzrKEOYDy9yuiW ztNJ6}(4nq;!ALS24!>5T)$ANB2+h`1#ETk z2;@B>>#*B{hbeuE+qB_LoyCQPDoiG0;%JH9Rj8B|oA&cwLu8ko+8T*5k!TL1pn`j< zB~4AFD#go%*3n{YEcv*z@@JI}9n)KqtAev%jrFEnH}-cbaHJCg^V3{b7xl{sh5c|e zPr4>b3xQQqEYWfz8GsI$6BpWr*$%34HL9g=D{S(Zx2t0Mx{1TxVuDB%OB0Lb#5+M@ z{G^_%;1iLgd^pJQ^Sgu+O(N1r&Keac!uAf_l^s}i=@CF1fBfg=_DDaTR zB|_z6#o1*~ta1R1_rdU?6AyPDK~EZbV*Arv><~l*jCSfsrcIJFLP&&6mcSq=&bnq# z@c(=hIM;weXnT5UxQb+AXSO9b;a4jwrj&u!27w*q^A@dqfeoG-PuQH&jQE6yIU>3lI&Mg(oSW7 zrRgs4bxJr34*$j?Sothbb567fRxHFLwTYOe#&0C*wvLX|ve9hV#Iw?eJIJi2Y{@II zOZ$U}UW|H(pgGTEGL@zRf07ndb8I!O@ewTZpAz6?PUPO8zVk`z$m%KV?|y!Al&fOk zmJ+Hffom||PD?pP>;M@+iI1KW@e*b7pBp#m zNlCp}eE-eKnTsz{`qbLmM2&Sv%Nh_LA2#53$UF^DozJ+RtG3S7j?5N3nH{)qH=qG# z. + * + * + * hello_world.c - App layer application of a simple hello world debug print every + * 2 seconds. + */ + + +#include +#include +#include + +#include +#include + +extern "C" +{ + #include "app.h" +} + +#include "FreeRTOS.h" +#include "task.h" + +#include "debug.h" + +#define DEBUG_MODULE "HELLOWORLD" + +class MyClass { + public: + int myNum; + std::string myString; +}; + +void appMain() +{ + DEBUG_PRINT("Waiting for activation ...\n"); + + MyClass *cl = new MyClass(); + DEBUG_PRINT("MyClass has a num: %d\n", cl->myNum); + + /* make sure that the assertion is not simple enough to be optimized away + * by the compiler */ + assert(cl->myNum + cl->myString.size() == 0); + + while(1) { + vTaskDelay(M2T(2000)); + DEBUG_PRINT("Hello World!\n"); + } +} diff --git a/examples/app_hello_world/Kbuild b/examples/app_hello_world/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/examples/app_hello_world/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/examples/app_hello_world/Makefile b/examples/app_hello_world/Makefile index b885fb3786..97df5d7386 100644 --- a/examples/app_hello_world/Makefile +++ b/examples/app_hello_world/Makefile @@ -1,10 +1,28 @@ +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. -# enable app support -APP=1 -APP_STACKSIZE=300 +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../.. -VPATH += src/ -PROJ_OBJ += hello_world.o +# +# To include header files from another dir +# +EXTRA_CFLAGS += -I$(PWD)/other -CRAZYFLIE_BASE=../.. -include $(CRAZYFLIE_BASE)/Makefile \ No newline at end of file +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config + +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/app_hello_world/app-config b/examples/app_hello_world/app-config new file mode 100644 index 0000000000..c86fe9169e --- /dev/null +++ b/examples/app_hello_world/app-config @@ -0,0 +1,3 @@ +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/examples/app_hello_world/other/headerInOtherDir.h b/examples/app_hello_world/other/headerInOtherDir.h new file mode 100644 index 0000000000..afa86b71bf --- /dev/null +++ b/examples/app_hello_world/other/headerInOtherDir.h @@ -0,0 +1,3 @@ +#pragma once + +#define EXAMPLE_OF_HOW_TO_INCLUDE_HEADER_FILE_FROM_OTHER_DIR 4711 diff --git a/examples/app_hello_world/src/Kbuild b/examples/app_hello_world/src/Kbuild new file mode 100644 index 0000000000..6d9a651239 --- /dev/null +++ b/examples/app_hello_world/src/Kbuild @@ -0,0 +1 @@ +obj-y += hello_world.o diff --git a/examples/app_hello_world/src/hello_world.c b/examples/app_hello_world/src/hello_world.c index c2deca0a3d..fa3d08d1fe 100644 --- a/examples/app_hello_world/src/hello_world.c +++ b/examples/app_hello_world/src/hello_world.c @@ -23,7 +23,7 @@ * * * hello_world.c - App layer application of a simple hello world debug print every - * 2 seconds. + * 2 seconds. */ @@ -36,6 +36,8 @@ #include "FreeRTOS.h" #include "task.h" +#include "headerInOtherDir.h" + #include "debug.h" #define DEBUG_MODULE "HELLOWORLD" @@ -48,4 +50,4 @@ void appMain() vTaskDelay(M2T(2000)); DEBUG_PRINT("Hello World!\n"); } -} \ No newline at end of file +} diff --git a/examples/app_internal_param_log/Kbuild b/examples/app_internal_param_log/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/examples/app_internal_param_log/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/examples/app_internal_param_log/Makefile b/examples/app_internal_param_log/Makefile index 3c6e0372a4..d0a76e2ac3 100644 --- a/examples/app_internal_param_log/Makefile +++ b/examples/app_internal_param_log/Makefile @@ -1,10 +1,23 @@ +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. -# enable app support -APP=1 -APP_STACKSIZE=300 +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../.. -VPATH += src/ -PROJ_OBJ += internal_log_param_api.o +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config -CRAZYFLIE_BASE=../.. -include $(CRAZYFLIE_BASE)/Makefile \ No newline at end of file +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/app_internal_param_log/app-config b/examples/app_internal_param_log/app-config new file mode 100644 index 0000000000..c86fe9169e --- /dev/null +++ b/examples/app_internal_param_log/app-config @@ -0,0 +1,3 @@ +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/examples/app_internal_param_log/src/Kbuild b/examples/app_internal_param_log/src/Kbuild new file mode 100644 index 0000000000..46b86773de --- /dev/null +++ b/examples/app_internal_param_log/src/Kbuild @@ -0,0 +1 @@ +obj-y += app_internal_param_log.o diff --git a/examples/app_peer_to_peer/Kbuild b/examples/app_peer_to_peer/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/examples/app_peer_to_peer/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/examples/app_peer_to_peer/Makefile b/examples/app_peer_to_peer/Makefile index 9db884ec24..d0a76e2ac3 100644 --- a/examples/app_peer_to_peer/Makefile +++ b/examples/app_peer_to_peer/Makefile @@ -1,10 +1,23 @@ +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. -# enable app support -APP=1 -APP_STACKSIZE=300 +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../.. -VPATH += src/ -PROJ_OBJ += peer_to_peer.o +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config -CRAZYFLIE_BASE=../.. -include $(CRAZYFLIE_BASE)/Makefile \ No newline at end of file +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/app_peer_to_peer/app-config b/examples/app_peer_to_peer/app-config new file mode 100644 index 0000000000..c86fe9169e --- /dev/null +++ b/examples/app_peer_to_peer/app-config @@ -0,0 +1,3 @@ +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/examples/app_peer_to_peer/src/Kbuild b/examples/app_peer_to_peer/src/Kbuild new file mode 100644 index 0000000000..7d3fcc3699 --- /dev/null +++ b/examples/app_peer_to_peer/src/Kbuild @@ -0,0 +1 @@ +obj-y += peer_to_peer.o diff --git a/examples/demos/app_push_demo/Kbuild b/examples/demos/app_push_demo/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/examples/demos/app_push_demo/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/examples/demos/app_push_demo/Makefile b/examples/demos/app_push_demo/Makefile index 91edf5c329..f6dfea12fd 100644 --- a/examples/demos/app_push_demo/Makefile +++ b/examples/demos/app_push_demo/Makefile @@ -1,9 +1,23 @@ -# enable app support -APP=1 -APP_STACKSIZE=300 +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. -VPATH += src/ -PROJ_OBJ += push.o +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../../.. -CRAZYFLIE_BASE=../../.. -include $(CRAZYFLIE_BASE)/Makefile +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config + +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/demos/app_push_demo/app-config b/examples/demos/app_push_demo/app-config new file mode 100644 index 0000000000..c86fe9169e --- /dev/null +++ b/examples/demos/app_push_demo/app-config @@ -0,0 +1,3 @@ +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/examples/demos/app_push_demo/src/Kbuild b/examples/demos/app_push_demo/src/Kbuild new file mode 100644 index 0000000000..ecb24bd3b7 --- /dev/null +++ b/examples/demos/app_push_demo/src/Kbuild @@ -0,0 +1 @@ +obj-y += push.o diff --git a/examples/demos/app_wall_following_demo/Kbuild b/examples/demos/app_wall_following_demo/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/examples/demos/app_wall_following_demo/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/examples/demos/app_wall_following_demo/Makefile b/examples/demos/app_wall_following_demo/Makefile index 0758d56cb9..f6dfea12fd 100644 --- a/examples/demos/app_wall_following_demo/Makefile +++ b/examples/demos/app_wall_following_demo/Makefile @@ -1,10 +1,23 @@ -# enable app support -APP=1 -APP_STACKSIZE=300 +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. -VPATH += src/ -PROJ_OBJ += wall_following.o -PROJ_OBJ += wallfollowing_multiranger_onboard.o +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../../.. -CRAZYFLIE_BASE=../../.. -include $(CRAZYFLIE_BASE)/Makefile +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config + +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/demos/app_wall_following_demo/app-config b/examples/demos/app_wall_following_demo/app-config new file mode 100644 index 0000000000..c86fe9169e --- /dev/null +++ b/examples/demos/app_wall_following_demo/app-config @@ -0,0 +1,3 @@ +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/examples/demos/app_wall_following_demo/src/Kbuild b/examples/demos/app_wall_following_demo/src/Kbuild new file mode 100644 index 0000000000..701e87ecb9 --- /dev/null +++ b/examples/demos/app_wall_following_demo/src/Kbuild @@ -0,0 +1,2 @@ +obj-y += wall_following.o +obj-y += wall_following_multiranger_onboard.o diff --git a/examples/demos/swarm_demo/Kbuild b/examples/demos/swarm_demo/Kbuild new file mode 100644 index 0000000000..56eae3f7e0 --- /dev/null +++ b/examples/demos/swarm_demo/Kbuild @@ -0,0 +1 @@ +obj-y += app.o diff --git a/examples/demos/swarm_demo/Makefile b/examples/demos/swarm_demo/Makefile index 3477099997..f6dfea12fd 100644 --- a/examples/demos/swarm_demo/Makefile +++ b/examples/demos/swarm_demo/Makefile @@ -1,20 +1,23 @@ +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. -# enable app support -APP=1 -APP_STACKSIZE=300 +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../../.. -## Weight of the Crazyflie + Qi + Lighthouse deck -CFLAGS += -DCF_MASS=0.036f +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config -## Force lighthouse 2 -CFLAGS += -DLIGHTHOUSE_FORCE_TYPE=2 - -# CLOAD_CMDS = -w radio://0/30/2M -# DEBUG=1 - - -VPATH += src/ -PROJ_OBJ += app.o - -CRAZYFLIE_BASE=../../.. -include $(CRAZYFLIE_BASE)/Makefile +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/demos/swarm_demo/app-config b/examples/demos/swarm_demo/app-config new file mode 100644 index 0000000000..c86fe9169e --- /dev/null +++ b/examples/demos/swarm_demo/app-config @@ -0,0 +1,3 @@ +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/include/config/cc/optimize/for/performance.h b/include/config/cc/optimize/for/performance.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/config/cross/compile.h b/include/config/cross/compile.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/config/enable/foo/bar.h b/include/config/enable/foo/bar.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/config/estimator/name.h b/include/config/estimator/name.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/config/log/enable.h b/include/config/log/enable.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/config/log/error.h b/include/config/log/error.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/config/log/timestamp.h b/include/config/log/timestamp.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/module.json b/module.json index a49897f452..13b3894193 100644 --- a/module.json +++ b/module.json @@ -22,6 +22,9 @@ "cf2": [ "PLATFORM=cf2" ], + "bolt": [ + "PLATFORM=bolt" + ], "tag": [ "PLATFORM=tag" ] diff --git a/scripts/Kbuild.include b/scripts/Kbuild.include new file mode 100644 index 0000000000..0f82314621 --- /dev/null +++ b/scripts/Kbuild.include @@ -0,0 +1,391 @@ +#### +# kbuild: Generic definitions + +# Convenient variables +comma := , +quote := " +squote := ' +empty := +space := $(empty) $(empty) +space_escape := _-_SPACE_-_ + +### +# Name of target with a '.' as filename prefix. foo/bar.o => foo/.bar.o +dot-target = $(dir $@).$(notdir $@) + +### +# The temporary file to save gcc -MD generated dependencies must not +# contain a comma +depfile = $(subst $(comma),_,$(dot-target).d) + +### +# filename of target with directory and extension stripped +basetarget = $(basename $(notdir $@)) + +### +# filename of first prerequisite with directory and extension stripped +baseprereq = $(basename $(notdir $<)) + +### +# Escape single quote for use in echo statements +escsq = $(subst $(squote),'\$(squote)',$1) + +### +# Easy method for doing a status message + kecho := : + quiet_kecho := echo +silent_kecho := : +kecho := $($(quiet)kecho) + +### +# filechk is used to check if the content of a generated file is updated. +# Sample usage: +# define filechk_sample +# echo $KERNELRELEASE +# endef +# version.h : Makefile +# $(call filechk,sample) +# The rule defined shall write to stdout the content of the new file. +# The existing file will be compared with the new one. +# - If no file exist it is created +# - If the content differ the new file is used +# - If they are equal no change, and no timestamp update +# - stdin is piped in from the first prerequisite ($<) so one has +# to specify a valid file as first prerequisite (often the kbuild file) +define filechk + $(Q)set -e; \ + $(kecho) ' CHK $@'; \ + mkdir -p $(dir $@); \ + $(filechk_$(1)) < $< > $@.tmp; \ + if [ -r $@ ] && cmp -s $@ $@.tmp; then \ + rm -f $@.tmp; \ + else \ + $(kecho) ' UPD $@'; \ + mv -f $@.tmp $@; \ + fi +endef + +###### +# gcc support functions +# See documentation in Documentation/kbuild/makefiles.txt + +# cc-cross-prefix +# Usage: CROSS_COMPILE := $(call cc-cross-prefix, m68k-linux-gnu- m68k-linux-) +# Return first prefix where a prefix$(CC) is found in PATH. +# If no $(CC) found in PATH with listed prefixes return nothing +cc-cross-prefix = \ + $(word 1, $(foreach c,$(1), \ + $(shell set -e; \ + if (which $(strip $(c))$(CC)) > /dev/null 2>&1 ; then \ + echo $(c); \ + fi))) + +# output directory for tests below +TMPOUT := $(if $(KBUILD_EXTMOD),$(firstword $(KBUILD_EXTMOD))/) + +# try-run +# Usage: option = $(call try-run, $(CC)...-o "$$TMP",option-ok,otherwise) +# Exit code chooses option. "$$TMP" is can be used as temporary file and +# is automatically cleaned up. +try-run = $(shell set -e; \ + TMP="$(TMPOUT).$$$$.tmp"; \ + TMPO="$(TMPOUT).$$$$.o"; \ + if ($(1)) >/dev/null 2>&1; \ + then echo "$(2)"; \ + else echo "$(3)"; \ + fi; \ + rm -f "$$TMP" "$$TMPO") + +# as-option +# Usage: cflags-y += $(call as-option,-Wa$(comma)-isa=foo,) + +as-option = $(call try-run,\ + $(CC) $(KBUILD_CFLAGS) $(1) -c -x assembler /dev/null -o "$$TMP",$(1),$(2)) + +# as-instr +# Usage: cflags-y += $(call as-instr,instr,option1,option2) + +as-instr = $(call try-run,\ + printf "%b\n" "$(1)" | $(CC) $(KBUILD_AFLAGS) -c -x assembler -o "$$TMP" -,$(2),$(3)) + +# cc-option +# Usage: cflags-y += $(call cc-option,-march=winchip-c6,-march=i586) + +cc-option = $(call try-run,\ + $(CC) $(KBUILD_CPPFLAGS) $(KBUILD_CFLAGS) $(1) -c -x c /dev/null -o "$$TMP",$(1),$(2)) + +# cc-option-yn +# Usage: flag := $(call cc-option-yn,-march=winchip-c6) +cc-option-yn = $(call try-run,\ + $(CC) $(KBUILD_CPPFLAGS) $(KBUILD_CFLAGS) $(1) -c -x c /dev/null -o "$$TMP",y,n) + +# cc-option-align +# Prefix align with either -falign or -malign +cc-option-align = $(subst -functions=0,,\ + $(call cc-option,-falign-functions=0,-malign-functions=0)) + +# cc-disable-warning +# Usage: cflags-y += $(call cc-disable-warning,unused-but-set-variable) +cc-disable-warning = $(call try-run,\ + $(CC) $(KBUILD_CPPFLAGS) $(KBUILD_CFLAGS) -W$(strip $(1)) -c -x c /dev/null -o "$$TMP",-Wno-$(strip $(1))) + +# cc-name +# Expands to either gcc or clang +cc-name = $(shell $(CC) -v 2>&1 | grep -q "clang version" && echo clang || echo gcc) + +# cc-version +cc-version = $(shell $(CONFIG_SHELL) $(srctree)/scripts/gcc-version.sh $(CC)) + +# cc-fullversion +cc-fullversion = $(shell $(CONFIG_SHELL) \ + $(srctree)/scripts/gcc-version.sh -p $(CC)) + +# cc-ifversion +# Usage: EXTRA_CFLAGS += $(call cc-ifversion, -lt, 0402, -O1) +cc-ifversion = $(shell [ $(cc-version) $(1) $(2) ] && echo $(3) || echo $(4)) + +# cc-ldoption +# Usage: ldflags += $(call cc-ldoption, -Wl$(comma)--hash-style=both) +cc-ldoption = $(call try-run,\ + $(CC) $(1) -nostdlib -x c /dev/null -o "$$TMP",$(1),$(2)) + +# ld-option +# Usage: LDFLAGS += $(call ld-option, -X) +ld-option = $(call try-run,\ + $(CC) -x c /dev/null -c -o "$$TMPO" ; $(LD) $(1) "$$TMPO" -o "$$TMP",$(1),$(2)) + +# ar-option +# Usage: KBUILD_ARFLAGS := $(call ar-option,D) +# Important: no spaces around options +ar-option = $(call try-run, $(AR) rc$(1) "$$TMP",$(1),$(2)) + +# ld-version +# Note this is mainly for HJ Lu's 3 number binutil versions +ld-version = $(shell $(LD) --version | $(srctree)/scripts/ld-version.sh) + +# ld-ifversion +# Usage: $(call ld-ifversion, -ge, 22252, y) +ld-ifversion = $(shell [ $(ld-version) $(1) $(2) ] && echo $(3) || echo $(4)) + +###### + +### +# Shorthand for $(Q)$(MAKE) -f scripts/Makefile.build obj= +# Usage: +# $(Q)$(MAKE) $(build)=dir +build := -f $(srctree)/scripts/Makefile.build obj + +### +# Shorthand for $(Q)$(MAKE) -f scripts/Makefile.modbuiltin obj= +# Usage: +# $(Q)$(MAKE) $(modbuiltin)=dir +modbuiltin := -f $(srctree)/scripts/Makefile.modbuiltin obj + +### +# Shorthand for $(Q)$(MAKE) -f scripts/Makefile.dtbinst obj= +# Usage: +# $(Q)$(MAKE) $(dtbinst)=dir +dtbinst := -f $(if $(KBUILD_SRC),$(srctree)/)scripts/Makefile.dtbinst obj + +### +# Shorthand for $(Q)$(MAKE) -f scripts/Makefile.clean obj= +# Usage: +# $(Q)$(MAKE) $(clean)=dir +clean := -f $(srctree)/scripts/Makefile.clean obj + +### +# Shorthand for $(Q)$(MAKE) -f scripts/Makefile.headersinst obj= +# Usage: +# $(Q)$(MAKE) $(hdr-inst)=dir +hdr-inst := -f $(srctree)/scripts/Makefile.headersinst obj + +# Prefix -I with $(srctree) if it is not an absolute path. +# skip if -I has no parameter +addtree = $(if $(patsubst -I%,%,$(1)), \ +$(if $(filter-out -I/%,$(1)),$(patsubst -I%,-I$(srctree)/%,$(1))) $(1)) + +# Find all -I options and call addtree +flags = $(foreach o,$($(1)),$(if $(filter -I%,$(o)),$(call addtree,$(o)),$(o))) + +# echo command. +# Short version is used, if $(quiet) equals `quiet_', otherwise full one. +echo-cmd = $(if $($(quiet)cmd_$(1)),\ + echo ' $(call escsq,$($(quiet)cmd_$(1)))$(echo-why)';) + +# printing commands +cmd = @$(echo-cmd) $(cmd_$(1)) + +# Add $(obj)/ for paths that are not absolute +objectify = $(foreach o,$(1),$(if $(filter /%,$(o)),$(o),$(obj)/$(o))) + +### +# if_changed - execute command if any prerequisite is newer than +# target, or command line has changed +# if_changed_dep - as if_changed, but uses fixdep to reveal dependencies +# including used config symbols +# if_changed_rule - as if_changed but execute rule instead +# See Documentation/kbuild/makefiles.txt for more info + +ifneq ($(KBUILD_NOCMDDEP),1) +# Check if both arguments are the same including their order. Result is empty +# string if equal. User may override this check using make KBUILD_NOCMDDEP=1 +arg-check = $(filter-out $(subst $(space),$(space_escape),$(strip $(cmd_$@))), \ + $(subst $(space),$(space_escape),$(strip $(cmd_$1)))) +else +arg-check = $(if $(strip $(cmd_$@)),,1) +endif + +# Replace >$< with >$$< to preserve $ when reloading the .cmd file +# (needed for make) +# Replace >#< with >\#< to avoid starting a comment in the .cmd file +# (needed for make) +# Replace >'< with >'\''< to be able to enclose the whole string in '...' +# (needed for the shell) +make-cmd = $(call escsq,$(subst \#,\\\#,$(subst $$,$$$$,$(cmd_$(1))))) + +# Find any prerequisites that is newer than target or that does not exist. +# PHONY targets skipped in both cases. +any-prereq = $(filter-out $(PHONY),$?) $(filter-out $(PHONY) $(wildcard $^),$^) + +# Execute command if command has changed or prerequisite(s) are updated. +# +if_changed = $(if $(strip $(any-prereq) $(arg-check)), \ + @set -e; \ + $(echo-cmd) $(cmd_$(1)); \ + printf '%s\n' 'cmd_$@ := $(make-cmd)' > $(dot-target).cmd, @:) + +# Execute the command and also postprocess generated .d dependencies file. +if_changed_dep = $(if $(strip $(any-prereq) $(arg-check) ), \ + @set -e; \ + $(cmd_and_fixdep), @:) + +ifndef CONFIG_TRIM_UNUSED_KSYMS + +cmd_and_fixdep = \ + $(echo-cmd) $(cmd_$(1)); \ + scripts/basic/fixdep $(depfile) $@ '$(make-cmd)' > $(dot-target).tmp;\ + rm -f $(depfile); \ + mv -f $(dot-target).tmp $(dot-target).cmd; + +else + +# Filter out exported kernel symbol names from the preprocessor output. +# See also __KSYM_DEPS__ in include/linux/export.h. +# We disable the depfile generation here, so as not to overwrite the existing +# depfile while fixdep is parsing it. +flags_nodeps = $(filter-out -Wp$(comma)-M%, $($(1))) +ksym_dep_filter = \ + case "$(1)" in \ + cc_*_c|cpp_i_c) \ + $(CPP) $(call flags_nodeps,c_flags) -D__KSYM_DEPS__ $< ;; \ + as_*_S|cpp_s_S) \ + $(CPP) $(call flags_nodeps,a_flags) -D__KSYM_DEPS__ $< ;; \ + boot*|build*|*cpp_lds_S|dtc|host*|vdso*) : ;; \ + *) echo "Don't know how to preprocess $(1)" >&2; false ;; \ + esac | tr ";" "\n" | sed -rn 's/^.*=== __KSYM_(.*) ===.*$$/KSYM_\1/p' + +cmd_and_fixdep = \ + $(echo-cmd) $(cmd_$(1)); \ + $(ksym_dep_filter) | \ + scripts/basic/fixdep -e $(depfile) $@ '$(make-cmd)' \ + > $(dot-target).tmp; \ + rm -f $(depfile); \ + mv -f $(dot-target).tmp $(dot-target).cmd; + +endif + +# Usage: $(call if_changed_rule,foo) +# Will check if $(cmd_foo) or any of the prerequisites changed, +# and if so will execute $(rule_foo). +if_changed_rule = $(if $(strip $(any-prereq) $(arg-check) ), \ + @set -e; \ + $(rule_$(1)), @:) + +### +# why - tell why a a target got build +# enabled by make V=2 +# Output (listed in the order they are checked): +# (1) - due to target is PHONY +# (2) - due to target missing +# (3) - due to: file1.h file2.h +# (4) - due to command line change +# (5) - due to missing .cmd file +# (6) - due to target not in $(targets) +# (1) PHONY targets are always build +# (2) No target, so we better build it +# (3) Prerequisite is newer than target +# (4) The command line stored in the file named dir/.target.cmd +# differed from actual command line. This happens when compiler +# options changes +# (5) No dir/.target.cmd file (used to store command line) +# (6) No dir/.target.cmd file and target not listed in $(targets) +# This is a good hint that there is a bug in the kbuild file +ifeq ($(KBUILD_VERBOSE),2) +why = \ + $(if $(filter $@, $(PHONY)),- due to target is PHONY, \ + $(if $(wildcard $@), \ + $(if $(strip $(any-prereq)),- due to: $(any-prereq), \ + $(if $(arg-check), \ + $(if $(cmd_$@),- due to command line change, \ + $(if $(filter $@, $(targets)), \ + - due to missing .cmd file, \ + - due to $(notdir $@) not in $$(targets) \ + ) \ + ) \ + ) \ + ), \ + - due to target missing \ + ) \ + ) + +echo-why = $(call escsq, $(strip $(why))) +endif + +############################################################################### +# +# When a Kconfig string contains a filename, it is suitable for +# passing to shell commands. It is surrounded by double-quotes, and +# any double-quotes or backslashes within it are escaped by +# backslashes. +# +# This is no use for dependencies or $(wildcard). We need to strip the +# surrounding quotes and the escaping from quotes and backslashes, and +# we *do* need to escape any spaces in the string. So, for example: +# +# Usage: $(eval $(call config_filename,FOO)) +# +# Defines FOO_FILENAME based on the contents of the CONFIG_FOO option, +# transformed as described above to be suitable for use within the +# makefile. +# +# Also, if the filename is a relative filename and exists in the source +# tree but not the build tree, define FOO_SRCPREFIX as $(srctree)/ to +# be prefixed to *both* command invocation and dependencies. +# +# Note: We also print the filenames in the quiet_cmd_foo text, and +# perhaps ought to have a version specially escaped for that purpose. +# But it's only cosmetic, and $(patsubst "%",%,$(CONFIG_FOO)) is good +# enough. It'll strip the quotes in the common case where there's no +# space and it's a simple filename, and it'll retain the quotes when +# there's a space. There are some esoteric cases in which it'll print +# the wrong thing, but we don't really care. The actual dependencies +# and commands *do* get it right, with various combinations of single +# and double quotes, backslashes and spaces in the filenames. +# +############################################################################### +# +define config_filename +ifneq ($$(CONFIG_$(1)),"") +$(1)_FILENAME := $$(subst \\,\,$$(subst \$$(quote),$$(quote),$$(subst $$(space_escape),\$$(space),$$(patsubst "%",%,$$(subst $$(space),$$(space_escape),$$(CONFIG_$(1))))))) +ifneq ($$(patsubst /%,%,$$(firstword $$($(1)_FILENAME))),$$(firstword $$($(1)_FILENAME))) +else +ifeq ($$(wildcard $$($(1)_FILENAME)),) +ifneq ($$(wildcard $$(srctree)/$$($(1)_FILENAME)),) +$(1)_SRCPREFIX := $(srctree)/ +endif +endif +endif +endif +endef +# +############################################################################### diff --git a/scripts/Makefile b/scripts/Makefile new file mode 100644 index 0000000000..161620febe --- /dev/null +++ b/scripts/Makefile @@ -0,0 +1,2 @@ +# Let clean descend into subdirs +subdir- += basic kconfig diff --git a/scripts/Makefile.build b/scripts/Makefile.build new file mode 100644 index 0000000000..87fb224227 --- /dev/null +++ b/scripts/Makefile.build @@ -0,0 +1,335 @@ +# ========================================================================== +# Building +# ========================================================================== + +src := $(obj) + +PHONY := __build +__build: + +# Init all relevant variables used in kbuild files so +# 1) they have correct type +# 2) they do not inherit any value from the environment +obj-y := +lib-y := +always := +targets := +subdir-y := +EXTRA_AFLAGS := +EXTRA_CFLAGS := +EXTRA_CPPFLAGS := +EXTRA_LDFLAGS := +asflags-y := +ccflags-y := +cppflags-y := +ldflags-y := + +subdir-asflags-y := +subdir-ccflags-y := + +# Read auto.conf if it exists, otherwise ignore +-include include/config/auto.conf + +include scripts/Kbuild.include + +# For backward compatibility check that these variables do not change +save-cflags := $(CFLAGS) + +# The filename Kbuild has precedence over Makefile +kbuild-dir := $(if $(filter /%,$(src)),$(src),$(srctree)/$(src)) +kbuild-file := $(if $(wildcard $(kbuild-dir)/Kbuild),$(kbuild-dir)/Kbuild,$(kbuild-dir)/Makefile) +include $(kbuild-file) + +# If the save-* variables changed error out +ifeq ($(KBUILD_NOPEDANTIC),) + ifneq ("$(save-cflags)","$(CFLAGS)") + $(error CFLAGS was changed in "$(kbuild-file)". Fix it to use ccflags-y) + endif +endif + +include scripts/Makefile.lib + +ifdef host-progs +ifneq ($(hostprogs-y),$(host-progs)) +$(warning kbuild: $(obj)/Makefile - Usage of host-progs is deprecated. Please replace with hostprogs-y!) +hostprogs-y += $(host-progs) +endif +endif + +# Do not include host rules unless needed +ifneq ($(hostprogs-y),) +include scripts/Makefile.host +endif + +ifneq ($(KBUILD_SRC),) +# Create output directory if not already present +_dummy := $(shell [ -d $(obj) ] || mkdir -p $(obj)) + +# Create directories for object files if directory does not exist +# Needed when obj-y := dir/file.o syntax is used +_dummy := $(foreach d,$(obj-dirs), $(shell [ -d $(d) ] || mkdir -p $(d))) +endif + +ifndef obj +$(warning kbuild: Makefile.build is included improperly) +endif + +# =========================================================================== + +ifneq ($(strip $(lib-y) $(lib-)),) +lib-target := $(obj)/lib.a +endif + +ifneq ($(strip $(obj-y) $(obj-) $(lib-target)),) +builtin-target := $(obj)/built-in.o +endif + +__build: $(if $(KBUILD_BUILTIN),$(builtin-target) $(lib-target) $(extra-y)) \ + $(subdir-ym) $(always) + @: + +# Linus' kernel sanity checking tool +ifneq ($(KBUILD_CHECKSRC),0) + ifeq ($(KBUILD_CHECKSRC),2) + quiet_cmd_force_checksrc = CHECK $< + cmd_force_checksrc = $(CHECK) $(CHECKFLAGS) $(c_flags) $< ; + else + quiet_cmd_checksrc = CHECK $< + cmd_checksrc = $(CHECK) $(CHECKFLAGS) $(c_flags) $< ; + endif +endif + +# Compile C sources (.c) +# --------------------------------------------------------------------------- + +# Default is built-in, unless we know otherwise + +modkern_cflags = $(KBUILD_CFLAGS_KERNEL) $(CFLAGS_KERNEL) +quiet_modtag := $(empty) $(empty) + +# Default for not multi-part modules +modname = $(basetarget) + +$(multi-objs-y) : modname = $(modname-multi) +$(multi-objs-y:.o=.i) : modname = $(modname-multi) +$(multi-objs-y:.o=.s) : modname = $(modname-multi) +$(multi-objs-y:.o=.lst) : modname = $(modname-multi) + +quiet_cmd_cc_s_c = CC $(quiet_modtag) $@ +cmd_cc_s_c = $(CC) $(c_flags) $(DISABLE_LTO) -fverbose-asm -S -o $@ $< + +$(obj)/%.s: $(src)/%.c FORCE + $(call if_changed_dep,cc_s_c) + +quiet_cmd_cpp_i_c = CPP $(quiet_modtag) $@ +cmd_cpp_i_c = $(CPP) $(c_flags) -o $@ $< + +$(obj)/%.i: $(src)/%.c FORCE + $(call if_changed_dep,cpp_i_c) + +# C (.c) files +# The C file is compiled and updated dependency information is generated. +# (See cmd_cc_o_c + relevant part of rule_cc_o_c) + +quiet_cmd_cc_o_c = CC $(quiet_modtag) $@ + +cmd_cc_o_c = $(CC) $(c_flags) -c -o $@ $< + +cmd_cxx_o_cpp = $(CXX) $(cxx_flags) -c -o $@ $< +quiet_cmd_cxx_o_cpp = CXX $(quiet_modtag) $@ + +ifdef CONFIG_STACK_VALIDATION +ifneq ($(SKIP_STACK_VALIDATION),1) + +__objtool_obj := $(objtree)/tools/objtool/objtool + +objtool_args = check +ifndef CONFIG_FRAME_POINTER +objtool_args += --no-fp +endif + +# 'OBJECT_FILES_NON_STANDARD := y': skip objtool checking for a directory +# 'OBJECT_FILES_NON_STANDARD_foo.o := 'y': skip objtool checking for a file +# 'OBJECT_FILES_NON_STANDARD_foo.o := 'n': override directory skip for a file +cmd_objtool = $(if $(patsubst y%,, \ + $(OBJECT_FILES_NON_STANDARD_$(basetarget).o)$(OBJECT_FILES_NON_STANDARD)n), \ + $(__objtool_obj) $(objtool_args) "$(@)";) +objtool_obj = $(if $(patsubst y%,, \ + $(OBJECT_FILES_NON_STANDARD_$(basetarget).o)$(OBJECT_FILES_NON_STANDARD)n), \ + $(__objtool_obj)) + +endif # SKIP_STACK_VALIDATION +endif # CONFIG_STACK_VALIDATION + +define rule_cxx_o_cpp + $(call echo-cmd,checksrc) $(cmd_checksrc) \ + $(call cmd_and_fixdep,cxx_o_cpp) \ + $(cmd_objtool) +endef + +define rule_cc_o_c + $(call echo-cmd,checksrc) $(cmd_checksrc) \ + $(call cmd_and_fixdep,cc_o_c) \ + $(cmd_objtool) +endef + +define rule_as_o_S + $(call cmd_and_fixdep,as_o_S) \ + $(cmd_objtool) +endef + +# List module undefined symbols (or empty line if not enabled) +ifdef CONFIG_TRIM_UNUSED_KSYMS +cmd_undef_syms = $(NM) $@ | sed -n 's/^ \+U //p' | xargs echo +else +cmd_undef_syms = echo +endif + +# Built-in and composite module parts +$(obj)/%.o: $(src)/%.c $(objtool_obj) FORCE + $(call cmd,force_checksrc) + $(call if_changed_rule,cc_o_c) + +$(obj)/%.o: $(src)/%.cc $(objtool_obj) FORCE + $(call cmd,force_checksrc) + $(call if_changed_rule,cxx_o_cpp) + +$(obj)/%.o: $(src)/%.cpp $(objtool_obj) FORCE + $(call cmd,force_checksrc) + $(call if_changed_rule,cxx_o_cpp) + +quiet_cmd_cc_lst_c = MKLST $@ + cmd_cc_lst_c = $(CC) $(c_flags) -g -c -o $*.o $< && \ + $(CONFIG_SHELL) $(srctree)/scripts/makelst $*.o \ + System.map $(OBJDUMP) > $@ + +$(obj)/%.lst: $(src)/%.c FORCE + $(call if_changed_dep,cc_lst_c) + +# Compile assembler sources (.S) +# --------------------------------------------------------------------------- + +modkern_aflags := $(KBUILD_AFLAGS_KERNEL) $(AFLAGS_KERNEL) + +quiet_cmd_cpp_s_S = CPP $(quiet_modtag) $@ +cmd_cpp_s_S = $(CPP) $(a_flags) -o $@ $< + +$(obj)/%.s: $(src)/%.S FORCE + $(call if_changed_dep,cpp_s_S) + +quiet_cmd_as_o_S = AS $(quiet_modtag) $@ +cmd_as_o_S = $(CC) $(a_flags) -c -o $@ $< + +$(obj)/%.o: $(src)/%.S $(objtool_obj) FORCE + $(call if_changed_rule,as_o_S) + +targets += $(real-objs-y) $(lib-y) +targets += $(extra-y) $(MAKECMDGOALS) $(always) + +# Linker scripts preprocessor (.lds.S -> .lds) +# --------------------------------------------------------------------------- +quiet_cmd_cpp_lds_S = LDS $@ + cmd_cpp_lds_S = $(CPP) $(cpp_flags) -P -C -U$(ARCH) \ + -D__ASSEMBLY__ -DLINKER_SCRIPT -o $@ $< + +$(obj)/%.lds: $(src)/%.lds.S FORCE + $(call if_changed_dep,cpp_lds_S) + +# ASN.1 grammar +# --------------------------------------------------------------------------- +quiet_cmd_asn1_compiler = ASN.1 $@ + cmd_asn1_compiler = $(objtree)/scripts/asn1_compiler $< \ + $(subst .h,.c,$@) $(subst .c,.h,$@) + +.PRECIOUS: $(objtree)/$(obj)/%-asn1.c $(objtree)/$(obj)/%-asn1.h + +$(obj)/%-asn1.c $(obj)/%-asn1.h: $(src)/%.asn1 $(objtree)/scripts/asn1_compiler + $(call cmd,asn1_compiler) + +# Build the compiled-in targets +# --------------------------------------------------------------------------- + +# To build objects in subdirs, we need to descend into the directories +$(sort $(subdir-obj-y)): $(subdir-ym) ; + +# +# Rule to compile a set of .o files into one .o file +# +ifdef builtin-target +quiet_cmd_link_o_target = LD $@ +# If the list of objects to link is empty, just create an empty built-in.o +cmd_link_o_target = $(if $(strip $(obj-y)),\ + $(LD) $(ld_flags) -r -o $@ $(filter $(obj-y), $^), \ + rm -f $@; $(AR) rcs$(KBUILD_ARFLAGS) $@) + +$(builtin-target): $(obj-y) FORCE + $(call if_changed,link_o_target) + +targets += $(builtin-target) +endif # builtin-target + +# +# Rule to compile a set of .o files into one .a file +# +ifdef lib-target +quiet_cmd_link_l_target = AR $@ +cmd_link_l_target = rm -f $@; $(AR) rcs$(KBUILD_ARFLAGS) $@ $(lib-y) + +$(lib-target): $(lib-y) FORCE + $(call if_changed,link_l_target) + +targets += $(lib-target) +endif + +# +# Rule to link composite objects +# +# Composite objects are specified in kbuild makefile as follows: +# -objs := +# or +# -y := + +link_multi_deps = \ +$(filter $(addprefix $(obj)/, \ +$($(subst $(obj)/,,$(@:.o=-objs))) \ +$($(subst $(obj)/,,$(@:.o=-y)))), $^) + +quiet_cmd_link_multi-y = LD $@ +cmd_link_multi-y = $(LD) $(ld_flags) -r -o $@ $(link_multi_deps) $(cmd_secanalysis) + +$(multi-used-y): FORCE + $(call if_changed,link_multi-y) +$(call multi_depend, $(multi-used-y), .o, -objs -y) + +targets += $(multi-used-y) + +# Descending +# --------------------------------------------------------------------------- + +PHONY += $(subdir-ym) +$(subdir-ym): + $(Q)$(MAKE) $(build)=$@ + +# Add FORCE to the prequisites of a target to force it to be always rebuilt. +# --------------------------------------------------------------------------- + +PHONY += FORCE + +FORCE: + +# Read all saved command lines and dependencies for the $(targets) we +# may be building above, using $(if_changed{,_dep}). As an +# optimization, we don't need to read them if the target does not +# exist, we will rebuild anyway in that case. + +targets := $(wildcard $(sort $(targets))) +cmd_files := $(wildcard $(foreach f,$(targets),$(dir $(f)).$(notdir $(f)).cmd)) + +ifneq ($(cmd_files),) + include $(cmd_files) +endif + +# Declare the contents of the .PHONY variable as phony. We keep that +# information in a variable se we can use it in if_changed and friends. + +.PHONY: $(PHONY) diff --git a/scripts/Makefile.clean b/scripts/Makefile.clean new file mode 100644 index 0000000000..55c96cb807 --- /dev/null +++ b/scripts/Makefile.clean @@ -0,0 +1,91 @@ +# ========================================================================== +# Cleaning up +# ========================================================================== + +src := $(obj) + +PHONY := __clean +__clean: + +include scripts/Kbuild.include + +# The filename Kbuild has precedence over Makefile +kbuild-dir := $(if $(filter /%,$(src)),$(src),$(srctree)/$(src)) +include $(if $(wildcard $(kbuild-dir)/Kbuild), $(kbuild-dir)/Kbuild, $(kbuild-dir)/Makefile) + +# Figure out what we need to build from the various variables +# ========================================================================== + +__subdir-y := $(patsubst %/,%,$(filter %/, $(obj-y))) +subdir-y += $(__subdir-y) +__subdir-m := $(patsubst %/,%,$(filter %/, $(obj-m))) +subdir-m += $(__subdir-m) +__subdir- := $(patsubst %/,%,$(filter %/, $(obj-))) +subdir- += $(__subdir-) + +# Subdirectories we need to descend into + +subdir-ym := $(sort $(subdir-y) $(subdir-m)) +subdir-ymn := $(sort $(subdir-ym) $(subdir-)) + +# Add subdir path + +subdir-ymn := $(addprefix $(obj)/,$(subdir-ymn)) + +# build a list of files to remove, usually relative to the current +# directory + +__clean-files := $(extra-y) $(extra-m) $(extra-) \ + $(always) $(targets) $(clean-files) \ + $(host-progs) \ + $(hostprogs-y) $(hostprogs-m) $(hostprogs-) + +__clean-files := $(filter-out $(no-clean-files), $(__clean-files)) + +# clean-files is given relative to the current directory, unless it +# starts with $(objtree)/ (which means "./", so do not add "./" unless +# you want to delete a file from the toplevel object directory). + +__clean-files := $(wildcard \ + $(addprefix $(obj)/, $(filter-out $(objtree)/%, $(__clean-files))) \ + $(filter $(objtree)/%, $(__clean-files))) + +# same as clean-files + +__clean-dirs := $(wildcard \ + $(addprefix $(obj)/, $(filter-out $(objtree)/%, $(clean-dirs))) \ + $(filter $(objtree)/%, $(clean-dirs))) + +# ========================================================================== + +quiet_cmd_clean = CLEAN $(obj) + cmd_clean = rm -f $(__clean-files) +quiet_cmd_cleandir = CLEAN $(__clean-dirs) + cmd_cleandir = rm -rf $(__clean-dirs) + + +__clean: $(subdir-ymn) +ifneq ($(strip $(__clean-files)),) + +$(call cmd,clean) +endif +ifneq ($(strip $(__clean-dirs)),) + +$(call cmd,cleandir) +endif + @: + + +# =========================================================================== +# Generic stuff +# =========================================================================== + +# Descending +# --------------------------------------------------------------------------- + +PHONY += $(subdir-ymn) +$(subdir-ymn): + $(Q)$(MAKE) $(clean)=$@ + +# Declare the contents of the .PHONY variable as phony. We keep that +# information in a variable se we can use it in if_changed and friends. + +.PHONY: $(PHONY) diff --git a/scripts/Makefile.extrawarn b/scripts/Makefile.extrawarn new file mode 100644 index 0000000000..53449a6ff6 --- /dev/null +++ b/scripts/Makefile.extrawarn @@ -0,0 +1,71 @@ +# ========================================================================== +# +# make W=... settings +# +# W=1 - warnings that may be relevant and does not occur too often +# W=2 - warnings that occur quite often but may still be relevant +# W=3 - the more obscure warnings, can most likely be ignored +# +# $(call cc-option, -W...) handles gcc -W.. options which +# are not supported by all versions of the compiler +# ========================================================================== + +ifeq ("$(origin W)", "command line") + export KBUILD_ENABLE_EXTRA_GCC_CHECKS := $(W) +endif + +ifdef KBUILD_ENABLE_EXTRA_GCC_CHECKS +warning- := $(empty) + +warning-1 := -Wextra -Wunused -Wno-unused-parameter +warning-1 += -Wmissing-declarations +warning-1 += -Wmissing-format-attribute +warning-1 += $(call cc-option, -Wmissing-prototypes) +warning-1 += -Wold-style-definition +warning-1 += $(call cc-option, -Wmissing-include-dirs) +warning-1 += $(call cc-option, -Wunused-but-set-variable) +warning-1 += $(call cc-option, -Wunused-const-variable) +warning-1 += $(call cc-disable-warning, missing-field-initializers) +warning-1 += $(call cc-disable-warning, sign-compare) + +warning-2 := -Waggregate-return +warning-2 += -Wcast-align +warning-2 += -Wdisabled-optimization +warning-2 += -Wnested-externs +warning-2 += -Wshadow +warning-2 += $(call cc-option, -Wlogical-op) +warning-2 += $(call cc-option, -Wmissing-field-initializers) +warning-2 += $(call cc-option, -Wsign-compare) + +warning-3 := -Wbad-function-cast +warning-3 += -Wcast-qual +warning-3 += -Wconversion +warning-3 += -Wpacked +warning-3 += -Wpadded +warning-3 += -Wpointer-arith +warning-3 += -Wredundant-decls +warning-3 += -Wswitch-default +warning-3 += $(call cc-option, -Wpacked-bitfield-compat) +warning-3 += $(call cc-option, -Wvla) + +warning := $(warning-$(findstring 1, $(KBUILD_ENABLE_EXTRA_GCC_CHECKS))) +warning += $(warning-$(findstring 2, $(KBUILD_ENABLE_EXTRA_GCC_CHECKS))) +warning += $(warning-$(findstring 3, $(KBUILD_ENABLE_EXTRA_GCC_CHECKS))) + +ifeq ("$(strip $(warning))","") + $(error W=$(KBUILD_ENABLE_EXTRA_GCC_CHECKS) is unknown) +endif + +KBUILD_CFLAGS += $(warning) +else + +ifeq ($(cc-name),clang) +KBUILD_CFLAGS += $(call cc-disable-warning, initializer-overrides) +KBUILD_CFLAGS += $(call cc-disable-warning, unused-value) +KBUILD_CFLAGS += $(call cc-disable-warning, format) +KBUILD_CFLAGS += $(call cc-disable-warning, unknown-warning-option) +KBUILD_CFLAGS += $(call cc-disable-warning, sign-compare) +KBUILD_CFLAGS += $(call cc-disable-warning, format-zero-length) +KBUILD_CFLAGS += $(call cc-disable-warning, uninitialized) +endif +endif diff --git a/scripts/Makefile.host b/scripts/Makefile.host new file mode 100644 index 0000000000..097926dd4d --- /dev/null +++ b/scripts/Makefile.host @@ -0,0 +1,128 @@ +# ========================================================================== +# Building binaries on the host system +# Binaries are used during the compilation of the kernel, for example +# to preprocess a data file. +# +# Both C and C++ are supported, but preferred language is C for such utilities. +# +# Sample syntax (see Documentation/kbuild/makefiles.txt for reference) +# hostprogs-y := bin2hex +# Will compile bin2hex.c and create an executable named bin2hex +# +# hostprogs-y := lxdialog +# lxdialog-objs := checklist.o lxdialog.o +# Will compile lxdialog.c and checklist.c, and then link the executable +# lxdialog, based on checklist.o and lxdialog.o +# +# hostprogs-y := qconf +# qconf-cxxobjs := qconf.o +# qconf-objs := menu.o +# Will compile qconf as a C++ program, and menu as a C program. +# They are linked as C++ code to the executable qconf + +__hostprogs := $(sort $(hostprogs-y)) + +# C code +# Executables compiled from a single .c file +host-csingle := $(foreach m,$(__hostprogs), \ + $(if $($(m)-objs)$($(m)-cxxobjs),,$(m))) + +# C executables linked based on several .o files +host-cmulti := $(foreach m,$(__hostprogs),\ + $(if $($(m)-cxxobjs),,$(if $($(m)-objs),$(m)))) + +# Object (.o) files compiled from .c files +host-cobjs := $(sort $(foreach m,$(__hostprogs),$($(m)-objs))) + +# C++ code +# C++ executables compiled from at least one .cc file +# and zero or more .c files +host-cxxmulti := $(foreach m,$(__hostprogs),$(if $($(m)-cxxobjs),$(m))) + +# C++ Object (.o) files compiled from .cc files +host-cxxobjs := $(sort $(foreach m,$(host-cxxmulti),$($(m)-cxxobjs))) + +# output directory for programs/.o files +# hostprogs-y := tools/build may have been specified. +# Retrieve also directory of .o files from prog-objs or prog-cxxobjs notation +host-objdirs := $(dir $(__hostprogs) $(host-cobjs) $(host-cxxobjs)) + +host-objdirs := $(strip $(sort $(filter-out ./,$(host-objdirs)))) + + +__hostprogs := $(addprefix $(obj)/,$(__hostprogs)) +host-csingle := $(addprefix $(obj)/,$(host-csingle)) +host-cmulti := $(addprefix $(obj)/,$(host-cmulti)) +host-cobjs := $(addprefix $(obj)/,$(host-cobjs)) +host-cxxmulti := $(addprefix $(obj)/,$(host-cxxmulti)) +host-cxxobjs := $(addprefix $(obj)/,$(host-cxxobjs)) +host-objdirs := $(addprefix $(obj)/,$(host-objdirs)) + +obj-dirs += $(host-objdirs) + +##### +# Handle options to gcc. Support building with separate output directory + +_hostc_flags = $(HOSTCFLAGS) $(HOST_EXTRACFLAGS) \ + $(HOSTCFLAGS_$(basetarget).o) +_hostcxx_flags = $(HOSTCXXFLAGS) $(HOST_EXTRACXXFLAGS) \ + $(HOSTCXXFLAGS_$(basetarget).o) + +ifeq ($(KBUILD_SRC),) +__hostc_flags = $(_hostc_flags) +__hostcxx_flags = $(_hostcxx_flags) +else +__hostc_flags = -I$(obj) $(call flags,_hostc_flags) +__hostcxx_flags = -I$(obj) $(call flags,_hostcxx_flags) +endif + +hostc_flags = -Wp,-MD,$(depfile) $(__hostc_flags) +hostcxx_flags = -Wp,-MD,$(depfile) $(__hostcxx_flags) + +##### +# Compile programs on the host + +# Create executable from a single .c file +# host-csingle -> Executable +quiet_cmd_host-csingle = HOSTCC $@ + cmd_host-csingle = $(HOSTCC) $(hostc_flags) -o $@ $< \ + $(HOST_LOADLIBES) $(HOSTLOADLIBES_$(@F)) +$(host-csingle): $(obj)/%: $(src)/%.c FORCE + $(call if_changed_dep,host-csingle) + +# Link an executable based on list of .o files, all plain c +# host-cmulti -> executable +quiet_cmd_host-cmulti = HOSTLD $@ + cmd_host-cmulti = $(HOSTCC) $(HOSTLDFLAGS) -o $@ \ + $(addprefix $(obj)/,$($(@F)-objs)) \ + $(HOST_LOADLIBES) $(HOSTLOADLIBES_$(@F)) +$(host-cmulti): FORCE + $(call if_changed,host-cmulti) +$(call multi_depend, $(host-cmulti), , -objs) + +# Create .o file from a single .c file +# host-cobjs -> .o +quiet_cmd_host-cobjs = HOSTCC $@ + cmd_host-cobjs = $(HOSTCC) $(hostc_flags) -c -o $@ $< +$(host-cobjs): $(obj)/%.o: $(src)/%.c FORCE + $(call if_changed_dep,host-cobjs) + +# Link an executable based on list of .o files, a mixture of .c and .cc +# host-cxxmulti -> executable +quiet_cmd_host-cxxmulti = HOSTLD $@ + cmd_host-cxxmulti = $(HOSTCXX) $(HOSTLDFLAGS) -o $@ \ + $(foreach o,objs cxxobjs,\ + $(addprefix $(obj)/,$($(@F)-$(o)))) \ + $(HOST_LOADLIBES) $(HOSTLOADLIBES_$(@F)) +$(host-cxxmulti): FORCE + $(call if_changed,host-cxxmulti) +$(call multi_depend, $(host-cxxmulti), , -objs -cxxobjs) + +# Create .o file from a single .cc (C++) file +quiet_cmd_host-cxxobjs = HOSTCXX $@ + cmd_host-cxxobjs = $(HOSTCXX) $(hostcxx_flags) -c -o $@ $< +$(host-cxxobjs): $(obj)/%.o: $(src)/%.cc FORCE + $(call if_changed_dep,host-cxxobjs) + +targets += $(host-csingle) $(host-cmulti) $(host-cobjs)\ + $(host-cxxmulti) $(host-cxxobjs) diff --git a/scripts/Makefile.lib b/scripts/Makefile.lib new file mode 100644 index 0000000000..36beddacf0 --- /dev/null +++ b/scripts/Makefile.lib @@ -0,0 +1,296 @@ +# Backward compatibility +asflags-y += $(EXTRA_AFLAGS) +ccflags-y += $(EXTRA_CFLAGS) +cppflags-y += $(EXTRA_CPPFLAGS) +ldflags-y += $(EXTRA_LDFLAGS) + +# +# flags that take effect in sub directories +export KBUILD_SUBDIR_ASFLAGS := $(KBUILD_SUBDIR_ASFLAGS) $(subdir-asflags-y) +export KBUILD_SUBDIR_CCFLAGS := $(KBUILD_SUBDIR_CCFLAGS) $(subdir-ccflags-y) + +# Figure out what we need to build from the various variables +# =========================================================================== + +# Libraries are always collected in one lib file. +# Filter out objects already built-in + +lib-y := $(filter-out $(obj-y), $(sort $(lib-y))) + + +# Handle objects in subdirs +# --------------------------------------------------------------------------- +# o if we encounter foo/ in $(obj-y), replace it by foo/built-in.o +# and add the directory to the list of dirs to descend into: $(subdir-y) + +__subdir-y := $(patsubst %/,%,$(filter %/, $(obj-y))) +subdir-y += $(__subdir-y) +obj-y := $(patsubst %/, %/built-in.o, $(obj-y)) + +# Subdirectories we need to descend into + +subdir-ym := $(sort $(subdir-y)) + +# if $(foo-objs) exists, foo.o is a composite object +multi-used-y := $(sort $(foreach m,$(obj-y), $(if $(strip $($(m:.o=-objs)) $($(m:.o=-y))), $(m)))) +multi-used := $(multi-used-y) + +# Build list of the parts of our composite objects, our composite +# objects depend on those (obviously) +multi-objs-y := $(foreach m, $(multi-used-y), $($(m:.o=-objs)) $($(m:.o=-y))) +multi-objs := $(multi-objs-y) + +# $(subdir-obj-y) is the list of objects in $(obj-y) which uses dir/ to +# tell kbuild to descend +subdir-obj-y := $(filter %/built-in.o, $(obj-y)) + +# $(obj-dirs) is a list of directories that contain object files +obj-dirs := $(dir $(obj-y)) + +# Replace multi-part objects by their individual parts, look at local dir only +real-objs-y := $(foreach m, $(filter-out $(subdir-obj-y), $(obj-y)), $(if $(strip $($(m:.o=-objs)) $($(m:.o=-y))),$($(m:.o=-objs)) $($(m:.o=-y)),$(m))) $(extra-y) + +# Add subdir path + +extra-y := $(addprefix $(obj)/,$(extra-y)) +always := $(addprefix $(obj)/,$(always)) +targets := $(addprefix $(obj)/,$(targets)) +obj-y := $(addprefix $(obj)/,$(obj-y)) +lib-y := $(addprefix $(obj)/,$(lib-y)) +subdir-obj-y := $(addprefix $(obj)/,$(subdir-obj-y)) +real-objs-y := $(addprefix $(obj)/,$(real-objs-y)) +multi-used-y := $(addprefix $(obj)/,$(multi-used-y)) +multi-objs-y := $(addprefix $(obj)/,$(multi-objs-y)) +subdir-ym := $(addprefix $(obj)/,$(subdir-ym)) +obj-dirs := $(addprefix $(obj)/,$(obj-dirs)) + +orig_c_flags = $(KBUILD_CPPFLAGS) $(KBUILD_CFLAGS) $(KBUILD_SUBDIR_CCFLAGS) \ + $(ccflags-y) $(CFLAGS_$(basetarget).o) +_c_flags = $(filter-out $(CFLAGS_REMOVE_$(basetarget).o), $(orig_c_flags)) +orig_a_flags = $(KBUILD_CPPFLAGS) $(KBUILD_AFLAGS) $(KBUILD_SUBDIR_ASFLAGS) \ + $(asflags-y) $(AFLAGS_$(basetarget).o) +_a_flags = $(filter-out $(AFLAGS_REMOVE_$(basetarget).o), $(orig_a_flags)) +_cpp_flags = $(KBUILD_CPPFLAGS) $(KBUILD_CFLAGS) $(cppflags-y) $(CPPFLAGS_$(@F)) + +# Remove all non c++ compatible flags +_cxx_flags := $(filter-out -Wno-pointer-sign -Werror=implicit-int -std=gnu11,$(_c_flags)) + +# If building the kernel in a separate objtree expand all occurrences +# of -Idir to -I$(srctree)/dir except for absolute paths (starting with '/'). + +ifeq ($(KBUILD_SRC),) +__c_flags = $(_c_flags) +__cxx_flags = $(_cxx_flags) +__a_flags = $(_a_flags) +__cpp_flags = $(_cpp_flags) +else + +# -I$(obj) locates generated .h files +# $(call addtree,-I$(obj)) locates .h files in srctree, from generated .c files +# and locates generated .h files +# FIXME: Replace both with specific CFLAGS* statements in the makefiles +__c_flags = $(call addtree,-I$(obj)) $(call flags,_c_flags) +__cxx_flags = $(call flags,_cxx_flags) +__a_flags = $(call flags,_a_flags) +__cpp_flags = $(call flags,_cpp_flags) +endif + +c_flags = -Wp,-MD,$(depfile) $(NOSTDINC_FLAGS) $(MYAPPINCLUDE) \ + $(__c_flags) $(modkern_cflags) + +cxx_flags = -Wp,-MD,$(depfile) $(NOSTDINC_FLAGS) $(MYAPPINCLUDE) \ + $(__cxx_flags) $(modkern_cflags) + +a_flags = -Wp,-MD,$(depfile) $(NOSTDINC_FLAGS) $(MYAPPINCLUDE) \ + $(__a_flags) $(modkern_aflags) + +cpp_flags = -Wp,-MD,$(depfile) $(NOSTDINC_FLAGS) $(MYAPPINCLUDE) \ + $(__cpp_flags) + +ld_flags = $(LDFLAGS) $(ldflags-y) + +# Useful for describing the dependency of composite objects +# Usage: +# $(call multi_depend, multi_used_targets, suffix_to_remove, suffix_to_add) +define multi_depend +$(foreach m, $(notdir $1), \ + $(eval $(obj)/$m: \ + $(addprefix $(obj)/, $(foreach s, $3, $($(m:%$(strip $2)=%$(s))))))) +endef + +ifdef REGENERATE_PARSERS + +# GPERF +# --------------------------------------------------------------------------- +quiet_cmd_gperf = GPERF $@ + cmd_gperf = gperf -t --output-file $@ -a -C -E -g -k 1,3,$$ -p -t $< + +.PRECIOUS: $(src)/%.hash.c_shipped +$(src)/%.hash.c_shipped: $(src)/%.gperf + $(call cmd,gperf) + +# LEX +# --------------------------------------------------------------------------- +LEX_PREFIX = $(if $(LEX_PREFIX_${baseprereq}),$(LEX_PREFIX_${baseprereq}),yy) + +quiet_cmd_flex = LEX $@ + cmd_flex = flex -o$@ -L -P $(LEX_PREFIX) $< + +.PRECIOUS: $(src)/%.lex.c_shipped +$(src)/%.lex.c_shipped: $(src)/%.l + $(call cmd,flex) + +# YACC +# --------------------------------------------------------------------------- +YACC_PREFIX = $(if $(YACC_PREFIX_${baseprereq}),$(YACC_PREFIX_${baseprereq}),yy) + +quiet_cmd_bison = YACC $@ + cmd_bison = bison -o$@ -t -l -p $(YACC_PREFIX) $< + +.PRECIOUS: $(src)/%.tab.c_shipped +$(src)/%.tab.c_shipped: $(src)/%.y + $(call cmd,bison) + +quiet_cmd_bison_h = YACC $@ + cmd_bison_h = bison -o/dev/null --defines=$@ -t -l -p $(YACC_PREFIX) $< + +.PRECIOUS: $(src)/%.tab.h_shipped +$(src)/%.tab.h_shipped: $(src)/%.y + $(call cmd,bison_h) + +endif + +# Shipped files +# =========================================================================== + +quiet_cmd_shipped = SHIPPED $@ +cmd_shipped = cat $< > $@ + +$(obj)/%: $(src)/%_shipped + $(call cmd,shipped) + +# Commands useful for building a boot image +# =========================================================================== +# +# Use as following: +# +# target: source(s) FORCE +# $(if_changed,ld/objcopy/gzip) +# +# and add target to extra-y so that we know we have to +# read in the saved command line + +# Linking +# --------------------------------------------------------------------------- + +quiet_cmd_ld = LD $@ +cmd_ld = $(LD) $(LDFLAGS) $(ldflags-y) $(LDFLAGS_$(@F)) \ + $(filter-out FORCE,$^) -o $@ + +# Objcopy +# --------------------------------------------------------------------------- + +quiet_cmd_objcopy = OBJCOPY $@ +cmd_objcopy = $(OBJCOPY) $(OBJCOPYFLAGS) $(OBJCOPYFLAGS_$(@F)) $< $@ + +# Gzip +# --------------------------------------------------------------------------- + +quiet_cmd_gzip = GZIP $@ +cmd_gzip = (cat $(filter-out FORCE,$^) | gzip -n -f -9 > $@) || \ + (rm -f $@ ; false) + +# Bzip2 +# --------------------------------------------------------------------------- + +# Bzip2 and LZMA do not include size in file... so we have to fake that; +# append the size as a 32-bit littleendian number as gzip does. +size_append = printf $(shell \ +dec_size=0; \ +for F in $1; do \ + fsize=$$(stat -c "%s" $$F); \ + dec_size=$$(expr $$dec_size + $$fsize); \ +done; \ +printf "%08x\n" $$dec_size | \ + sed 's/\(..\)/\1 /g' | { \ + read ch0 ch1 ch2 ch3; \ + for ch in $$ch3 $$ch2 $$ch1 $$ch0; do \ + printf '%s%03o' '\\' $$((0x$$ch)); \ + done; \ + } \ +) + +quiet_cmd_bzip2 = BZIP2 $@ +cmd_bzip2 = (cat $(filter-out FORCE,$^) | \ + bzip2 -9 && $(call size_append, $(filter-out FORCE,$^))) > $@ || \ + (rm -f $@ ; false) + +# Lzma +# --------------------------------------------------------------------------- + +quiet_cmd_lzma = LZMA $@ +cmd_lzma = (cat $(filter-out FORCE,$^) | \ + lzma -9 && $(call size_append, $(filter-out FORCE,$^))) > $@ || \ + (rm -f $@ ; false) + +quiet_cmd_lzo = LZO $@ +cmd_lzo = (cat $(filter-out FORCE,$^) | \ + lzop -9 && $(call size_append, $(filter-out FORCE,$^))) > $@ || \ + (rm -f $@ ; false) + +quiet_cmd_lz4 = LZ4 $@ +cmd_lz4 = (cat $(filter-out FORCE,$^) | \ + lz4c -l -c1 stdin stdout && $(call size_append, $(filter-out FORCE,$^))) > $@ || \ + (rm -f $@ ; false) + +# U-Boot mkimage +# --------------------------------------------------------------------------- + +MKIMAGE := $(srctree)/scripts/mkuboot.sh + +# SRCARCH just happens to match slightly more than ARCH (on sparc), so reduces +# the number of overrides in arch makefiles +UIMAGE_ARCH ?= $(SRCARCH) +UIMAGE_COMPRESSION ?= $(if $(2),$(2),none) +UIMAGE_OPTS-y ?= +UIMAGE_TYPE ?= kernel +UIMAGE_LOADADDR ?= arch_must_set_this +UIMAGE_ENTRYADDR ?= $(UIMAGE_LOADADDR) +UIMAGE_NAME ?= 'Linux-$(KERNELRELEASE)' +UIMAGE_IN ?= $< +UIMAGE_OUT ?= $@ + +quiet_cmd_uimage = UIMAGE $(UIMAGE_OUT) + cmd_uimage = $(CONFIG_SHELL) $(MKIMAGE) -A $(UIMAGE_ARCH) -O linux \ + -C $(UIMAGE_COMPRESSION) $(UIMAGE_OPTS-y) \ + -T $(UIMAGE_TYPE) \ + -a $(UIMAGE_LOADADDR) -e $(UIMAGE_ENTRYADDR) \ + -n $(UIMAGE_NAME) -d $(UIMAGE_IN) $(UIMAGE_OUT) + +# XZ +# --------------------------------------------------------------------------- +# Use xzkern to compress the kernel image and xzmisc to compress other things. +# +# xzkern uses a big LZMA2 dictionary since it doesn't increase memory usage +# of the kernel decompressor. A BCJ filter is used if it is available for +# the target architecture. xzkern also appends uncompressed size of the data +# using size_append. The .xz format has the size information available at +# the end of the file too, but it's in more complex format and it's good to +# avoid changing the part of the boot code that reads the uncompressed size. +# Note that the bytes added by size_append will make the xz tool think that +# the file is corrupt. This is expected. +# +# xzmisc doesn't use size_append, so it can be used to create normal .xz +# files. xzmisc uses smaller LZMA2 dictionary than xzkern, because a very +# big dictionary would increase the memory usage too much in the multi-call +# decompression mode. A BCJ filter isn't used either. +quiet_cmd_xzkern = XZKERN $@ +cmd_xzkern = (cat $(filter-out FORCE,$^) | \ + sh $(srctree)/scripts/xz_wrap.sh && \ + $(call size_append, $(filter-out FORCE,$^))) > $@ || \ + (rm -f $@ ; false) + +quiet_cmd_xzmisc = XZMISC $@ +cmd_xzmisc = (cat $(filter-out FORCE,$^) | \ + xz --check=crc32 --lzma2=dict=1MiB) > $@ || \ + (rm -f $@ ; false) diff --git a/scripts/basic/.gitignore b/scripts/basic/.gitignore new file mode 100644 index 0000000000..9528ec9e5a --- /dev/null +++ b/scripts/basic/.gitignore @@ -0,0 +1,2 @@ +fixdep +bin2c diff --git a/scripts/basic/Makefile b/scripts/basic/Makefile new file mode 100644 index 0000000000..ec10d9345b --- /dev/null +++ b/scripts/basic/Makefile @@ -0,0 +1,16 @@ +### +# Makefile.basic lists the most basic programs used during the build process. +# The programs listed herein are what are needed to do the basic stuff, +# such as fix file dependencies. +# This initial step is needed to avoid files to be recompiled +# when kernel configuration changes (which is what happens when +# .config is included by main Makefile. +# --------------------------------------------------------------------------- +# fixdep: Used to generate dependency information during build process + +hostprogs-y := fixdep +hostprogs-$(CONFIG_BUILD_BIN2C) += bin2c +always := $(hostprogs-y) + +# fixdep is needed to compile other host programs +$(addprefix $(obj)/,$(filter-out fixdep,$(always))): $(obj)/fixdep diff --git a/scripts/basic/bin2c.c b/scripts/basic/bin2c.c new file mode 100644 index 0000000000..af187e6953 --- /dev/null +++ b/scripts/basic/bin2c.c @@ -0,0 +1,35 @@ +/* + * Unloved program to convert a binary on stdin to a C include on stdout + * + * Jan 1999 Matt Mackall + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + */ + +#include + +int main(int argc, char *argv[]) +{ + int ch, total = 0; + + if (argc > 1) + printf("const char %s[] %s=\n", + argv[1], argc > 2 ? argv[2] : ""); + + do { + printf("\t\""); + while ((ch = getchar()) != EOF) { + total++; + printf("\\x%02x", ch); + if (total % 16 == 0) + break; + } + printf("\"\n"); + } while (ch != EOF); + + if (argc > 1) + printf("\t;\n\nconst int %s_size = %d;\n", argv[1], total); + + return 0; +} diff --git a/scripts/basic/fixdep.c b/scripts/basic/fixdep.c new file mode 100644 index 0000000000..746ec1ece6 --- /dev/null +++ b/scripts/basic/fixdep.c @@ -0,0 +1,479 @@ +/* + * "Optimize" a list of dependencies as spit out by gcc -MD + * for the kernel build + * =========================================================================== + * + * Author Kai Germaschewski + * Copyright 2002 by Kai Germaschewski + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + * + * + * Introduction: + * + * gcc produces a very nice and correct list of dependencies which + * tells make when to remake a file. + * + * To use this list as-is however has the drawback that virtually + * every file in the kernel includes autoconf.h. + * + * If the user re-runs make *config, autoconf.h will be + * regenerated. make notices that and will rebuild every file which + * includes autoconf.h, i.e. basically all files. This is extremely + * annoying if the user just changed CONFIG_HIS_DRIVER from n to m. + * + * So we play the same trick that "mkdep" played before. We replace + * the dependency on autoconf.h by a dependency on every config + * option which is mentioned in any of the listed prequisites. + * + * kconfig populates a tree in include/config/ with an empty file + * for each config symbol and when the configuration is updated + * the files representing changed config options are touched + * which then let make pick up the changes and the files that use + * the config symbols are rebuilt. + * + * So if the user changes his CONFIG_HIS_DRIVER option, only the objects + * which depend on "include/linux/config/his/driver.h" will be rebuilt, + * so most likely only his driver ;-) + * + * The idea above dates, by the way, back to Michael E Chastain, AFAIK. + * + * So to get dependencies right, there are two issues: + * o if any of the files the compiler read changed, we need to rebuild + * o if the command line given to the compile the file changed, we + * better rebuild as well. + * + * The former is handled by using the -MD output, the later by saving + * the command line used to compile the old object and comparing it + * to the one we would now use. + * + * Again, also this idea is pretty old and has been discussed on + * kbuild-devel a long time ago. I don't have a sensibly working + * internet connection right now, so I rather don't mention names + * without double checking. + * + * This code here has been based partially based on mkdep.c, which + * says the following about its history: + * + * Copyright abandoned, Michael Chastain, . + * This is a C version of syncdep.pl by Werner Almesberger. + * + * + * It is invoked as + * + * fixdep + * + * and will read the dependency file + * + * The transformed dependency snipped is written to stdout. + * + * It first generates a line + * + * cmd_ = + * + * and then basically copies the ..d file to stdout, in the + * process filtering out the dependency on autoconf.h and adding + * dependencies on include/config/my/option.h for every + * CONFIG_MY_OPTION encountered in any of the prequisites. + * + * It will also filter out all the dependencies on *.ver. We need + * to make sure that the generated version checksum are globally up + * to date before even starting the recursive build, so it's too late + * at this point anyway. + * + * The algorithm to grep for "CONFIG_..." is bit unusual, but should + * be fast ;-) We don't even try to really parse the header files, but + * merely grep, i.e. if CONFIG_FOO is mentioned in a comment, it will + * be picked up as well. It's not a problem with respect to + * correctness, since that can only give too many dependencies, thus + * we cannot miss a rebuild. Since people tend to not mention totally + * unrelated CONFIG_ options all over the place, it's not an + * efficiency problem either. + * + * (Note: it'd be easy to port over the complete mkdep state machine, + * but I don't think the added complexity is worth it) + */ +/* + * Note 2: if somebody writes HELLO_CONFIG_BOOM in a file, it will depend onto + * CONFIG_BOOM. This could seem a bug (not too hard to fix), but please do not + * fix it! Some UserModeLinux files (look at arch/um/) call CONFIG_BOOM as + * UML_CONFIG_BOOM, to avoid conflicts with /usr/include/linux/autoconf.h, + * through arch/um/include/uml-config.h; this fixdep "bug" makes sure that + * those files will have correct dependencies. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define INT_CONF ntohl(0x434f4e46) +#define INT_ONFI ntohl(0x4f4e4649) +#define INT_NFIG ntohl(0x4e464947) +#define INT_FIG_ ntohl(0x4649475f) + +int insert_extra_deps; +char *target; +char *depfile; +char *cmdline; + +static void usage(void) +{ + fprintf(stderr, "Usage: fixdep [-e] \n"); + fprintf(stderr, " -e insert extra dependencies given on stdin\n"); + exit(1); +} + +/* + * Print out the commandline prefixed with cmd_ := + */ +static void print_cmdline(void) +{ + printf("cmd_%s := %s\n\n", target, cmdline); +} + +/* + * Print out a dependency path from a symbol name + */ +static void print_config(const char *m, int slen) +{ + int c, i; + + printf(" $(wildcard include/config/"); + for (i = 0; i < slen; i++) { + c = m[i]; + if (c == '_') + c = '/'; + else + c = tolower(c); + putchar(c); + } + printf(".h) \\\n"); +} + +static void do_extra_deps(void) +{ + if (insert_extra_deps) { + char buf[80]; + while(fgets(buf, sizeof(buf), stdin)) { + int len = strlen(buf); + if (len < 2 || buf[len-1] != '\n') { + fprintf(stderr, "fixdep: bad data on stdin\n"); + exit(1); + } + print_config(buf, len-1); + } + } +} + +struct item { + struct item *next; + unsigned int len; + unsigned int hash; + char name[0]; +}; + +#define HASHSZ 256 +static struct item *hashtab[HASHSZ]; + +static unsigned int strhash(const char *str, unsigned int sz) +{ + /* fnv32 hash */ + unsigned int i, hash = 2166136261U; + + for (i = 0; i < sz; i++) + hash = (hash ^ str[i]) * 0x01000193; + return hash; +} + +/* + * Lookup a value in the configuration string. + */ +static int is_defined_config(const char *name, int len, unsigned int hash) +{ + struct item *aux; + + for (aux = hashtab[hash % HASHSZ]; aux; aux = aux->next) { + if (aux->hash == hash && aux->len == len && + memcmp(aux->name, name, len) == 0) + return 1; + } + return 0; +} + +/* + * Add a new value to the configuration string. + */ +static void define_config(const char *name, int len, unsigned int hash) +{ + struct item *aux = malloc(sizeof(*aux) + len); + + if (!aux) { + perror("fixdep:malloc"); + exit(1); + } + memcpy(aux->name, name, len); + aux->len = len; + aux->hash = hash; + aux->next = hashtab[hash % HASHSZ]; + hashtab[hash % HASHSZ] = aux; +} + +/* + * Record the use of a CONFIG_* word. + */ +static void use_config(const char *m, int slen) +{ + unsigned int hash = strhash(m, slen); + + if (is_defined_config(m, slen, hash)) + return; + + define_config(m, slen, hash); + print_config(m, slen); +} + +static void parse_config_file(const char *map, size_t len) +{ + const int *end = (const int *) (map + len); + /* start at +1, so that p can never be < map */ + const int *m = (const int *) map + 1; + const char *p, *q; + + for (; m < end; m++) { + if (*m == INT_CONF) { p = (char *) m ; goto conf; } + if (*m == INT_ONFI) { p = (char *) m-1; goto conf; } + if (*m == INT_NFIG) { p = (char *) m-2; goto conf; } + if (*m == INT_FIG_) { p = (char *) m-3; goto conf; } + continue; + conf: + if (p > map + len - 7) + continue; + if (memcmp(p, "CONFIG_", 7)) + continue; + p += 7; + for (q = p; q < map + len; q++) { + if (!(isalnum(*q) || *q == '_')) + goto found; + } + continue; + + found: + if (!memcmp(q - 7, "_MODULE", 7)) + q -= 7; + if (q - p < 0) + continue; + use_config(p, q - p); + } +} + +/* test if s ends in sub */ +static int strrcmp(const char *s, const char *sub) +{ + int slen = strlen(s); + int sublen = strlen(sub); + + if (sublen > slen) + return 1; + + return memcmp(s + slen - sublen, sub, sublen); +} + +static void do_config_file(const char *filename) +{ + struct stat st; + int fd; + void *map; + + fd = open(filename, O_RDONLY); + if (fd < 0) { + fprintf(stderr, "fixdep: error opening config file: "); + perror(filename); + exit(2); + } + if (fstat(fd, &st) < 0) { + fprintf(stderr, "fixdep: error fstat'ing config file: "); + perror(filename); + exit(2); + } + if (st.st_size == 0) { + close(fd); + return; + } + map = mmap(NULL, st.st_size, PROT_READ, MAP_PRIVATE, fd, 0); + if ((long) map == -1) { + perror("fixdep: mmap"); + close(fd); + return; + } + + parse_config_file(map, st.st_size); + + munmap(map, st.st_size); + + close(fd); +} + +/* + * Important: The below generated source_foo.o and deps_foo.o variable + * assignments are parsed not only by make, but also by the rather simple + * parser in scripts/mod/sumversion.c. + */ +static void parse_dep_file(void *map, size_t len) +{ + char *m = map; + char *end = m + len; + char *p; + char s[PATH_MAX]; + int is_target; + int saw_any_target = 0; + int is_first_dep = 0; + + while (m < end) { + /* Skip any "white space" */ + while (m < end && (*m == ' ' || *m == '\\' || *m == '\n')) + m++; + /* Find next "white space" */ + p = m; + while (p < end && *p != ' ' && *p != '\\' && *p != '\n') + p++; + /* Is the token we found a target name? */ + is_target = (*(p-1) == ':'); + /* Don't write any target names into the dependency file */ + if (is_target) { + /* The /next/ file is the first dependency */ + is_first_dep = 1; + } else { + /* Save this token/filename */ + memcpy(s, m, p-m); + s[p - m] = 0; + + /* Ignore certain dependencies */ + if (strrcmp(s, "include/generated/autoconf.h") && + strrcmp(s, "include/generated/autoksyms.h") && + strrcmp(s, "arch/um/include/uml-config.h") && + strrcmp(s, "include/linux/kconfig.h") && + strrcmp(s, ".ver")) { + /* + * Do not list the source file as dependency, + * so that kbuild is not confused if a .c file + * is rewritten into .S or vice versa. Storing + * it in source_* is needed for modpost to + * compute srcversions. + */ + if (is_first_dep) { + /* + * If processing the concatenation of + * multiple dependency files, only + * process the first target name, which + * will be the original source name, + * and ignore any other target names, + * which will be intermediate temporary + * files. + */ + if (!saw_any_target) { + saw_any_target = 1; + printf("source_%s := %s\n\n", + target, s); + printf("deps_%s := \\\n", + target); + } + is_first_dep = 0; + } else + printf(" %s \\\n", s); + do_config_file(s); + } + } + /* + * Start searching for next token immediately after the first + * "whitespace" character that follows this token. + */ + m = p + 1; + } + + if (!saw_any_target) { + fprintf(stderr, "fixdep: parse error; no targets found\n"); + exit(1); + } + + do_extra_deps(); + + printf("\n%s: $(deps_%s)\n\n", target, target); + printf("$(deps_%s):\n", target); +} + +static void print_deps(void) +{ + struct stat st; + int fd; + void *map; + + fd = open(depfile, O_RDONLY); + if (fd < 0) { + fprintf(stderr, "fixdep: error opening depfile: "); + perror(depfile); + exit(2); + } + if (fstat(fd, &st) < 0) { + fprintf(stderr, "fixdep: error fstat'ing depfile: "); + perror(depfile); + exit(2); + } + if (st.st_size == 0) { + fprintf(stderr,"fixdep: %s is empty\n",depfile); + close(fd); + return; + } + map = mmap(NULL, st.st_size, PROT_READ, MAP_PRIVATE, fd, 0); + if ((long) map == -1) { + perror("fixdep: mmap"); + close(fd); + return; + } + + parse_dep_file(map, st.st_size); + + munmap(map, st.st_size); + + close(fd); +} + +static void traps(void) +{ + static char test[] __attribute__((aligned(sizeof(int)))) = "CONF"; + int *p = (int *)test; + + if (*p != INT_CONF) { + fprintf(stderr, "fixdep: sizeof(int) != 4 or wrong endianness? %#x\n", + *p); + exit(2); + } +} + +int main(int argc, char *argv[]) +{ + traps(); + + if (argc == 5 && !strcmp(argv[1], "-e")) { + insert_extra_deps = 1; + argv++; + } else if (argc != 4) + usage(); + + depfile = argv[1]; + target = argv[2]; + cmdline = argv[3]; + + print_cmdline(); + print_deps(); + + return 0; +} diff --git a/scripts/checkincludes.pl b/scripts/checkincludes.pl new file mode 100755 index 0000000000..97b2c6143f --- /dev/null +++ b/scripts/checkincludes.pl @@ -0,0 +1,89 @@ +#!/usr/bin/perl +# +# checkincludes: find/remove files included more than once +# +# Copyright abandoned, 2000, Niels Kristian Bech Jensen . +# Copyright 2009 Luis R. Rodriguez +# +# This script checks for duplicate includes. It also has support +# to remove them in place. Note that this will not take into +# consideration macros so you should run this only if you know +# you do have real dups and do not have them under #ifdef's. You +# could also just review the results. + +use strict; + +sub usage { + print "Usage: checkincludes.pl [-r]\n"; + print "By default we just warn of duplicates\n"; + print "To remove duplicated includes in place use -r\n"; + exit 1; +} + +my $remove = 0; + +if ($#ARGV < 0) { + usage(); +} + +if ($#ARGV >= 1) { + if ($ARGV[0] =~ /^-/) { + if ($ARGV[0] eq "-r") { + $remove = 1; + shift; + } else { + usage(); + } + } +} + +foreach my $file (@ARGV) { + open(my $f, '<', $file) + or die "Cannot open $file: $!.\n"; + + my %includedfiles = (); + my @file_lines = (); + + while (<$f>) { + if (m/^\s*#\s*include\s*[<"](\S*)[>"]/o) { + ++$includedfiles{$1}; + } + push(@file_lines, $_); + } + + close($f); + + if (!$remove) { + foreach my $filename (keys %includedfiles) { + if ($includedfiles{$filename} > 1) { + print "$file: $filename is included more than once.\n"; + } + } + next; + } + + open($f, '>', $file) + or die("Cannot write to $file: $!"); + + my $dups = 0; + foreach (@file_lines) { + if (m/^\s*#\s*include\s*[<"](\S*)[>"]/o) { + foreach my $filename (keys %includedfiles) { + if ($1 eq $filename) { + if ($includedfiles{$filename} > 1) { + $includedfiles{$filename}--; + $dups++; + } else { + print {$f} $_; + } + } + } + } else { + print {$f} $_; + } + } + if ($dups > 0) { + print "$file: removed $dups duplicate includes\n"; + } + close($f); +} diff --git a/scripts/gcc-goto.sh b/scripts/gcc-goto.sh new file mode 100755 index 0000000000..c9469d34ec --- /dev/null +++ b/scripts/gcc-goto.sh @@ -0,0 +1,21 @@ +#!/bin/sh +# Test for gcc 'asm goto' support +# Copyright (C) 2010, Jason Baron + +cat << "END" | $@ -x c - -c -o /dev/null >/dev/null 2>&1 && echo "y" +int main(void) +{ +#if defined(__arm__) || defined(__aarch64__) + /* + * Not related to asm goto, but used by jump label + * and broken on some ARM GCC versions (see GCC Bug 48637). + */ + static struct { int dummy; int state; } tp; + asm (".long %c0" :: "i" (&tp.state)); +#endif + +entry: + asm goto ("" :::: entry); + return 0; +} +END diff --git a/scripts/headerdep.pl b/scripts/headerdep.pl new file mode 100755 index 0000000000..8dd019bc5a --- /dev/null +++ b/scripts/headerdep.pl @@ -0,0 +1,192 @@ +#! /usr/bin/perl +# +# Detect cycles in the header file dependency graph +# Vegard Nossum +# + +use strict; +use warnings; + +use Getopt::Long; + +my $opt_all; +my @opt_include; +my $opt_graph; + +&Getopt::Long::Configure(qw(bundling pass_through)); +&GetOptions( + help => \&help, + version => \&version, + + all => \$opt_all, + "I=s" => \@opt_include, + graph => \$opt_graph, +); + +push @opt_include, 'include'; +my %deps = (); +my %linenos = (); + +my @headers = grep { strip($_) } @ARGV; + +parse_all(@headers); + +if($opt_graph) { + graph(); +} else { + detect_cycles(@headers); +} + + +sub help { + print "Usage: $0 [options] file...\n"; + print "\n"; + print "Options:\n"; + print " --all\n"; + print " --graph\n"; + print "\n"; + print " -I includedir\n"; + print "\n"; + print "To make nice graphs, try:\n"; + print " $0 --graph include/linux/kernel.h | dot -Tpng -o graph.png\n"; + exit; +} + +sub version { + print "headerdep version 2\n"; + exit; +} + +# Get a file name that is relative to our include paths +sub strip { + my $filename = shift; + + for my $i (@opt_include) { + my $stripped = $filename; + $stripped =~ s/^$i\///; + + return $stripped if $stripped ne $filename; + } + + return $filename; +} + +# Search for the file name in the list of include paths +sub search { + my $filename = shift; + return $filename if -f $filename; + + for my $i (@opt_include) { + my $path = "$i/$filename"; + return $path if -f $path; + } + return; +} + +sub parse_all { + # Parse all the headers. + my @queue = @_; + while(@queue) { + my $header = pop @queue; + next if exists $deps{$header}; + + $deps{$header} = [] unless exists $deps{$header}; + + my $path = search($header); + next unless $path; + + open(my $file, '<', $path) or die($!); + chomp(my @lines = <$file>); + close($file); + + for my $i (0 .. $#lines) { + my $line = $lines[$i]; + if(my($dep) = ($line =~ m/^#\s*include\s*<(.*?)>/)) { + push @queue, $dep; + push @{$deps{$header}}, [$i + 1, $dep]; + } + } + } +} + +sub print_cycle { + # $cycle[n] includes $cycle[n + 1]; + # $cycle[-1] will be the culprit + my $cycle = shift; + + # Adjust the line numbers + for my $i (0 .. $#$cycle - 1) { + $cycle->[$i]->[0] = $cycle->[$i + 1]->[0]; + } + $cycle->[-1]->[0] = 0; + + my $first = shift @$cycle; + my $last = pop @$cycle; + + my $msg = "In file included"; + printf "%s from %s,\n", $msg, $last->[1] if defined $last; + + for my $header (reverse @$cycle) { + printf "%s from %s:%d%s\n", + " " x length $msg, + $header->[1], $header->[0], + $header->[1] eq $last->[1] ? ' <-- here' : ''; + } + + printf "%s:%d: warning: recursive header inclusion\n", + $first->[1], $first->[0]; +} + +# Find and print the smallest cycle starting in the specified node. +sub detect_cycles { + my @queue = map { [[0, $_]] } @_; + while(@queue) { + my $top = pop @queue; + my $name = $top->[-1]->[1]; + + for my $dep (@{$deps{$name}}) { + my $chain = [@$top, [$dep->[0], $dep->[1]]]; + + # If the dep already exists in the chain, we have a + # cycle... + if(grep { $_->[1] eq $dep->[1] } @$top) { + print_cycle($chain); + next if $opt_all; + return; + } + + push @queue, $chain; + } + } +} + +sub mangle { + $_ = shift; + s/\//__/g; + s/\./_/g; + s/-/_/g; + $_; +} + +# Output dependency graph in GraphViz language. +sub graph { + print "digraph {\n"; + + print "\t/* vertices */\n"; + for my $header (keys %deps) { + printf "\t%s [label=\"%s\"];\n", + mangle($header), $header; + } + + print "\n"; + + print "\t/* edges */\n"; + for my $header (keys %deps) { + for my $dep (@{$deps{$header}}) { + printf "\t%s -> %s;\n", + mangle($header), mangle($dep->[1]); + } + } + + print "}\n"; +} diff --git a/scripts/kconfig/.gitignore b/scripts/kconfig/.gitignore new file mode 100644 index 0000000000..be603c4fef --- /dev/null +++ b/scripts/kconfig/.gitignore @@ -0,0 +1,22 @@ +# +# Generated files +# +config* +*.lex.c +*.tab.c +*.tab.h +zconf.hash.c +*.moc +gconf.glade.h +*.pot +*.mo + +# +# configuration programs +# +conf +mconf +nconf +qconf +gconf +kxgettext diff --git a/scripts/kconfig/Makefile b/scripts/kconfig/Makefile new file mode 100644 index 0000000000..736213dca0 --- /dev/null +++ b/scripts/kconfig/Makefile @@ -0,0 +1,294 @@ +# =========================================================================== +# Kernel configuration targets +# These targets are used from top-level makefile + +PHONY += xconfig gconfig menuconfig config silentoldconfig update-po-config \ + localmodconfig localyesconfig + +ifdef KBUILD_KCONFIG +Kconfig := $(KBUILD_KCONFIG) +else +Kconfig := Kconfig +endif + +ifeq ($(quiet),silent_) +silent := -s +endif + +# We need this, in case the user has it in its environment +unexport CONFIG_ + +xconfig: $(obj)/qconf + $< $(silent) $(Kconfig) + +gconfig: $(obj)/gconf + $< $(silent) $(Kconfig) + +menuconfig: $(obj)/mconf + $< $(silent) $(Kconfig) + +config: $(obj)/conf + $< $(silent) --oldaskconfig $(Kconfig) + +nconfig: $(obj)/nconf + $< $(silent) $(Kconfig) + +silentoldconfig: $(obj)/conf + $(Q)mkdir -p include/config include/generated + $< $(silent) --$@ $(Kconfig) + +localyesconfig localmodconfig: $(obj)/streamline_config.pl $(obj)/conf + $(Q)mkdir -p include/config include/generated + $(Q)perl $< --$@ $(srctree) $(Kconfig) > .tmp.config + $(Q)if [ -f .config ]; then \ + cmp -s .tmp.config .config || \ + (mv -f .config .config.old.1; \ + mv -f .tmp.config .config; \ + $(obj)/conf $(silent) --silentoldconfig $(Kconfig); \ + mv -f .config.old.1 .config.old) \ + else \ + mv -f .tmp.config .config; \ + $(obj)/conf $(silent) --silentoldconfig $(Kconfig); \ + fi + $(Q)rm -f .tmp.config + +# Create new linux.pot file +# Adjust charset to UTF-8 in .po file to accept UTF-8 in Kconfig files +update-po-config: $(obj)/kxgettext $(obj)/gconf.glade.h + $(Q)$(kecho) " GEN config.pot" + $(Q)xgettext --default-domain=linux \ + --add-comments --keyword=_ --keyword=N_ \ + --from-code=UTF-8 \ + --files-from=$(srctree)/scripts/kconfig/POTFILES.in \ + --directory=$(srctree) --directory=$(objtree) \ + --output $(obj)/config.pot + $(Q)sed -i s/CHARSET/UTF-8/ $(obj)/config.pot + $(Q)(for i in `ls $(srctree)/arch/*/Kconfig \ + $(srctree)/arch/*/um/Kconfig`; \ + do \ + $(kecho) " GEN $$i"; \ + $(obj)/kxgettext $$i \ + >> $(obj)/config.pot; \ + done ) + $(Q)$(kecho) " GEN linux.pot" + $(Q)msguniq --sort-by-file --to-code=UTF-8 $(obj)/config.pot \ + --output $(obj)/linux.pot + $(Q)rm -f $(obj)/config.pot + +# These targets map 1:1 to the commandline options of 'conf' +simple-targets := oldconfig allnoconfig allyesconfig allmodconfig \ + alldefconfig randconfig listnewconfig olddefconfig +PHONY += $(simple-targets) + +$(simple-targets): $(obj)/conf + $< $(silent) --$@ $(Kconfig) + +PHONY += oldnoconfig savedefconfig defconfig + +# oldnoconfig is an alias of olddefconfig, because people already are dependent +# on its behavior (sets new symbols to their default value but not 'n') with the +# counter-intuitive name. +oldnoconfig: olddefconfig + +savedefconfig: $(obj)/conf + $< $(silent) --$@=defconfig $(Kconfig) + +defconfig: $(obj)/conf +ifeq ($(KBUILD_DEFCONFIG),) + $< $(silent) --defconfig $(Kconfig) +else +ifneq ($(wildcard $(srctree)/$(SRCARCH)/configs/$(KBUILD_DEFCONFIG)),) + @$(kecho) "*** Default configuration is based on '$(KBUILD_DEFCONFIG)'" + $(Q)$< $(silent) --defconfig=$(SRCARCH)/configs/$(KBUILD_DEFCONFIG) $(Kconfig) +else + @$(kecho) "*** Default configuration is based on target '$(KBUILD_DEFCONFIG)'" + $(Q)$(MAKE) -f $(srctree)/Makefile $(KBUILD_DEFCONFIG) +endif +endif + +%_defconfig: $(obj)/conf + $(Q)$< $(silent) --defconfig=configs/$@ $(Kconfig) + +configfiles=$(wildcard $(srctree)/kernel/configs/$@ configs/$@) + +%.config: $(obj)/conf + $(if $(call configfiles),, $(error No configuration exists for this target on this architecture)) + $(Q)$(CONFIG_SHELL) $(srctree)/scripts/kconfig/merge_config.sh -m .config $(configfiles) + +$(Q)yes "" | $(MAKE) -f $(srctree)/Makefile oldconfig + +PHONY += kvmconfig +kvmconfig: kvm_guest.config + @: + +PHONY += xenconfig +xenconfig: xen.config + @: + +PHONY += tinyconfig +tinyconfig: + $(Q)$(MAKE) -f $(srctree)/Makefile allnoconfig tiny.config + +# Help text used by make help +help: + @echo ' config - Update current config utilising a line-oriented program' + @echo ' nconfig - Update current config utilising a ncurses menu based' + @echo ' program' + @echo ' menuconfig - Update current config utilising a menu based program' + @echo ' xconfig - Update current config utilising a Qt based front-end' + @echo ' gconfig - Update current config utilising a GTK+ based front-end' + @echo ' oldconfig - Update current config utilising a provided .config as base' + @echo ' localmodconfig - Update current config disabling modules not loaded' + @echo ' localyesconfig - Update current config converting local mods to core' + @echo ' silentoldconfig - Same as oldconfig, but quietly, additionally update deps' + @echo ' defconfig - New config with default from ARCH supplied defconfig' + @echo ' savedefconfig - Save current config as ./defconfig (minimal config)' + @echo ' allnoconfig - New config where all options are answered with no' + @echo ' allyesconfig - New config where all options are accepted with yes' + @echo ' allmodconfig - New config selecting modules when possible' + @echo ' alldefconfig - New config with all symbols set to default' + @echo ' randconfig - New config with random answer to all options' + @echo ' listnewconfig - List new options' + @echo ' olddefconfig - Same as silentoldconfig but sets new symbols to their' + @echo ' default value' + @echo ' kvmconfig - Enable additional options for kvm guest kernel support' + @echo ' xenconfig - Enable additional options for xen dom0 and guest kernel support' + @echo ' tinyconfig - Configure the tiniest possible kernel' + +# lxdialog stuff +check-lxdialog := $(srctree)/$(src)/lxdialog/check-lxdialog.sh + +# Use recursively expanded variables so we do not call gcc unless +# we really need to do so. (Do not call gcc as part of make mrproper) +HOST_EXTRACFLAGS += $(shell $(CONFIG_SHELL) $(check-lxdialog) -ccflags) \ + -DLOCALE + +# =========================================================================== +# Shared Makefile for the various kconfig executables: +# conf: Used for defconfig, oldconfig and related targets +# nconf: Used for the nconfig target. +# Utilizes ncurses +# mconf: Used for the menuconfig target +# Utilizes the lxdialog package +# qconf: Used for the xconfig target +# Based on Qt which needs to be installed to compile it +# gconf: Used for the gconfig target +# Based on GTK+ which needs to be installed to compile it +# object files used by all kconfig flavours + +lxdialog := lxdialog/checklist.o lxdialog/util.o lxdialog/inputbox.o +lxdialog += lxdialog/textbox.o lxdialog/yesno.o lxdialog/menubox.o + +conf-objs := conf.o zconf.tab.o +mconf-objs := mconf.o zconf.tab.o $(lxdialog) +nconf-objs := nconf.o zconf.tab.o nconf.gui.o +kxgettext-objs := kxgettext.o zconf.tab.o +qconf-cxxobjs := qconf.o +qconf-objs := zconf.tab.o +gconf-objs := gconf.o zconf.tab.o + +hostprogs-y := conf nconf mconf kxgettext qconf gconf + +clean-files := qconf.moc .tmp_qtcheck .tmp_gtkcheck +clean-files += zconf.tab.c zconf.lex.c zconf.hash.c gconf.glade.h +clean-files += config.pot linux.pot + +# Check that we have the required ncurses stuff installed for lxdialog (menuconfig) +PHONY += $(obj)/dochecklxdialog +$(addprefix $(obj)/,$(lxdialog)): $(obj)/dochecklxdialog +$(obj)/dochecklxdialog: + $(Q)$(CONFIG_SHELL) $(check-lxdialog) -check $(HOSTCC) $(HOST_EXTRACFLAGS) $(HOSTLOADLIBES_mconf) + +always := dochecklxdialog + +# Add environment specific flags +HOST_EXTRACFLAGS += $(shell $(CONFIG_SHELL) $(srctree)/$(src)/check.sh $(HOSTCC) $(HOSTCFLAGS)) + +# generated files seem to need this to find local include files +HOSTCFLAGS_zconf.lex.o := -I$(src) +HOSTCFLAGS_zconf.tab.o := -I$(src) + +LEX_PREFIX_zconf := zconf +YACC_PREFIX_zconf := zconf + +HOSTLOADLIBES_qconf = $(KC_QT_LIBS) +HOSTCXXFLAGS_qconf.o = $(KC_QT_CFLAGS) + +HOSTLOADLIBES_gconf = `pkg-config --libs gtk+-2.0 gmodule-2.0 libglade-2.0` +HOSTCFLAGS_gconf.o = `pkg-config --cflags gtk+-2.0 gmodule-2.0 libglade-2.0` \ + -Wno-missing-prototypes + +HOSTLOADLIBES_mconf = $(shell $(CONFIG_SHELL) $(check-lxdialog) -ldflags $(HOSTCC)) + +HOSTLOADLIBES_nconf = $(shell \ + pkg-config --libs menuw panelw ncursesw 2>/dev/null \ + || pkg-config --libs menu panel ncurses 2>/dev/null \ + || echo "-lmenu -lpanel -lncurses" ) +$(obj)/qconf.o: $(obj)/.tmp_qtcheck + +ifeq ($(MAKECMDGOALS),xconfig) +$(obj)/.tmp_qtcheck: $(src)/Makefile +-include $(obj)/.tmp_qtcheck + +# Qt needs some extra effort... +$(obj)/.tmp_qtcheck: + @set -e; $(kecho) " CHECK qt"; \ + if pkg-config --exists Qt5Core; then \ + cflags="-std=c++11 -fPIC `pkg-config --cflags Qt5Core Qt5Gui Qt5Widgets`"; \ + libs=`pkg-config --libs Qt5Core Qt5Gui Qt5Widgets`; \ + moc=`pkg-config --variable=host_bins Qt5Core`/moc; \ + elif pkg-config --exists QtCore; then \ + cflags=`pkg-config --cflags QtCore QtGui`; \ + libs=`pkg-config --libs QtCore QtGui`; \ + moc=`pkg-config --variable=moc_location QtCore`; \ + else \ + echo >&2 "*"; \ + echo >&2 "* Could not find Qt via pkg-config."; \ + echo >&2 "* Please install either Qt 4.8 or 5.x. and make sure it's in PKG_CONFIG_PATH"; \ + echo >&2 "*"; \ + exit 1; \ + fi; \ + echo "KC_QT_CFLAGS=$$cflags" > $@; \ + echo "KC_QT_LIBS=$$libs" >> $@; \ + echo "KC_QT_MOC=$$moc" >> $@ +endif + +$(obj)/gconf.o: $(obj)/.tmp_gtkcheck + +ifeq ($(MAKECMDGOALS),gconfig) +-include $(obj)/.tmp_gtkcheck + +# GTK+ needs some extra effort, too... +$(obj)/.tmp_gtkcheck: + @if `pkg-config --exists gtk+-2.0 gmodule-2.0 libglade-2.0`; then \ + if `pkg-config --atleast-version=2.0.0 gtk+-2.0`; then \ + touch $@; \ + else \ + echo >&2 "*"; \ + echo >&2 "* GTK+ is present but version >= 2.0.0 is required."; \ + echo >&2 "*"; \ + false; \ + fi \ + else \ + echo >&2 "*"; \ + echo >&2 "* Unable to find the GTK+ installation. Please make sure that"; \ + echo >&2 "* the GTK+ 2.0 development package is correctly installed..."; \ + echo >&2 "* You need gtk+-2.0, glib-2.0 and libglade-2.0."; \ + echo >&2 "*"; \ + false; \ + fi +endif + +$(obj)/zconf.tab.o: $(obj)/zconf.lex.c $(obj)/zconf.hash.c + +$(obj)/qconf.o: $(obj)/qconf.moc + +quiet_cmd_moc = MOC $@ + cmd_moc = $(KC_QT_MOC) -i $< -o $@ + +$(obj)/%.moc: $(src)/%.h $(obj)/.tmp_qtcheck + $(call cmd,moc) + +# Extract gconf menu items for i18n support +$(obj)/gconf.glade.h: $(obj)/gconf.glade + $(Q)intltool-extract --type=gettext/glade --srcdir=$(srctree) \ + $(obj)/gconf.glade diff --git a/scripts/kconfig/POTFILES.in b/scripts/kconfig/POTFILES.in new file mode 100644 index 0000000000..9674573969 --- /dev/null +++ b/scripts/kconfig/POTFILES.in @@ -0,0 +1,12 @@ +scripts/kconfig/lxdialog/checklist.c +scripts/kconfig/lxdialog/inputbox.c +scripts/kconfig/lxdialog/menubox.c +scripts/kconfig/lxdialog/textbox.c +scripts/kconfig/lxdialog/util.c +scripts/kconfig/lxdialog/yesno.c +scripts/kconfig/mconf.c +scripts/kconfig/conf.c +scripts/kconfig/confdata.c +scripts/kconfig/gconf.c +scripts/kconfig/gconf.glade.h +scripts/kconfig/qconf.cc diff --git a/scripts/kconfig/check.sh b/scripts/kconfig/check.sh new file mode 100755 index 0000000000..55b79ba1ba --- /dev/null +++ b/scripts/kconfig/check.sh @@ -0,0 +1,13 @@ +#!/bin/sh +# Needed for systems without gettext +$* -x c -o /dev/null - > /dev/null 2>&1 << EOF +#include +int main() +{ + gettext(""); + return 0; +} +EOF +if [ ! "$?" -eq "0" ]; then + echo -DKBUILD_NO_NLS; +fi diff --git a/scripts/kconfig/conf.c b/scripts/kconfig/conf.c new file mode 100644 index 0000000000..866369f10f --- /dev/null +++ b/scripts/kconfig/conf.c @@ -0,0 +1,723 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lkc.h" + +static void conf(struct menu *menu); +static void check_conf(struct menu *menu); +static void xfgets(char *str, int size, FILE *in); + +enum input_mode { + oldaskconfig, + silentoldconfig, + oldconfig, + allnoconfig, + allyesconfig, + allmodconfig, + alldefconfig, + randconfig, + defconfig, + savedefconfig, + listnewconfig, + olddefconfig, +} input_mode = oldaskconfig; + +static int indent = 1; +static int tty_stdio; +static int valid_stdin = 1; +static int sync_kconfig; +static int conf_cnt; +static char line[PATH_MAX]; +static struct menu *rootEntry; + +static void print_help(struct menu *menu) +{ + struct gstr help = str_new(); + + menu_get_ext_help(menu, &help); + + printf("\n%s\n", str_get(&help)); + str_free(&help); +} + +static void strip(char *str) +{ + char *p = str; + int l; + + while ((isspace(*p))) + p++; + l = strlen(p); + if (p != str) + memmove(str, p, l + 1); + if (!l) + return; + p = str + l - 1; + while ((isspace(*p))) + *p-- = 0; +} + +static void check_stdin(void) +{ + if (!valid_stdin) { + printf(_("aborted!\n\n")); + printf(_("Console input/output is redirected. ")); + printf(_("Run 'make oldconfig' to update configuration.\n\n")); + exit(1); + } +} + +static int conf_askvalue(struct symbol *sym, const char *def) +{ + enum symbol_type type = sym_get_type(sym); + + if (!sym_has_value(sym)) + printf(_("(NEW) ")); + + line[0] = '\n'; + line[1] = 0; + + if (!sym_is_changable(sym)) { + printf("%s\n", def); + line[0] = '\n'; + line[1] = 0; + return 0; + } + + switch (input_mode) { + case oldconfig: + case silentoldconfig: + if (sym_has_value(sym)) { + printf("%s\n", def); + return 0; + } + check_stdin(); + /* fall through */ + case oldaskconfig: + fflush(stdout); + xfgets(line, sizeof(line), stdin); + if (!tty_stdio) + printf("\n"); + return 1; + default: + break; + } + + switch (type) { + case S_INT: + case S_HEX: + case S_STRING: + printf("%s\n", def); + return 1; + default: + ; + } + printf("%s", line); + return 1; +} + +static int conf_string(struct menu *menu) +{ + struct symbol *sym = menu->sym; + const char *def; + + while (1) { + printf("%*s%s ", indent - 1, "", _(menu->prompt->text)); + printf("(%s) ", sym->name); + def = sym_get_string_value(sym); + if (sym_get_string_value(sym)) + printf("[%s] ", def); + if (!conf_askvalue(sym, def)) + return 0; + switch (line[0]) { + case '\n': + break; + case '?': + /* print help */ + if (line[1] == '\n') { + print_help(menu); + def = NULL; + break; + } + /* fall through */ + default: + line[strlen(line)-1] = 0; + def = line; + } + if (def && sym_set_string_value(sym, def)) + return 0; + } +} + +static int conf_sym(struct menu *menu) +{ + struct symbol *sym = menu->sym; + tristate oldval, newval; + + while (1) { + printf("%*s%s ", indent - 1, "", _(menu->prompt->text)); + if (sym->name) + printf("(%s) ", sym->name); + putchar('['); + oldval = sym_get_tristate_value(sym); + switch (oldval) { + case no: + putchar('N'); + break; + case mod: + putchar('M'); + break; + case yes: + putchar('Y'); + break; + } + if (oldval != no && sym_tristate_within_range(sym, no)) + printf("/n"); + if (oldval != mod && sym_tristate_within_range(sym, mod)) + printf("/m"); + if (oldval != yes && sym_tristate_within_range(sym, yes)) + printf("/y"); + if (menu_has_help(menu)) + printf("/?"); + printf("] "); + if (!conf_askvalue(sym, sym_get_string_value(sym))) + return 0; + strip(line); + + switch (line[0]) { + case 'n': + case 'N': + newval = no; + if (!line[1] || !strcmp(&line[1], "o")) + break; + continue; + case 'm': + case 'M': + newval = mod; + if (!line[1]) + break; + continue; + case 'y': + case 'Y': + newval = yes; + if (!line[1] || !strcmp(&line[1], "es")) + break; + continue; + case 0: + newval = oldval; + break; + case '?': + goto help; + default: + continue; + } + if (sym_set_tristate_value(sym, newval)) + return 0; +help: + print_help(menu); + } +} + +static int conf_choice(struct menu *menu) +{ + struct symbol *sym, *def_sym; + struct menu *child; + bool is_new; + + sym = menu->sym; + is_new = !sym_has_value(sym); + if (sym_is_changable(sym)) { + conf_sym(menu); + sym_calc_value(sym); + switch (sym_get_tristate_value(sym)) { + case no: + return 1; + case mod: + return 0; + case yes: + break; + } + } else { + switch (sym_get_tristate_value(sym)) { + case no: + return 1; + case mod: + printf("%*s%s\n", indent - 1, "", _(menu_get_prompt(menu))); + return 0; + case yes: + break; + } + } + + while (1) { + int cnt, def; + + printf("%*s%s\n", indent - 1, "", _(menu_get_prompt(menu))); + def_sym = sym_get_choice_value(sym); + cnt = def = 0; + line[0] = 0; + for (child = menu->list; child; child = child->next) { + if (!menu_is_visible(child)) + continue; + if (!child->sym) { + printf("%*c %s\n", indent, '*', _(menu_get_prompt(child))); + continue; + } + cnt++; + if (child->sym == def_sym) { + def = cnt; + printf("%*c", indent, '>'); + } else + printf("%*c", indent, ' '); + printf(" %d. %s", cnt, _(menu_get_prompt(child))); + if (child->sym->name) + printf(" (%s)", child->sym->name); + if (!sym_has_value(child->sym)) + printf(_(" (NEW)")); + printf("\n"); + } + printf(_("%*schoice"), indent - 1, ""); + if (cnt == 1) { + printf("[1]: 1\n"); + goto conf_childs; + } + printf("[1-%d", cnt); + if (menu_has_help(menu)) + printf("?"); + printf("]: "); + switch (input_mode) { + case oldconfig: + case silentoldconfig: + if (!is_new) { + cnt = def; + printf("%d\n", cnt); + break; + } + check_stdin(); + /* fall through */ + case oldaskconfig: + fflush(stdout); + xfgets(line, sizeof(line), stdin); + strip(line); + if (line[0] == '?') { + print_help(menu); + continue; + } + if (!line[0]) + cnt = def; + else if (isdigit(line[0])) + cnt = atoi(line); + else + continue; + break; + default: + break; + } + + conf_childs: + for (child = menu->list; child; child = child->next) { + if (!child->sym || !menu_is_visible(child)) + continue; + if (!--cnt) + break; + } + if (!child) + continue; + if (line[0] && line[strlen(line) - 1] == '?') { + print_help(child); + continue; + } + sym_set_choice_value(sym, child->sym); + for (child = child->list; child; child = child->next) { + indent += 2; + conf(child); + indent -= 2; + } + return 1; + } +} + +static void conf(struct menu *menu) +{ + struct symbol *sym; + struct property *prop; + struct menu *child; + + if (!menu_is_visible(menu)) + return; + + sym = menu->sym; + prop = menu->prompt; + if (prop) { + const char *prompt; + + switch (prop->type) { + case P_MENU: + if ((input_mode == silentoldconfig || + input_mode == listnewconfig || + input_mode == olddefconfig) && + rootEntry != menu) { + check_conf(menu); + return; + } + /* fall through */ + case P_COMMENT: + prompt = menu_get_prompt(menu); + if (prompt) + printf("%*c\n%*c %s\n%*c\n", + indent, '*', + indent, '*', _(prompt), + indent, '*'); + default: + ; + } + } + + if (!sym) + goto conf_childs; + + if (sym_is_choice(sym)) { + conf_choice(menu); + if (sym->curr.tri != mod) + return; + goto conf_childs; + } + + switch (sym->type) { + case S_INT: + case S_HEX: + case S_STRING: + conf_string(menu); + break; + default: + conf_sym(menu); + break; + } + +conf_childs: + if (sym) + indent += 2; + for (child = menu->list; child; child = child->next) + conf(child); + if (sym) + indent -= 2; +} + +static void check_conf(struct menu *menu) +{ + struct symbol *sym; + struct menu *child; + + if (!menu_is_visible(menu)) + return; + + sym = menu->sym; + if (sym && !sym_has_value(sym)) { + if (sym_is_changable(sym) || + (sym_is_choice(sym) && sym_get_tristate_value(sym) == yes)) { + if (input_mode == listnewconfig) { + if (sym->name && !sym_is_choice_value(sym)) { + printf("%s%s\n", CONFIG_, sym->name); + } + } else if (input_mode != olddefconfig) { + if (!conf_cnt++) + printf(_("*\n* Restart config...\n*\n")); + rootEntry = menu_get_parent_menu(menu); + conf(rootEntry); + } + } + } + + for (child = menu->list; child; child = child->next) + check_conf(child); +} + +static struct option long_opts[] = { + {"oldaskconfig", no_argument, NULL, oldaskconfig}, + {"oldconfig", no_argument, NULL, oldconfig}, + {"silentoldconfig", no_argument, NULL, silentoldconfig}, + {"defconfig", optional_argument, NULL, defconfig}, + {"savedefconfig", required_argument, NULL, savedefconfig}, + {"allnoconfig", no_argument, NULL, allnoconfig}, + {"allyesconfig", no_argument, NULL, allyesconfig}, + {"allmodconfig", no_argument, NULL, allmodconfig}, + {"alldefconfig", no_argument, NULL, alldefconfig}, + {"randconfig", no_argument, NULL, randconfig}, + {"listnewconfig", no_argument, NULL, listnewconfig}, + {"olddefconfig", no_argument, NULL, olddefconfig}, + /* + * oldnoconfig is an alias of olddefconfig, because people already + * are dependent on its behavior(sets new symbols to their default + * value but not 'n') with the counter-intuitive name. + */ + {"oldnoconfig", no_argument, NULL, olddefconfig}, + {NULL, 0, NULL, 0} +}; + +static void conf_usage(const char *progname) +{ + + printf("Usage: %s [-s] [option] \n", progname); + printf("[option] is _one_ of the following:\n"); + printf(" --listnewconfig List new options\n"); + printf(" --oldaskconfig Start a new configuration using a line-oriented program\n"); + printf(" --oldconfig Update a configuration using a provided .config as base\n"); + printf(" --silentoldconfig Same as oldconfig, but quietly, additionally update deps\n"); + printf(" --olddefconfig Same as silentoldconfig but sets new symbols to their default value\n"); + printf(" --oldnoconfig An alias of olddefconfig\n"); + printf(" --defconfig New config with default defined in \n"); + printf(" --savedefconfig Save the minimal current configuration to \n"); + printf(" --allnoconfig New config where all options are answered with no\n"); + printf(" --allyesconfig New config where all options are answered with yes\n"); + printf(" --allmodconfig New config where all options are answered with mod\n"); + printf(" --alldefconfig New config with all symbols set to default\n"); + printf(" --randconfig New config with random answer to all options\n"); +} + +int main(int ac, char **av) +{ + const char *progname = av[0]; + int opt; + const char *name, *defconfig_file = NULL /* gcc uninit */; + struct stat tmpstat; + + setlocale(LC_ALL, ""); + bindtextdomain(PACKAGE, LOCALEDIR); + textdomain(PACKAGE); + + tty_stdio = isatty(0) && isatty(1) && isatty(2); + + while ((opt = getopt_long(ac, av, "s", long_opts, NULL)) != -1) { + if (opt == 's') { + conf_set_message_callback(NULL); + continue; + } + input_mode = (enum input_mode)opt; + switch (opt) { + case silentoldconfig: + sync_kconfig = 1; + break; + case defconfig: + case savedefconfig: + defconfig_file = optarg; + break; + case randconfig: + { + struct timeval now; + unsigned int seed; + char *seed_env; + + /* + * Use microseconds derived seed, + * compensate for systems where it may be zero + */ + gettimeofday(&now, NULL); + seed = (unsigned int)((now.tv_sec + 1) * (now.tv_usec + 1)); + + seed_env = getenv("KCONFIG_SEED"); + if( seed_env && *seed_env ) { + char *endp; + int tmp = (int)strtol(seed_env, &endp, 0); + if (*endp == '\0') { + seed = tmp; + } + } + fprintf( stderr, "KCONFIG_SEED=0x%X\n", seed ); + srand(seed); + break; + } + case oldaskconfig: + case oldconfig: + case allnoconfig: + case allyesconfig: + case allmodconfig: + case alldefconfig: + case listnewconfig: + case olddefconfig: + break; + case '?': + conf_usage(progname); + exit(1); + break; + } + } + if (ac == optind) { + printf(_("%s: Kconfig file missing\n"), av[0]); + conf_usage(progname); + exit(1); + } + name = av[optind]; + conf_parse(name); + //zconfdump(stdout); + if (sync_kconfig) { + name = conf_get_configname(); + if (stat(name, &tmpstat)) { + fprintf(stderr, _("***\n" + "*** Configuration file \"%s\" not found!\n" + "***\n" + "*** Please run some configurator (e.g. \"make oldconfig\" or\n" + "*** \"make menuconfig\" or \"make xconfig\").\n" + "***\n"), name); + exit(1); + } + } + + switch (input_mode) { + case defconfig: + if (!defconfig_file) + defconfig_file = conf_get_default_confname(); + if (conf_read(defconfig_file)) { + printf(_("***\n" + "*** Can't find default configuration \"%s\"!\n" + "***\n"), defconfig_file); + exit(1); + } + break; + case savedefconfig: + case silentoldconfig: + case oldaskconfig: + case oldconfig: + case listnewconfig: + case olddefconfig: + conf_read(NULL); + break; + case allnoconfig: + case allyesconfig: + case allmodconfig: + case alldefconfig: + case randconfig: + name = getenv("KCONFIG_ALLCONFIG"); + if (!name) + break; + if ((strcmp(name, "") != 0) && (strcmp(name, "1") != 0)) { + if (conf_read_simple(name, S_DEF_USER)) { + fprintf(stderr, + _("*** Can't read seed configuration \"%s\"!\n"), + name); + exit(1); + } + break; + } + switch (input_mode) { + case allnoconfig: name = "allno.config"; break; + case allyesconfig: name = "allyes.config"; break; + case allmodconfig: name = "allmod.config"; break; + case alldefconfig: name = "alldef.config"; break; + case randconfig: name = "allrandom.config"; break; + default: break; + } + if (conf_read_simple(name, S_DEF_USER) && + conf_read_simple("all.config", S_DEF_USER)) { + fprintf(stderr, + _("*** KCONFIG_ALLCONFIG set, but no \"%s\" or \"all.config\" file found\n"), + name); + exit(1); + } + break; + default: + break; + } + + if (sync_kconfig) { + if (conf_get_changed()) { + name = getenv("KCONFIG_NOSILENTUPDATE"); + if (name && *name) { + fprintf(stderr, + _("\n*** The configuration requires explicit update.\n\n")); + return 1; + } + } + valid_stdin = tty_stdio; + } + + switch (input_mode) { + case allnoconfig: + conf_set_all_new_symbols(def_no); + break; + case allyesconfig: + conf_set_all_new_symbols(def_yes); + break; + case allmodconfig: + conf_set_all_new_symbols(def_mod); + break; + case alldefconfig: + conf_set_all_new_symbols(def_default); + break; + case randconfig: + /* Really nothing to do in this loop */ + while (conf_set_all_new_symbols(def_random)) ; + break; + case defconfig: + conf_set_all_new_symbols(def_default); + break; + case savedefconfig: + break; + case oldaskconfig: + rootEntry = &rootmenu; + conf(&rootmenu); + input_mode = silentoldconfig; + /* fall through */ + case oldconfig: + case listnewconfig: + case olddefconfig: + case silentoldconfig: + /* Update until a loop caused no more changes */ + do { + conf_cnt = 0; + check_conf(&rootmenu); + } while (conf_cnt && + (input_mode != listnewconfig && + input_mode != olddefconfig)); + break; + } + + if (sync_kconfig) { + /* silentoldconfig is used during the build so we shall update autoconf. + * All other commands are only used to generate a config. + */ + if (conf_get_changed() && conf_write(NULL)) { + fprintf(stderr, _("\n*** Error during writing of the configuration.\n\n")); + exit(1); + } + if (conf_write_autoconf()) { + fprintf(stderr, _("\n*** Error during update of the configuration.\n\n")); + return 1; + } + } else if (input_mode == savedefconfig) { + if (conf_write_defconfig(defconfig_file)) { + fprintf(stderr, _("n*** Error while saving defconfig to: %s\n\n"), + defconfig_file); + return 1; + } + } else if (input_mode != listnewconfig) { + if (conf_write(NULL)) { + fprintf(stderr, _("\n*** Error during writing of the configuration.\n\n")); + exit(1); + } + } + return 0; +} + +/* + * Helper function to facilitate fgets() by Jean Sacren. + */ +void xfgets(char *str, int size, FILE *in) +{ + if (fgets(str, size, in) == NULL) + fprintf(stderr, "\nError in reading or end of file.\n"); +} diff --git a/scripts/kconfig/confdata.c b/scripts/kconfig/confdata.c new file mode 100644 index 0000000000..86f746266a --- /dev/null +++ b/scripts/kconfig/confdata.c @@ -0,0 +1,1249 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lkc.h" + +struct conf_printer { + void (*print_symbol)(FILE *, struct symbol *, const char *, void *); + void (*print_comment)(FILE *, const char *, void *); +}; + +static void conf_warning(const char *fmt, ...) + __attribute__ ((format (printf, 1, 2))); + +static void conf_message(const char *fmt, ...) + __attribute__ ((format (printf, 1, 2))); + +static const char *conf_filename; +static int conf_lineno, conf_warnings, conf_unsaved; + +const char conf_defname[] = "configs/defconfig"; + +static void conf_warning(const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + fprintf(stderr, "%s:%d:warning: ", conf_filename, conf_lineno); + vfprintf(stderr, fmt, ap); + fprintf(stderr, "\n"); + va_end(ap); + conf_warnings++; +} + +static void conf_default_message_callback(const char *fmt, va_list ap) +{ + printf("#\n# "); + vprintf(fmt, ap); + printf("\n#\n"); +} + +static void (*conf_message_callback) (const char *fmt, va_list ap) = + conf_default_message_callback; +void conf_set_message_callback(void (*fn) (const char *fmt, va_list ap)) +{ + conf_message_callback = fn; +} + +static void conf_message(const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + if (conf_message_callback) + conf_message_callback(fmt, ap); + va_end(ap); +} + +const char *conf_get_configname(void) +{ + char *name = getenv("KCONFIG_CONFIG"); + + return name ? name : ".config"; +} + +const char *conf_get_autoconfig_name(void) +{ + char *name = getenv("KCONFIG_AUTOCONFIG"); + + return name ? name : "include/config/auto.conf"; +} + +static char *conf_expand_value(const char *in) +{ + struct symbol *sym; + const char *src; + static char res_value[SYMBOL_MAXLENGTH]; + char *dst, name[SYMBOL_MAXLENGTH]; + + res_value[0] = 0; + dst = name; + while ((src = strchr(in, '$'))) { + strncat(res_value, in, src - in); + src++; + dst = name; + while (isalnum(*src) || *src == '_') + *dst++ = *src++; + *dst = 0; + sym = sym_lookup(name, 0); + sym_calc_value(sym); + strcat(res_value, sym_get_string_value(sym)); + in = src; + } + strcat(res_value, in); + + return res_value; +} + +char *conf_get_default_confname(void) +{ + struct stat buf; + static char fullname[PATH_MAX+1]; + char *env, *name; + + name = conf_expand_value(conf_defname); + env = getenv(SRCTREE); + if (env) { + sprintf(fullname, "%s/%s", env, name); + if (!stat(fullname, &buf)) + return fullname; + } + return name; +} + +static int conf_set_sym_val(struct symbol *sym, int def, int def_flags, char *p) +{ + char *p2; + + switch (sym->type) { + case S_TRISTATE: + if (p[0] == 'm') { + sym->def[def].tri = mod; + sym->flags |= def_flags; + break; + } + /* fall through */ + case S_BOOLEAN: + if (p[0] == 'y') { + sym->def[def].tri = yes; + sym->flags |= def_flags; + break; + } + if (p[0] == 'n') { + sym->def[def].tri = no; + sym->flags |= def_flags; + break; + } + if (def != S_DEF_AUTO) + conf_warning("symbol value '%s' invalid for %s", + p, sym->name); + return 1; + case S_OTHER: + if (*p != '"') { + for (p2 = p; *p2 && !isspace(*p2); p2++) + ; + sym->type = S_STRING; + goto done; + } + /* fall through */ + case S_STRING: + if (*p++ != '"') + break; + for (p2 = p; (p2 = strpbrk(p2, "\"\\")); p2++) { + if (*p2 == '"') { + *p2 = 0; + break; + } + memmove(p2, p2 + 1, strlen(p2)); + } + if (!p2) { + if (def != S_DEF_AUTO) + conf_warning("invalid string found"); + return 1; + } + /* fall through */ + case S_INT: + case S_HEX: + done: + if (sym_string_valid(sym, p)) { + sym->def[def].val = strdup(p); + sym->flags |= def_flags; + } else { + if (def != S_DEF_AUTO) + conf_warning("symbol value '%s' invalid for %s", + p, sym->name); + return 1; + } + break; + default: + ; + } + return 0; +} + +#define LINE_GROWTH 16 +static int add_byte(int c, char **lineptr, size_t slen, size_t *n) +{ + char *nline; + size_t new_size = slen + 1; + if (new_size > *n) { + new_size += LINE_GROWTH - 1; + new_size *= 2; + nline = realloc(*lineptr, new_size); + if (!nline) + return -1; + + *lineptr = nline; + *n = new_size; + } + + (*lineptr)[slen] = c; + + return 0; +} + +static ssize_t compat_getline(char **lineptr, size_t *n, FILE *stream) +{ + char *line = *lineptr; + size_t slen = 0; + + for (;;) { + int c = getc(stream); + + switch (c) { + case '\n': + if (add_byte(c, &line, slen, n) < 0) + goto e_out; + slen++; + /* fall through */ + case EOF: + if (add_byte('\0', &line, slen, n) < 0) + goto e_out; + *lineptr = line; + if (slen == 0) + return -1; + return slen; + default: + if (add_byte(c, &line, slen, n) < 0) + goto e_out; + slen++; + } + } + +e_out: + line[slen-1] = '\0'; + *lineptr = line; + return -1; +} + +int conf_read_simple(const char *name, int def) +{ + FILE *in = NULL; + char *line = NULL; + size_t line_asize = 0; + char *p, *p2; + struct symbol *sym; + int i, def_flags; + + if (name) { + in = zconf_fopen(name); + } else { + struct property *prop; + + name = conf_get_configname(); + in = zconf_fopen(name); + if (in) + goto load; + sym_add_change_count(1); + if (!sym_defconfig_list) + return 1; + + for_all_defaults(sym_defconfig_list, prop) { + if (expr_calc_value(prop->visible.expr) == no || + prop->expr->type != E_SYMBOL) + continue; + name = conf_expand_value(prop->expr->left.sym->name); + in = zconf_fopen(name); + if (in) { + conf_message(_("using defaults found in %s"), + name); + goto load; + } + } + } + if (!in) + return 1; + +load: + conf_filename = name; + conf_lineno = 0; + conf_warnings = 0; + conf_unsaved = 0; + + def_flags = SYMBOL_DEF << def; + for_all_symbols(i, sym) { + sym->flags |= SYMBOL_CHANGED; + sym->flags &= ~(def_flags|SYMBOL_VALID); + if (sym_is_choice(sym)) + sym->flags |= def_flags; + switch (sym->type) { + case S_INT: + case S_HEX: + case S_STRING: + if (sym->def[def].val) + free(sym->def[def].val); + /* fall through */ + default: + sym->def[def].val = NULL; + sym->def[def].tri = no; + } + } + + while (compat_getline(&line, &line_asize, in) != -1) { + conf_lineno++; + sym = NULL; + if (line[0] == '#') { + if (memcmp(line + 2, CONFIG_, strlen(CONFIG_))) + continue; + p = strchr(line + 2 + strlen(CONFIG_), ' '); + if (!p) + continue; + *p++ = 0; + if (strncmp(p, "is not set", 10)) + continue; + if (def == S_DEF_USER) { + sym = sym_find(line + 2 + strlen(CONFIG_)); + if (!sym) { + sym_add_change_count(1); + goto setsym; + } + } else { + sym = sym_lookup(line + 2 + strlen(CONFIG_), 0); + if (sym->type == S_UNKNOWN) + sym->type = S_BOOLEAN; + } + if (sym->flags & def_flags) { + conf_warning("override: reassigning to symbol %s", sym->name); + } + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + sym->def[def].tri = no; + sym->flags |= def_flags; + break; + default: + ; + } + } else if (memcmp(line, CONFIG_, strlen(CONFIG_)) == 0) { + p = strchr(line + strlen(CONFIG_), '='); + if (!p) + continue; + *p++ = 0; + p2 = strchr(p, '\n'); + if (p2) { + *p2-- = 0; + if (*p2 == '\r') + *p2 = 0; + } + if (def == S_DEF_USER) { + sym = sym_find(line + strlen(CONFIG_)); + if (!sym) { + sym_add_change_count(1); + goto setsym; + } + } else { + sym = sym_lookup(line + strlen(CONFIG_), 0); + if (sym->type == S_UNKNOWN) + sym->type = S_OTHER; + } + if (sym->flags & def_flags) { + conf_warning("override: reassigning to symbol %s", sym->name); + } + if (conf_set_sym_val(sym, def, def_flags, p)) + continue; + } else { + if (line[0] != '\r' && line[0] != '\n') + conf_warning("unexpected data: %.*s", + (int)strcspn(line, "\r\n"), line); + + continue; + } +setsym: + if (sym && sym_is_choice_value(sym)) { + struct symbol *cs = prop_get_symbol(sym_get_choice_prop(sym)); + switch (sym->def[def].tri) { + case no: + break; + case mod: + if (cs->def[def].tri == yes) { + conf_warning("%s creates inconsistent choice state", sym->name); + cs->flags &= ~def_flags; + } + break; + case yes: + if (cs->def[def].tri != no) + conf_warning("override: %s changes choice state", sym->name); + cs->def[def].val = sym; + break; + } + cs->def[def].tri = EXPR_OR(cs->def[def].tri, sym->def[def].tri); + } + } + free(line); + fclose(in); + return 0; +} + +int conf_read(const char *name) +{ + struct symbol *sym; + int i; + + sym_set_change_count(0); + + if (conf_read_simple(name, S_DEF_USER)) { + sym_calc_value(modules_sym); + return 1; + } + + sym_calc_value(modules_sym); + + for_all_symbols(i, sym) { + sym_calc_value(sym); + if (sym_is_choice(sym) || (sym->flags & SYMBOL_AUTO)) + continue; + if (sym_has_value(sym) && (sym->flags & SYMBOL_WRITE)) { + /* check that calculated value agrees with saved value */ + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + if (sym->def[S_DEF_USER].tri != sym_get_tristate_value(sym)) + break; + if (!sym_is_choice(sym)) + continue; + /* fall through */ + default: + if (!strcmp(sym->curr.val, sym->def[S_DEF_USER].val)) + continue; + break; + } + } else if (!sym_has_value(sym) && !(sym->flags & SYMBOL_WRITE)) + /* no previous value and not saved */ + continue; + conf_unsaved++; + /* maybe print value in verbose mode... */ + } + + for_all_symbols(i, sym) { + if (sym_has_value(sym) && !sym_is_choice_value(sym)) { + /* Reset values of generates values, so they'll appear + * as new, if they should become visible, but that + * doesn't quite work if the Kconfig and the saved + * configuration disagree. + */ + if (sym->visible == no && !conf_unsaved) + sym->flags &= ~SYMBOL_DEF_USER; + switch (sym->type) { + case S_STRING: + case S_INT: + case S_HEX: + /* Reset a string value if it's out of range */ + if (sym_string_within_range(sym, sym->def[S_DEF_USER].val)) + break; + sym->flags &= ~(SYMBOL_VALID|SYMBOL_DEF_USER); + conf_unsaved++; + break; + default: + break; + } + } + } + + sym_add_change_count(conf_warnings || conf_unsaved); + + return 0; +} + +/* + * Kconfig configuration printer + * + * This printer is used when generating the resulting configuration after + * kconfig invocation and `defconfig' files. Unset symbol might be omitted by + * passing a non-NULL argument to the printer. + * + */ +static void +kconfig_print_symbol(FILE *fp, struct symbol *sym, const char *value, void *arg) +{ + + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + if (*value == 'n') { + bool skip_unset = (arg != NULL); + + if (!skip_unset) + fprintf(fp, "# %s%s is not set\n", + CONFIG_, sym->name); + return; + } + break; + default: + break; + } + + fprintf(fp, "%s%s=%s\n", CONFIG_, sym->name, value); +} + +static void +kconfig_print_comment(FILE *fp, const char *value, void *arg) +{ + const char *p = value; + size_t l; + + for (;;) { + l = strcspn(p, "\n"); + fprintf(fp, "#"); + if (l) { + fprintf(fp, " "); + xfwrite(p, l, 1, fp); + p += l; + } + fprintf(fp, "\n"); + if (*p++ == '\0') + break; + } +} + +static struct conf_printer kconfig_printer_cb = +{ + .print_symbol = kconfig_print_symbol, + .print_comment = kconfig_print_comment, +}; + +/* + * Header printer + * + * This printer is used when generating the `include/generated/autoconf.h' file. + */ +static void +header_print_symbol(FILE *fp, struct symbol *sym, const char *value, void *arg) +{ + + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: { + const char *suffix = ""; + + switch (*value) { + case 'n': + break; + case 'm': + suffix = "_MODULE"; + /* fall through */ + default: + fprintf(fp, "#define %s%s%s 1\n", + CONFIG_, sym->name, suffix); + } + break; + } + case S_HEX: { + const char *prefix = ""; + + if (value[0] != '0' || (value[1] != 'x' && value[1] != 'X')) + prefix = "0x"; + fprintf(fp, "#define %s%s %s%s\n", + CONFIG_, sym->name, prefix, value); + break; + } + case S_STRING: + case S_INT: + fprintf(fp, "#define %s%s %s\n", + CONFIG_, sym->name, value); + break; + default: + break; + } + +} + +static void +header_print_comment(FILE *fp, const char *value, void *arg) +{ + const char *p = value; + size_t l; + + fprintf(fp, "/*\n"); + for (;;) { + l = strcspn(p, "\n"); + fprintf(fp, " *"); + if (l) { + fprintf(fp, " "); + xfwrite(p, l, 1, fp); + p += l; + } + fprintf(fp, "\n"); + if (*p++ == '\0') + break; + } + fprintf(fp, " */\n"); +} + +static struct conf_printer header_printer_cb = +{ + .print_symbol = header_print_symbol, + .print_comment = header_print_comment, +}; + +/* + * Tristate printer + * + * This printer is used when generating the `include/config/tristate.conf' file. + */ +static void +tristate_print_symbol(FILE *fp, struct symbol *sym, const char *value, void *arg) +{ + + if (sym->type == S_TRISTATE && *value != 'n') + fprintf(fp, "%s%s=%c\n", CONFIG_, sym->name, (char)toupper(*value)); +} + +static struct conf_printer tristate_printer_cb = +{ + .print_symbol = tristate_print_symbol, + .print_comment = kconfig_print_comment, +}; + +static void conf_write_symbol(FILE *fp, struct symbol *sym, + struct conf_printer *printer, void *printer_arg) +{ + const char *str; + + switch (sym->type) { + case S_OTHER: + case S_UNKNOWN: + break; + case S_STRING: + str = sym_get_string_value(sym); + str = sym_escape_string_value(str); + printer->print_symbol(fp, sym, str, printer_arg); + free((void *)str); + break; + default: + str = sym_get_string_value(sym); + printer->print_symbol(fp, sym, str, printer_arg); + } +} + +static void +conf_write_heading(FILE *fp, struct conf_printer *printer, void *printer_arg) +{ + char buf[256]; + + snprintf(buf, sizeof(buf), + "\n" + "Automatically generated file; DO NOT EDIT.\n" + "%s\n", + rootmenu.prompt->text); + + printer->print_comment(fp, buf, printer_arg); +} + +/* + * Write out a minimal config. + * All values that has default values are skipped as this is redundant. + */ +int conf_write_defconfig(const char *filename) +{ + struct symbol *sym; + struct menu *menu; + FILE *out; + + out = fopen(filename, "w"); + if (!out) + return 1; + + sym_clear_all_valid(); + + /* Traverse all menus to find all relevant symbols */ + menu = rootmenu.list; + + while (menu != NULL) + { + sym = menu->sym; + if (sym == NULL) { + if (!menu_is_visible(menu)) + goto next_menu; + } else if (!sym_is_choice(sym)) { + sym_calc_value(sym); + if (!(sym->flags & SYMBOL_WRITE)) + goto next_menu; + sym->flags &= ~SYMBOL_WRITE; + /* If we cannot change the symbol - skip */ + if (!sym_is_changable(sym)) + goto next_menu; + /* If symbol equals to default value - skip */ + if (strcmp(sym_get_string_value(sym), sym_get_string_default(sym)) == 0) + goto next_menu; + + /* + * If symbol is a choice value and equals to the + * default for a choice - skip. + * But only if value is bool and equal to "y" and + * choice is not "optional". + * (If choice is "optional" then all values can be "n") + */ + if (sym_is_choice_value(sym)) { + struct symbol *cs; + struct symbol *ds; + + cs = prop_get_symbol(sym_get_choice_prop(sym)); + ds = sym_choice_default(cs); + if (!sym_is_optional(cs) && sym == ds) { + if ((sym->type == S_BOOLEAN) && + sym_get_tristate_value(sym) == yes) + goto next_menu; + } + } + conf_write_symbol(out, sym, &kconfig_printer_cb, NULL); + } +next_menu: + if (menu->list != NULL) { + menu = menu->list; + } + else if (menu->next != NULL) { + menu = menu->next; + } else { + while ((menu = menu->parent)) { + if (menu->next != NULL) { + menu = menu->next; + break; + } + } + } + } + fclose(out); + return 0; +} + +int conf_write(const char *name) +{ + FILE *out; + struct symbol *sym; + struct menu *menu; + const char *basename; + const char *str; + char dirname[PATH_MAX+1], tmpname[PATH_MAX+1], newname[PATH_MAX+1]; + char *env; + + dirname[0] = 0; + if (name && name[0]) { + struct stat st; + char *slash; + + if (!stat(name, &st) && S_ISDIR(st.st_mode)) { + strcpy(dirname, name); + strcat(dirname, "/"); + basename = conf_get_configname(); + } else if ((slash = strrchr(name, '/'))) { + int size = slash - name + 1; + memcpy(dirname, name, size); + dirname[size] = 0; + if (slash[1]) + basename = slash + 1; + else + basename = conf_get_configname(); + } else + basename = name; + } else + basename = conf_get_configname(); + + sprintf(newname, "%s%s", dirname, basename); + env = getenv("KCONFIG_OVERWRITECONFIG"); + if (!env || !*env) { + sprintf(tmpname, "%s.tmpconfig.%d", dirname, (int)getpid()); + out = fopen(tmpname, "w"); + } else { + *tmpname = 0; + out = fopen(newname, "w"); + } + if (!out) + return 1; + + conf_write_heading(out, &kconfig_printer_cb, NULL); + + if (!conf_get_changed()) + sym_clear_all_valid(); + + menu = rootmenu.list; + while (menu) { + sym = menu->sym; + if (!sym) { + if (!menu_is_visible(menu)) + goto next; + str = menu_get_prompt(menu); + fprintf(out, "\n" + "#\n" + "# %s\n" + "#\n", str); + } else if (!(sym->flags & SYMBOL_CHOICE)) { + sym_calc_value(sym); + if (!(sym->flags & SYMBOL_WRITE)) + goto next; + sym->flags &= ~SYMBOL_WRITE; + + conf_write_symbol(out, sym, &kconfig_printer_cb, NULL); + } + +next: + if (menu->list) { + menu = menu->list; + continue; + } + if (menu->next) + menu = menu->next; + else while ((menu = menu->parent)) { + if (menu->next) { + menu = menu->next; + break; + } + } + } + fclose(out); + + if (*tmpname) { + strcat(dirname, basename); + strcat(dirname, ".old"); + rename(newname, dirname); + if (rename(tmpname, newname)) + return 1; + } + + conf_message(_("configuration written to %s"), newname); + + sym_set_change_count(0); + + return 0; +} + +static int conf_split_config(void) +{ + const char *name; + char path[PATH_MAX+1]; + char *s, *d, c; + struct symbol *sym; + struct stat sb; + int res, i, fd; + + name = conf_get_autoconfig_name(); + conf_read_simple(name, S_DEF_AUTO); + sym_calc_value(modules_sym); + + if (chdir("include/config")) + return 1; + + res = 0; + for_all_symbols(i, sym) { + sym_calc_value(sym); + if ((sym->flags & SYMBOL_AUTO) || !sym->name) + continue; + if (sym->flags & SYMBOL_WRITE) { + if (sym->flags & SYMBOL_DEF_AUTO) { + /* + * symbol has old and new value, + * so compare them... + */ + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + if (sym_get_tristate_value(sym) == + sym->def[S_DEF_AUTO].tri) + continue; + break; + case S_STRING: + case S_HEX: + case S_INT: + if (!strcmp(sym_get_string_value(sym), + sym->def[S_DEF_AUTO].val)) + continue; + break; + default: + break; + } + } else { + /* + * If there is no old value, only 'no' (unset) + * is allowed as new value. + */ + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + if (sym_get_tristate_value(sym) == no) + continue; + break; + default: + break; + } + } + } else if (!(sym->flags & SYMBOL_DEF_AUTO)) + /* There is neither an old nor a new value. */ + continue; + /* else + * There is an old value, but no new value ('no' (unset) + * isn't saved in auto.conf, so the old value is always + * different from 'no'). + */ + + /* Replace all '_' and append ".h" */ + s = sym->name; + d = path; + while ((c = *s++)) { + c = tolower(c); + *d++ = (c == '_') ? '/' : c; + } + strcpy(d, ".h"); + + /* Assume directory path already exists. */ + fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0644); + if (fd == -1) { + if (errno != ENOENT) { + res = 1; + break; + } + /* + * Create directory components, + * unless they exist already. + */ + d = path; + while ((d = strchr(d, '/'))) { + *d = 0; + if (stat(path, &sb) && mkdir(path, 0755)) { + res = 1; + goto out; + } + *d++ = '/'; + } + /* Try it again. */ + fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0644); + if (fd == -1) { + res = 1; + break; + } + } + close(fd); + } +out: + if (chdir("../..")) + return 1; + + return res; +} + +int conf_write_autoconf(void) +{ + struct symbol *sym; + const char *name; + FILE *out, *tristate, *out_h; + int i; + + sym_clear_all_valid(); + + file_write_dep("include/config/auto.conf.cmd"); + + if (conf_split_config()) + return 1; + + out = fopen(".tmpconfig", "w"); + if (!out) + return 1; + + tristate = fopen(".tmpconfig_tristate", "w"); + if (!tristate) { + fclose(out); + return 1; + } + + out_h = fopen(".tmpconfig.h", "w"); + if (!out_h) { + fclose(out); + fclose(tristate); + return 1; + } + + conf_write_heading(out, &kconfig_printer_cb, NULL); + + conf_write_heading(tristate, &tristate_printer_cb, NULL); + + conf_write_heading(out_h, &header_printer_cb, NULL); + + for_all_symbols(i, sym) { + sym_calc_value(sym); + if (!(sym->flags & SYMBOL_WRITE) || !sym->name) + continue; + + /* write symbol to auto.conf, tristate and header files */ + conf_write_symbol(out, sym, &kconfig_printer_cb, (void *)1); + + conf_write_symbol(tristate, sym, &tristate_printer_cb, (void *)1); + + conf_write_symbol(out_h, sym, &header_printer_cb, NULL); + } + fclose(out); + fclose(tristate); + fclose(out_h); + + name = getenv("KCONFIG_AUTOHEADER"); + if (!name) + name = "include/generated/autoconf.h"; + if (rename(".tmpconfig.h", name)) + return 1; + name = getenv("KCONFIG_TRISTATE"); + if (!name) + name = "include/config/tristate.conf"; + if (rename(".tmpconfig_tristate", name)) + return 1; + name = conf_get_autoconfig_name(); + /* + * This must be the last step, kbuild has a dependency on auto.conf + * and this marks the successful completion of the previous steps. + */ + if (rename(".tmpconfig", name)) + return 1; + + return 0; +} + +static int sym_change_count; +static void (*conf_changed_callback)(void); + +void sym_set_change_count(int count) +{ + int _sym_change_count = sym_change_count; + sym_change_count = count; + if (conf_changed_callback && + (bool)_sym_change_count != (bool)count) + conf_changed_callback(); +} + +void sym_add_change_count(int count) +{ + sym_set_change_count(count + sym_change_count); +} + +bool conf_get_changed(void) +{ + return sym_change_count; +} + +void conf_set_changed_callback(void (*fn)(void)) +{ + conf_changed_callback = fn; +} + +static bool randomize_choice_values(struct symbol *csym) +{ + struct property *prop; + struct symbol *sym; + struct expr *e; + int cnt, def; + + /* + * If choice is mod then we may have more items selected + * and if no then no-one. + * In both cases stop. + */ + if (csym->curr.tri != yes) + return false; + + prop = sym_get_choice_prop(csym); + + /* count entries in choice block */ + cnt = 0; + expr_list_for_each_sym(prop->expr, e, sym) + cnt++; + + /* + * find a random value and set it to yes, + * set the rest to no so we have only one set + */ + def = (rand() % cnt); + + cnt = 0; + expr_list_for_each_sym(prop->expr, e, sym) { + if (def == cnt++) { + sym->def[S_DEF_USER].tri = yes; + csym->def[S_DEF_USER].val = sym; + } + else { + sym->def[S_DEF_USER].tri = no; + } + sym->flags |= SYMBOL_DEF_USER; + /* clear VALID to get value calculated */ + sym->flags &= ~SYMBOL_VALID; + } + csym->flags |= SYMBOL_DEF_USER; + /* clear VALID to get value calculated */ + csym->flags &= ~(SYMBOL_VALID); + + return true; +} + +void set_all_choice_values(struct symbol *csym) +{ + struct property *prop; + struct symbol *sym; + struct expr *e; + + prop = sym_get_choice_prop(csym); + + /* + * Set all non-assinged choice values to no + */ + expr_list_for_each_sym(prop->expr, e, sym) { + if (!sym_has_value(sym)) + sym->def[S_DEF_USER].tri = no; + } + csym->flags |= SYMBOL_DEF_USER; + /* clear VALID to get value calculated */ + csym->flags &= ~(SYMBOL_VALID | SYMBOL_NEED_SET_CHOICE_VALUES); +} + +bool conf_set_all_new_symbols(enum conf_def_mode mode) +{ + struct symbol *sym, *csym; + int i, cnt, pby, pty, ptm; /* pby: probability of boolean = y + * pty: probability of tristate = y + * ptm: probability of tristate = m + */ + + pby = 50; pty = ptm = 33; /* can't go as the default in switch-case + * below, otherwise gcc whines about + * -Wmaybe-uninitialized */ + if (mode == def_random) { + int n, p[3]; + char *env = getenv("KCONFIG_PROBABILITY"); + n = 0; + while( env && *env ) { + char *endp; + int tmp = strtol( env, &endp, 10 ); + if( tmp >= 0 && tmp <= 100 ) { + p[n++] = tmp; + } else { + errno = ERANGE; + perror( "KCONFIG_PROBABILITY" ); + exit( 1 ); + } + env = (*endp == ':') ? endp+1 : endp; + if( n >=3 ) { + break; + } + } + switch( n ) { + case 1: + pby = p[0]; ptm = pby/2; pty = pby-ptm; + break; + case 2: + pty = p[0]; ptm = p[1]; pby = pty + ptm; + break; + case 3: + pby = p[0]; pty = p[1]; ptm = p[2]; + break; + } + + if( pty+ptm > 100 ) { + errno = ERANGE; + perror( "KCONFIG_PROBABILITY" ); + exit( 1 ); + } + } + bool has_changed = false; + + for_all_symbols(i, sym) { + if (sym_has_value(sym) || (sym->flags & SYMBOL_VALID)) + continue; + switch (sym_get_type(sym)) { + case S_BOOLEAN: + case S_TRISTATE: + has_changed = true; + switch (mode) { + case def_yes: + sym->def[S_DEF_USER].tri = yes; + break; + case def_mod: + sym->def[S_DEF_USER].tri = mod; + break; + case def_no: + if (sym->flags & SYMBOL_ALLNOCONFIG_Y) + sym->def[S_DEF_USER].tri = yes; + else + sym->def[S_DEF_USER].tri = no; + break; + case def_random: + sym->def[S_DEF_USER].tri = no; + cnt = rand() % 100; + if (sym->type == S_TRISTATE) { + if (cnt < pty) + sym->def[S_DEF_USER].tri = yes; + else if (cnt < (pty+ptm)) + sym->def[S_DEF_USER].tri = mod; + } else if (cnt < pby) + sym->def[S_DEF_USER].tri = yes; + break; + default: + continue; + } + if (!(sym_is_choice(sym) && mode == def_random)) + sym->flags |= SYMBOL_DEF_USER; + break; + default: + break; + } + + } + + sym_clear_all_valid(); + + /* + * We have different type of choice blocks. + * If curr.tri equals to mod then we can select several + * choice symbols in one block. + * In this case we do nothing. + * If curr.tri equals yes then only one symbol can be + * selected in a choice block and we set it to yes, + * and the rest to no. + */ + if (mode != def_random) { + for_all_symbols(i, csym) { + if ((sym_is_choice(csym) && !sym_has_value(csym)) || + sym_is_choice_value(csym)) + csym->flags |= SYMBOL_NEED_SET_CHOICE_VALUES; + } + } + + for_all_symbols(i, csym) { + if (sym_has_value(csym) || !sym_is_choice(csym)) + continue; + + sym_calc_value(csym); + if (mode == def_random) + has_changed = randomize_choice_values(csym); + else { + set_all_choice_values(csym); + has_changed = true; + } + } + + return has_changed; +} diff --git a/scripts/kconfig/expr.c b/scripts/kconfig/expr.c new file mode 100644 index 0000000000..cbf4996dd9 --- /dev/null +++ b/scripts/kconfig/expr.c @@ -0,0 +1,1206 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include + +#include "lkc.h" + +#define DEBUG_EXPR 0 + +static int expr_eq(struct expr *e1, struct expr *e2); +static struct expr *expr_eliminate_yn(struct expr *e); + +struct expr *expr_alloc_symbol(struct symbol *sym) +{ + struct expr *e = xcalloc(1, sizeof(*e)); + e->type = E_SYMBOL; + e->left.sym = sym; + return e; +} + +struct expr *expr_alloc_one(enum expr_type type, struct expr *ce) +{ + struct expr *e = xcalloc(1, sizeof(*e)); + e->type = type; + e->left.expr = ce; + return e; +} + +struct expr *expr_alloc_two(enum expr_type type, struct expr *e1, struct expr *e2) +{ + struct expr *e = xcalloc(1, sizeof(*e)); + e->type = type; + e->left.expr = e1; + e->right.expr = e2; + return e; +} + +struct expr *expr_alloc_comp(enum expr_type type, struct symbol *s1, struct symbol *s2) +{ + struct expr *e = xcalloc(1, sizeof(*e)); + e->type = type; + e->left.sym = s1; + e->right.sym = s2; + return e; +} + +struct expr *expr_alloc_and(struct expr *e1, struct expr *e2) +{ + if (!e1) + return e2; + return e2 ? expr_alloc_two(E_AND, e1, e2) : e1; +} + +struct expr *expr_alloc_or(struct expr *e1, struct expr *e2) +{ + if (!e1) + return e2; + return e2 ? expr_alloc_two(E_OR, e1, e2) : e1; +} + +struct expr *expr_copy(const struct expr *org) +{ + struct expr *e; + + if (!org) + return NULL; + + e = xmalloc(sizeof(*org)); + memcpy(e, org, sizeof(*org)); + switch (org->type) { + case E_SYMBOL: + e->left = org->left; + break; + case E_NOT: + e->left.expr = expr_copy(org->left.expr); + break; + case E_EQUAL: + case E_GEQ: + case E_GTH: + case E_LEQ: + case E_LTH: + case E_UNEQUAL: + e->left.sym = org->left.sym; + e->right.sym = org->right.sym; + break; + case E_AND: + case E_OR: + case E_LIST: + e->left.expr = expr_copy(org->left.expr); + e->right.expr = expr_copy(org->right.expr); + break; + default: + printf("can't copy type %d\n", e->type); + free(e); + e = NULL; + break; + } + + return e; +} + +void expr_free(struct expr *e) +{ + if (!e) + return; + + switch (e->type) { + case E_SYMBOL: + break; + case E_NOT: + expr_free(e->left.expr); + return; + case E_EQUAL: + case E_GEQ: + case E_GTH: + case E_LEQ: + case E_LTH: + case E_UNEQUAL: + break; + case E_OR: + case E_AND: + expr_free(e->left.expr); + expr_free(e->right.expr); + break; + default: + printf("how to free type %d?\n", e->type); + break; + } + free(e); +} + +static int trans_count; + +#define e1 (*ep1) +#define e2 (*ep2) + +static void __expr_eliminate_eq(enum expr_type type, struct expr **ep1, struct expr **ep2) +{ + if (e1->type == type) { + __expr_eliminate_eq(type, &e1->left.expr, &e2); + __expr_eliminate_eq(type, &e1->right.expr, &e2); + return; + } + if (e2->type == type) { + __expr_eliminate_eq(type, &e1, &e2->left.expr); + __expr_eliminate_eq(type, &e1, &e2->right.expr); + return; + } + if (e1->type == E_SYMBOL && e2->type == E_SYMBOL && + e1->left.sym == e2->left.sym && + (e1->left.sym == &symbol_yes || e1->left.sym == &symbol_no)) + return; + if (!expr_eq(e1, e2)) + return; + trans_count++; + expr_free(e1); expr_free(e2); + switch (type) { + case E_OR: + e1 = expr_alloc_symbol(&symbol_no); + e2 = expr_alloc_symbol(&symbol_no); + break; + case E_AND: + e1 = expr_alloc_symbol(&symbol_yes); + e2 = expr_alloc_symbol(&symbol_yes); + break; + default: + ; + } +} + +void expr_eliminate_eq(struct expr **ep1, struct expr **ep2) +{ + if (!e1 || !e2) + return; + switch (e1->type) { + case E_OR: + case E_AND: + __expr_eliminate_eq(e1->type, ep1, ep2); + default: + ; + } + if (e1->type != e2->type) switch (e2->type) { + case E_OR: + case E_AND: + __expr_eliminate_eq(e2->type, ep1, ep2); + default: + ; + } + e1 = expr_eliminate_yn(e1); + e2 = expr_eliminate_yn(e2); +} + +#undef e1 +#undef e2 + +static int expr_eq(struct expr *e1, struct expr *e2) +{ + int res, old_count; + + if (e1->type != e2->type) + return 0; + switch (e1->type) { + case E_EQUAL: + case E_GEQ: + case E_GTH: + case E_LEQ: + case E_LTH: + case E_UNEQUAL: + return e1->left.sym == e2->left.sym && e1->right.sym == e2->right.sym; + case E_SYMBOL: + return e1->left.sym == e2->left.sym; + case E_NOT: + return expr_eq(e1->left.expr, e2->left.expr); + case E_AND: + case E_OR: + e1 = expr_copy(e1); + e2 = expr_copy(e2); + old_count = trans_count; + expr_eliminate_eq(&e1, &e2); + res = (e1->type == E_SYMBOL && e2->type == E_SYMBOL && + e1->left.sym == e2->left.sym); + expr_free(e1); + expr_free(e2); + trans_count = old_count; + return res; + case E_LIST: + case E_RANGE: + case E_NONE: + /* panic */; + } + + if (DEBUG_EXPR) { + expr_fprint(e1, stdout); + printf(" = "); + expr_fprint(e2, stdout); + printf(" ?\n"); + } + + return 0; +} + +static struct expr *expr_eliminate_yn(struct expr *e) +{ + struct expr *tmp; + + if (e) switch (e->type) { + case E_AND: + e->left.expr = expr_eliminate_yn(e->left.expr); + e->right.expr = expr_eliminate_yn(e->right.expr); + if (e->left.expr->type == E_SYMBOL) { + if (e->left.expr->left.sym == &symbol_no) { + expr_free(e->left.expr); + expr_free(e->right.expr); + e->type = E_SYMBOL; + e->left.sym = &symbol_no; + e->right.expr = NULL; + return e; + } else if (e->left.expr->left.sym == &symbol_yes) { + free(e->left.expr); + tmp = e->right.expr; + *e = *(e->right.expr); + free(tmp); + return e; + } + } + if (e->right.expr->type == E_SYMBOL) { + if (e->right.expr->left.sym == &symbol_no) { + expr_free(e->left.expr); + expr_free(e->right.expr); + e->type = E_SYMBOL; + e->left.sym = &symbol_no; + e->right.expr = NULL; + return e; + } else if (e->right.expr->left.sym == &symbol_yes) { + free(e->right.expr); + tmp = e->left.expr; + *e = *(e->left.expr); + free(tmp); + return e; + } + } + break; + case E_OR: + e->left.expr = expr_eliminate_yn(e->left.expr); + e->right.expr = expr_eliminate_yn(e->right.expr); + if (e->left.expr->type == E_SYMBOL) { + if (e->left.expr->left.sym == &symbol_no) { + free(e->left.expr); + tmp = e->right.expr; + *e = *(e->right.expr); + free(tmp); + return e; + } else if (e->left.expr->left.sym == &symbol_yes) { + expr_free(e->left.expr); + expr_free(e->right.expr); + e->type = E_SYMBOL; + e->left.sym = &symbol_yes; + e->right.expr = NULL; + return e; + } + } + if (e->right.expr->type == E_SYMBOL) { + if (e->right.expr->left.sym == &symbol_no) { + free(e->right.expr); + tmp = e->left.expr; + *e = *(e->left.expr); + free(tmp); + return e; + } else if (e->right.expr->left.sym == &symbol_yes) { + expr_free(e->left.expr); + expr_free(e->right.expr); + e->type = E_SYMBOL; + e->left.sym = &symbol_yes; + e->right.expr = NULL; + return e; + } + } + break; + default: + ; + } + return e; +} + +/* + * bool FOO!=n => FOO + */ +struct expr *expr_trans_bool(struct expr *e) +{ + if (!e) + return NULL; + switch (e->type) { + case E_AND: + case E_OR: + case E_NOT: + e->left.expr = expr_trans_bool(e->left.expr); + e->right.expr = expr_trans_bool(e->right.expr); + break; + case E_UNEQUAL: + // FOO!=n -> FOO + if (e->left.sym->type == S_TRISTATE) { + if (e->right.sym == &symbol_no) { + e->type = E_SYMBOL; + e->right.sym = NULL; + } + } + break; + default: + ; + } + return e; +} + +/* + * e1 || e2 -> ? + */ +static struct expr *expr_join_or(struct expr *e1, struct expr *e2) +{ + struct expr *tmp; + struct symbol *sym1, *sym2; + + if (expr_eq(e1, e2)) + return expr_copy(e1); + if (e1->type != E_EQUAL && e1->type != E_UNEQUAL && e1->type != E_SYMBOL && e1->type != E_NOT) + return NULL; + if (e2->type != E_EQUAL && e2->type != E_UNEQUAL && e2->type != E_SYMBOL && e2->type != E_NOT) + return NULL; + if (e1->type == E_NOT) { + tmp = e1->left.expr; + if (tmp->type != E_EQUAL && tmp->type != E_UNEQUAL && tmp->type != E_SYMBOL) + return NULL; + sym1 = tmp->left.sym; + } else + sym1 = e1->left.sym; + if (e2->type == E_NOT) { + if (e2->left.expr->type != E_SYMBOL) + return NULL; + sym2 = e2->left.expr->left.sym; + } else + sym2 = e2->left.sym; + if (sym1 != sym2) + return NULL; + if (sym1->type != S_BOOLEAN && sym1->type != S_TRISTATE) + return NULL; + if (sym1->type == S_TRISTATE) { + if (e1->type == E_EQUAL && e2->type == E_EQUAL && + ((e1->right.sym == &symbol_yes && e2->right.sym == &symbol_mod) || + (e1->right.sym == &symbol_mod && e2->right.sym == &symbol_yes))) { + // (a='y') || (a='m') -> (a!='n') + return expr_alloc_comp(E_UNEQUAL, sym1, &symbol_no); + } + if (e1->type == E_EQUAL && e2->type == E_EQUAL && + ((e1->right.sym == &symbol_yes && e2->right.sym == &symbol_no) || + (e1->right.sym == &symbol_no && e2->right.sym == &symbol_yes))) { + // (a='y') || (a='n') -> (a!='m') + return expr_alloc_comp(E_UNEQUAL, sym1, &symbol_mod); + } + if (e1->type == E_EQUAL && e2->type == E_EQUAL && + ((e1->right.sym == &symbol_mod && e2->right.sym == &symbol_no) || + (e1->right.sym == &symbol_no && e2->right.sym == &symbol_mod))) { + // (a='m') || (a='n') -> (a!='y') + return expr_alloc_comp(E_UNEQUAL, sym1, &symbol_yes); + } + } + if (sym1->type == S_BOOLEAN && sym1 == sym2) { + if ((e1->type == E_NOT && e1->left.expr->type == E_SYMBOL && e2->type == E_SYMBOL) || + (e2->type == E_NOT && e2->left.expr->type == E_SYMBOL && e1->type == E_SYMBOL)) + return expr_alloc_symbol(&symbol_yes); + } + + if (DEBUG_EXPR) { + printf("optimize ("); + expr_fprint(e1, stdout); + printf(") || ("); + expr_fprint(e2, stdout); + printf(")?\n"); + } + return NULL; +} + +static struct expr *expr_join_and(struct expr *e1, struct expr *e2) +{ + struct expr *tmp; + struct symbol *sym1, *sym2; + + if (expr_eq(e1, e2)) + return expr_copy(e1); + if (e1->type != E_EQUAL && e1->type != E_UNEQUAL && e1->type != E_SYMBOL && e1->type != E_NOT) + return NULL; + if (e2->type != E_EQUAL && e2->type != E_UNEQUAL && e2->type != E_SYMBOL && e2->type != E_NOT) + return NULL; + if (e1->type == E_NOT) { + tmp = e1->left.expr; + if (tmp->type != E_EQUAL && tmp->type != E_UNEQUAL && tmp->type != E_SYMBOL) + return NULL; + sym1 = tmp->left.sym; + } else + sym1 = e1->left.sym; + if (e2->type == E_NOT) { + if (e2->left.expr->type != E_SYMBOL) + return NULL; + sym2 = e2->left.expr->left.sym; + } else + sym2 = e2->left.sym; + if (sym1 != sym2) + return NULL; + if (sym1->type != S_BOOLEAN && sym1->type != S_TRISTATE) + return NULL; + + if ((e1->type == E_SYMBOL && e2->type == E_EQUAL && e2->right.sym == &symbol_yes) || + (e2->type == E_SYMBOL && e1->type == E_EQUAL && e1->right.sym == &symbol_yes)) + // (a) && (a='y') -> (a='y') + return expr_alloc_comp(E_EQUAL, sym1, &symbol_yes); + + if ((e1->type == E_SYMBOL && e2->type == E_UNEQUAL && e2->right.sym == &symbol_no) || + (e2->type == E_SYMBOL && e1->type == E_UNEQUAL && e1->right.sym == &symbol_no)) + // (a) && (a!='n') -> (a) + return expr_alloc_symbol(sym1); + + if ((e1->type == E_SYMBOL && e2->type == E_UNEQUAL && e2->right.sym == &symbol_mod) || + (e2->type == E_SYMBOL && e1->type == E_UNEQUAL && e1->right.sym == &symbol_mod)) + // (a) && (a!='m') -> (a='y') + return expr_alloc_comp(E_EQUAL, sym1, &symbol_yes); + + if (sym1->type == S_TRISTATE) { + if (e1->type == E_EQUAL && e2->type == E_UNEQUAL) { + // (a='b') && (a!='c') -> 'b'='c' ? 'n' : a='b' + sym2 = e1->right.sym; + if ((e2->right.sym->flags & SYMBOL_CONST) && (sym2->flags & SYMBOL_CONST)) + return sym2 != e2->right.sym ? expr_alloc_comp(E_EQUAL, sym1, sym2) + : expr_alloc_symbol(&symbol_no); + } + if (e1->type == E_UNEQUAL && e2->type == E_EQUAL) { + // (a='b') && (a!='c') -> 'b'='c' ? 'n' : a='b' + sym2 = e2->right.sym; + if ((e1->right.sym->flags & SYMBOL_CONST) && (sym2->flags & SYMBOL_CONST)) + return sym2 != e1->right.sym ? expr_alloc_comp(E_EQUAL, sym1, sym2) + : expr_alloc_symbol(&symbol_no); + } + if (e1->type == E_UNEQUAL && e2->type == E_UNEQUAL && + ((e1->right.sym == &symbol_yes && e2->right.sym == &symbol_no) || + (e1->right.sym == &symbol_no && e2->right.sym == &symbol_yes))) + // (a!='y') && (a!='n') -> (a='m') + return expr_alloc_comp(E_EQUAL, sym1, &symbol_mod); + + if (e1->type == E_UNEQUAL && e2->type == E_UNEQUAL && + ((e1->right.sym == &symbol_yes && e2->right.sym == &symbol_mod) || + (e1->right.sym == &symbol_mod && e2->right.sym == &symbol_yes))) + // (a!='y') && (a!='m') -> (a='n') + return expr_alloc_comp(E_EQUAL, sym1, &symbol_no); + + if (e1->type == E_UNEQUAL && e2->type == E_UNEQUAL && + ((e1->right.sym == &symbol_mod && e2->right.sym == &symbol_no) || + (e1->right.sym == &symbol_no && e2->right.sym == &symbol_mod))) + // (a!='m') && (a!='n') -> (a='m') + return expr_alloc_comp(E_EQUAL, sym1, &symbol_yes); + + if ((e1->type == E_SYMBOL && e2->type == E_EQUAL && e2->right.sym == &symbol_mod) || + (e2->type == E_SYMBOL && e1->type == E_EQUAL && e1->right.sym == &symbol_mod) || + (e1->type == E_SYMBOL && e2->type == E_UNEQUAL && e2->right.sym == &symbol_yes) || + (e2->type == E_SYMBOL && e1->type == E_UNEQUAL && e1->right.sym == &symbol_yes)) + return NULL; + } + + if (DEBUG_EXPR) { + printf("optimize ("); + expr_fprint(e1, stdout); + printf(") && ("); + expr_fprint(e2, stdout); + printf(")?\n"); + } + return NULL; +} + +static void expr_eliminate_dups1(enum expr_type type, struct expr **ep1, struct expr **ep2) +{ +#define e1 (*ep1) +#define e2 (*ep2) + struct expr *tmp; + + if (e1->type == type) { + expr_eliminate_dups1(type, &e1->left.expr, &e2); + expr_eliminate_dups1(type, &e1->right.expr, &e2); + return; + } + if (e2->type == type) { + expr_eliminate_dups1(type, &e1, &e2->left.expr); + expr_eliminate_dups1(type, &e1, &e2->right.expr); + return; + } + if (e1 == e2) + return; + + switch (e1->type) { + case E_OR: case E_AND: + expr_eliminate_dups1(e1->type, &e1, &e1); + default: + ; + } + + switch (type) { + case E_OR: + tmp = expr_join_or(e1, e2); + if (tmp) { + expr_free(e1); expr_free(e2); + e1 = expr_alloc_symbol(&symbol_no); + e2 = tmp; + trans_count++; + } + break; + case E_AND: + tmp = expr_join_and(e1, e2); + if (tmp) { + expr_free(e1); expr_free(e2); + e1 = expr_alloc_symbol(&symbol_yes); + e2 = tmp; + trans_count++; + } + break; + default: + ; + } +#undef e1 +#undef e2 +} + +struct expr *expr_eliminate_dups(struct expr *e) +{ + int oldcount; + if (!e) + return e; + + oldcount = trans_count; + while (1) { + trans_count = 0; + switch (e->type) { + case E_OR: case E_AND: + expr_eliminate_dups1(e->type, &e, &e); + default: + ; + } + if (!trans_count) + break; + e = expr_eliminate_yn(e); + } + trans_count = oldcount; + return e; +} + +struct expr *expr_transform(struct expr *e) +{ + struct expr *tmp; + + if (!e) + return NULL; + switch (e->type) { + case E_EQUAL: + case E_GEQ: + case E_GTH: + case E_LEQ: + case E_LTH: + case E_UNEQUAL: + case E_SYMBOL: + case E_LIST: + break; + default: + e->left.expr = expr_transform(e->left.expr); + e->right.expr = expr_transform(e->right.expr); + } + + switch (e->type) { + case E_EQUAL: + if (e->left.sym->type != S_BOOLEAN) + break; + if (e->right.sym == &symbol_no) { + e->type = E_NOT; + e->left.expr = expr_alloc_symbol(e->left.sym); + e->right.sym = NULL; + break; + } + if (e->right.sym == &symbol_mod) { + printf("boolean symbol %s tested for 'm'? test forced to 'n'\n", e->left.sym->name); + e->type = E_SYMBOL; + e->left.sym = &symbol_no; + e->right.sym = NULL; + break; + } + if (e->right.sym == &symbol_yes) { + e->type = E_SYMBOL; + e->right.sym = NULL; + break; + } + break; + case E_UNEQUAL: + if (e->left.sym->type != S_BOOLEAN) + break; + if (e->right.sym == &symbol_no) { + e->type = E_SYMBOL; + e->right.sym = NULL; + break; + } + if (e->right.sym == &symbol_mod) { + printf("boolean symbol %s tested for 'm'? test forced to 'y'\n", e->left.sym->name); + e->type = E_SYMBOL; + e->left.sym = &symbol_yes; + e->right.sym = NULL; + break; + } + if (e->right.sym == &symbol_yes) { + e->type = E_NOT; + e->left.expr = expr_alloc_symbol(e->left.sym); + e->right.sym = NULL; + break; + } + break; + case E_NOT: + switch (e->left.expr->type) { + case E_NOT: + // !!a -> a + tmp = e->left.expr->left.expr; + free(e->left.expr); + free(e); + e = tmp; + e = expr_transform(e); + break; + case E_EQUAL: + case E_UNEQUAL: + // !a='x' -> a!='x' + tmp = e->left.expr; + free(e); + e = tmp; + e->type = e->type == E_EQUAL ? E_UNEQUAL : E_EQUAL; + break; + case E_LEQ: + case E_GEQ: + // !a<='x' -> a>'x' + tmp = e->left.expr; + free(e); + e = tmp; + e->type = e->type == E_LEQ ? E_GTH : E_LTH; + break; + case E_LTH: + case E_GTH: + // !a<'x' -> a>='x' + tmp = e->left.expr; + free(e); + e = tmp; + e->type = e->type == E_LTH ? E_GEQ : E_LEQ; + break; + case E_OR: + // !(a || b) -> !a && !b + tmp = e->left.expr; + e->type = E_AND; + e->right.expr = expr_alloc_one(E_NOT, tmp->right.expr); + tmp->type = E_NOT; + tmp->right.expr = NULL; + e = expr_transform(e); + break; + case E_AND: + // !(a && b) -> !a || !b + tmp = e->left.expr; + e->type = E_OR; + e->right.expr = expr_alloc_one(E_NOT, tmp->right.expr); + tmp->type = E_NOT; + tmp->right.expr = NULL; + e = expr_transform(e); + break; + case E_SYMBOL: + if (e->left.expr->left.sym == &symbol_yes) { + // !'y' -> 'n' + tmp = e->left.expr; + free(e); + e = tmp; + e->type = E_SYMBOL; + e->left.sym = &symbol_no; + break; + } + if (e->left.expr->left.sym == &symbol_mod) { + // !'m' -> 'm' + tmp = e->left.expr; + free(e); + e = tmp; + e->type = E_SYMBOL; + e->left.sym = &symbol_mod; + break; + } + if (e->left.expr->left.sym == &symbol_no) { + // !'n' -> 'y' + tmp = e->left.expr; + free(e); + e = tmp; + e->type = E_SYMBOL; + e->left.sym = &symbol_yes; + break; + } + break; + default: + ; + } + break; + default: + ; + } + return e; +} + +int expr_contains_symbol(struct expr *dep, struct symbol *sym) +{ + if (!dep) + return 0; + + switch (dep->type) { + case E_AND: + case E_OR: + return expr_contains_symbol(dep->left.expr, sym) || + expr_contains_symbol(dep->right.expr, sym); + case E_SYMBOL: + return dep->left.sym == sym; + case E_EQUAL: + case E_GEQ: + case E_GTH: + case E_LEQ: + case E_LTH: + case E_UNEQUAL: + return dep->left.sym == sym || + dep->right.sym == sym; + case E_NOT: + return expr_contains_symbol(dep->left.expr, sym); + default: + ; + } + return 0; +} + +bool expr_depends_symbol(struct expr *dep, struct symbol *sym) +{ + if (!dep) + return false; + + switch (dep->type) { + case E_AND: + return expr_depends_symbol(dep->left.expr, sym) || + expr_depends_symbol(dep->right.expr, sym); + case E_SYMBOL: + return dep->left.sym == sym; + case E_EQUAL: + if (dep->left.sym == sym) { + if (dep->right.sym == &symbol_yes || dep->right.sym == &symbol_mod) + return true; + } + break; + case E_UNEQUAL: + if (dep->left.sym == sym) { + if (dep->right.sym == &symbol_no) + return true; + } + break; + default: + ; + } + return false; +} + +struct expr *expr_trans_compare(struct expr *e, enum expr_type type, struct symbol *sym) +{ + struct expr *e1, *e2; + + if (!e) { + e = expr_alloc_symbol(sym); + if (type == E_UNEQUAL) + e = expr_alloc_one(E_NOT, e); + return e; + } + switch (e->type) { + case E_AND: + e1 = expr_trans_compare(e->left.expr, E_EQUAL, sym); + e2 = expr_trans_compare(e->right.expr, E_EQUAL, sym); + if (sym == &symbol_yes) + e = expr_alloc_two(E_AND, e1, e2); + if (sym == &symbol_no) + e = expr_alloc_two(E_OR, e1, e2); + if (type == E_UNEQUAL) + e = expr_alloc_one(E_NOT, e); + return e; + case E_OR: + e1 = expr_trans_compare(e->left.expr, E_EQUAL, sym); + e2 = expr_trans_compare(e->right.expr, E_EQUAL, sym); + if (sym == &symbol_yes) + e = expr_alloc_two(E_OR, e1, e2); + if (sym == &symbol_no) + e = expr_alloc_two(E_AND, e1, e2); + if (type == E_UNEQUAL) + e = expr_alloc_one(E_NOT, e); + return e; + case E_NOT: + return expr_trans_compare(e->left.expr, type == E_EQUAL ? E_UNEQUAL : E_EQUAL, sym); + case E_UNEQUAL: + case E_LTH: + case E_LEQ: + case E_GTH: + case E_GEQ: + case E_EQUAL: + if (type == E_EQUAL) { + if (sym == &symbol_yes) + return expr_copy(e); + if (sym == &symbol_mod) + return expr_alloc_symbol(&symbol_no); + if (sym == &symbol_no) + return expr_alloc_one(E_NOT, expr_copy(e)); + } else { + if (sym == &symbol_yes) + return expr_alloc_one(E_NOT, expr_copy(e)); + if (sym == &symbol_mod) + return expr_alloc_symbol(&symbol_yes); + if (sym == &symbol_no) + return expr_copy(e); + } + break; + case E_SYMBOL: + return expr_alloc_comp(type, e->left.sym, sym); + case E_LIST: + case E_RANGE: + case E_NONE: + /* panic */; + } + return NULL; +} + +enum string_value_kind { + k_string, + k_signed, + k_unsigned, + k_invalid +}; + +union string_value { + unsigned long long u; + signed long long s; +}; + +static enum string_value_kind expr_parse_string(const char *str, + enum symbol_type type, + union string_value *val) +{ + char *tail; + enum string_value_kind kind; + + errno = 0; + switch (type) { + case S_BOOLEAN: + case S_TRISTATE: + return k_string; + case S_INT: + val->s = strtoll(str, &tail, 10); + kind = k_signed; + break; + case S_HEX: + val->u = strtoull(str, &tail, 16); + kind = k_unsigned; + break; + case S_STRING: + case S_UNKNOWN: + val->s = strtoll(str, &tail, 0); + kind = k_signed; + break; + default: + return k_invalid; + } + return !errno && !*tail && tail > str && isxdigit(tail[-1]) + ? kind : k_string; +} + +tristate expr_calc_value(struct expr *e) +{ + tristate val1, val2; + const char *str1, *str2; + enum string_value_kind k1 = k_string, k2 = k_string; + union string_value lval = {}, rval = {}; + int res; + + if (!e) + return yes; + + switch (e->type) { + case E_SYMBOL: + sym_calc_value(e->left.sym); + return e->left.sym->curr.tri; + case E_AND: + val1 = expr_calc_value(e->left.expr); + val2 = expr_calc_value(e->right.expr); + return EXPR_AND(val1, val2); + case E_OR: + val1 = expr_calc_value(e->left.expr); + val2 = expr_calc_value(e->right.expr); + return EXPR_OR(val1, val2); + case E_NOT: + val1 = expr_calc_value(e->left.expr); + return EXPR_NOT(val1); + case E_EQUAL: + case E_GEQ: + case E_GTH: + case E_LEQ: + case E_LTH: + case E_UNEQUAL: + break; + default: + printf("expr_calc_value: %d?\n", e->type); + return no; + } + + sym_calc_value(e->left.sym); + sym_calc_value(e->right.sym); + str1 = sym_get_string_value(e->left.sym); + str2 = sym_get_string_value(e->right.sym); + + if (e->left.sym->type != S_STRING || e->right.sym->type != S_STRING) { + k1 = expr_parse_string(str1, e->left.sym->type, &lval); + k2 = expr_parse_string(str2, e->right.sym->type, &rval); + } + + if (k1 == k_string || k2 == k_string) + res = strcmp(str1, str2); + else if (k1 == k_invalid || k2 == k_invalid) { + if (e->type != E_EQUAL && e->type != E_UNEQUAL) { + printf("Cannot compare \"%s\" and \"%s\"\n", str1, str2); + return no; + } + res = strcmp(str1, str2); + } else if (k1 == k_unsigned || k2 == k_unsigned) + res = (lval.u > rval.u) - (lval.u < rval.u); + else /* if (k1 == k_signed && k2 == k_signed) */ + res = (lval.s > rval.s) - (lval.s < rval.s); + + switch(e->type) { + case E_EQUAL: + return res ? no : yes; + case E_GEQ: + return res >= 0 ? yes : no; + case E_GTH: + return res > 0 ? yes : no; + case E_LEQ: + return res <= 0 ? yes : no; + case E_LTH: + return res < 0 ? yes : no; + case E_UNEQUAL: + return res ? yes : no; + default: + printf("expr_calc_value: relation %d?\n", e->type); + return no; + } +} + +static int expr_compare_type(enum expr_type t1, enum expr_type t2) +{ + if (t1 == t2) + return 0; + switch (t1) { + case E_LEQ: + case E_LTH: + case E_GEQ: + case E_GTH: + if (t2 == E_EQUAL || t2 == E_UNEQUAL) + return 1; + case E_EQUAL: + case E_UNEQUAL: + if (t2 == E_NOT) + return 1; + case E_NOT: + if (t2 == E_AND) + return 1; + case E_AND: + if (t2 == E_OR) + return 1; + case E_OR: + if (t2 == E_LIST) + return 1; + case E_LIST: + if (t2 == 0) + return 1; + default: + return -1; + } + printf("[%dgt%d?]", t1, t2); + return 0; +} + +static inline struct expr * +expr_get_leftmost_symbol(const struct expr *e) +{ + + if (e == NULL) + return NULL; + + while (e->type != E_SYMBOL) + e = e->left.expr; + + return expr_copy(e); +} + +/* + * Given expression `e1' and `e2', returns the leaf of the longest + * sub-expression of `e1' not containing 'e2. + */ +struct expr *expr_simplify_unmet_dep(struct expr *e1, struct expr *e2) +{ + struct expr *ret; + + switch (e1->type) { + case E_OR: + return expr_alloc_and( + expr_simplify_unmet_dep(e1->left.expr, e2), + expr_simplify_unmet_dep(e1->right.expr, e2)); + case E_AND: { + struct expr *e; + e = expr_alloc_and(expr_copy(e1), expr_copy(e2)); + e = expr_eliminate_dups(e); + ret = (!expr_eq(e, e1)) ? e1 : NULL; + expr_free(e); + break; + } + default: + ret = e1; + break; + } + + return expr_get_leftmost_symbol(ret); +} + +void expr_print(struct expr *e, void (*fn)(void *, struct symbol *, const char *), void *data, int prevtoken) +{ + if (!e) { + fn(data, NULL, "y"); + return; + } + + if (expr_compare_type(prevtoken, e->type) > 0) + fn(data, NULL, "("); + switch (e->type) { + case E_SYMBOL: + if (e->left.sym->name) + fn(data, e->left.sym, e->left.sym->name); + else + fn(data, NULL, ""); + break; + case E_NOT: + fn(data, NULL, "!"); + expr_print(e->left.expr, fn, data, E_NOT); + break; + case E_EQUAL: + if (e->left.sym->name) + fn(data, e->left.sym, e->left.sym->name); + else + fn(data, NULL, ""); + fn(data, NULL, "="); + fn(data, e->right.sym, e->right.sym->name); + break; + case E_LEQ: + case E_LTH: + if (e->left.sym->name) + fn(data, e->left.sym, e->left.sym->name); + else + fn(data, NULL, ""); + fn(data, NULL, e->type == E_LEQ ? "<=" : "<"); + fn(data, e->right.sym, e->right.sym->name); + break; + case E_GEQ: + case E_GTH: + if (e->left.sym->name) + fn(data, e->left.sym, e->left.sym->name); + else + fn(data, NULL, ""); + fn(data, NULL, e->type == E_GEQ ? ">=" : ">"); + fn(data, e->right.sym, e->right.sym->name); + break; + case E_UNEQUAL: + if (e->left.sym->name) + fn(data, e->left.sym, e->left.sym->name); + else + fn(data, NULL, ""); + fn(data, NULL, "!="); + fn(data, e->right.sym, e->right.sym->name); + break; + case E_OR: + expr_print(e->left.expr, fn, data, E_OR); + fn(data, NULL, " || "); + expr_print(e->right.expr, fn, data, E_OR); + break; + case E_AND: + expr_print(e->left.expr, fn, data, E_AND); + fn(data, NULL, " && "); + expr_print(e->right.expr, fn, data, E_AND); + break; + case E_LIST: + fn(data, e->right.sym, e->right.sym->name); + if (e->left.expr) { + fn(data, NULL, " ^ "); + expr_print(e->left.expr, fn, data, E_LIST); + } + break; + case E_RANGE: + fn(data, NULL, "["); + fn(data, e->left.sym, e->left.sym->name); + fn(data, NULL, " "); + fn(data, e->right.sym, e->right.sym->name); + fn(data, NULL, "]"); + break; + default: + { + char buf[32]; + sprintf(buf, "", e->type); + fn(data, NULL, buf); + break; + } + } + if (expr_compare_type(prevtoken, e->type) > 0) + fn(data, NULL, ")"); +} + +static void expr_print_file_helper(void *data, struct symbol *sym, const char *str) +{ + xfwrite(str, strlen(str), 1, data); +} + +void expr_fprint(struct expr *e, FILE *out) +{ + expr_print(e, expr_print_file_helper, out, E_NONE); +} + +static void expr_print_gstr_helper(void *data, struct symbol *sym, const char *str) +{ + struct gstr *gs = (struct gstr*)data; + const char *sym_str = NULL; + + if (sym) + sym_str = sym_get_string_value(sym); + + if (gs->max_width) { + unsigned extra_length = strlen(str); + const char *last_cr = strrchr(gs->s, '\n'); + unsigned last_line_length; + + if (sym_str) + extra_length += 4 + strlen(sym_str); + + if (!last_cr) + last_cr = gs->s; + + last_line_length = strlen(gs->s) - (last_cr - gs->s); + + if ((last_line_length + extra_length) > gs->max_width) + str_append(gs, "\\\n"); + } + + str_append(gs, str); + if (sym && sym->type != S_UNKNOWN) + str_printf(gs, " [=%s]", sym_str); +} + +void expr_gstr_print(struct expr *e, struct gstr *gs) +{ + expr_print(e, expr_print_gstr_helper, gs, E_NONE); +} diff --git a/scripts/kconfig/expr.h b/scripts/kconfig/expr.h new file mode 100644 index 0000000000..973b6f7333 --- /dev/null +++ b/scripts/kconfig/expr.h @@ -0,0 +1,238 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#ifndef EXPR_H +#define EXPR_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "list.h" +#ifndef __cplusplus +#include +#endif + +struct file { + struct file *next; + struct file *parent; + const char *name; + int lineno; +}; + +typedef enum tristate { + no, mod, yes +} tristate; + +enum expr_type { + E_NONE, E_OR, E_AND, E_NOT, + E_EQUAL, E_UNEQUAL, E_LTH, E_LEQ, E_GTH, E_GEQ, + E_LIST, E_SYMBOL, E_RANGE +}; + +union expr_data { + struct expr *expr; + struct symbol *sym; +}; + +struct expr { + enum expr_type type; + union expr_data left, right; +}; + +#define EXPR_OR(dep1, dep2) (((dep1)>(dep2))?(dep1):(dep2)) +#define EXPR_AND(dep1, dep2) (((dep1)<(dep2))?(dep1):(dep2)) +#define EXPR_NOT(dep) (2-(dep)) + +#define expr_list_for_each_sym(l, e, s) \ + for (e = (l); e && (s = e->right.sym); e = e->left.expr) + +struct expr_value { + struct expr *expr; + tristate tri; +}; + +struct symbol_value { + void *val; + tristate tri; +}; + +enum symbol_type { + S_UNKNOWN, S_BOOLEAN, S_TRISTATE, S_INT, S_HEX, S_STRING, S_OTHER +}; + +/* enum values are used as index to symbol.def[] */ +enum { + S_DEF_USER, /* main user value */ + S_DEF_AUTO, /* values read from auto.conf */ + S_DEF_DEF3, /* Reserved for UI usage */ + S_DEF_DEF4, /* Reserved for UI usage */ + S_DEF_COUNT +}; + +struct symbol { + struct symbol *next; + char *name; + enum symbol_type type; + struct symbol_value curr; + struct symbol_value def[S_DEF_COUNT]; + tristate visible; + int flags; + struct property *prop; + struct expr_value dir_dep; + struct expr_value rev_dep; +}; + +#define for_all_symbols(i, sym) for (i = 0; i < SYMBOL_HASHSIZE; i++) for (sym = symbol_hash[i]; sym; sym = sym->next) if (sym->type != S_OTHER) + +#define SYMBOL_CONST 0x0001 /* symbol is const */ +#define SYMBOL_CHECK 0x0008 /* used during dependency checking */ +#define SYMBOL_CHOICE 0x0010 /* start of a choice block (null name) */ +#define SYMBOL_CHOICEVAL 0x0020 /* used as a value in a choice block */ +#define SYMBOL_VALID 0x0080 /* set when symbol.curr is calculated */ +#define SYMBOL_OPTIONAL 0x0100 /* choice is optional - values can be 'n' */ +#define SYMBOL_WRITE 0x0200 /* write symbol to file (KCONFIG_CONFIG) */ +#define SYMBOL_CHANGED 0x0400 /* ? */ +#define SYMBOL_AUTO 0x1000 /* value from environment variable */ +#define SYMBOL_CHECKED 0x2000 /* used during dependency checking */ +#define SYMBOL_WARNED 0x8000 /* warning has been issued */ + +/* Set when symbol.def[] is used */ +#define SYMBOL_DEF 0x10000 /* First bit of SYMBOL_DEF */ +#define SYMBOL_DEF_USER 0x10000 /* symbol.def[S_DEF_USER] is valid */ +#define SYMBOL_DEF_AUTO 0x20000 /* symbol.def[S_DEF_AUTO] is valid */ +#define SYMBOL_DEF3 0x40000 /* symbol.def[S_DEF_3] is valid */ +#define SYMBOL_DEF4 0x80000 /* symbol.def[S_DEF_4] is valid */ + +/* choice values need to be set before calculating this symbol value */ +#define SYMBOL_NEED_SET_CHOICE_VALUES 0x100000 + +/* Set symbol to y if allnoconfig; used for symbols that hide others */ +#define SYMBOL_ALLNOCONFIG_Y 0x200000 + +#define SYMBOL_MAXLENGTH 256 +#define SYMBOL_HASHSIZE 9973 + +/* A property represent the config options that can be associated + * with a config "symbol". + * Sample: + * config FOO + * default y + * prompt "foo prompt" + * select BAR + * config BAZ + * int "BAZ Value" + * range 1..255 + */ +enum prop_type { + P_UNKNOWN, + P_PROMPT, /* prompt "foo prompt" or "BAZ Value" */ + P_COMMENT, /* text associated with a comment */ + P_MENU, /* prompt associated with a menuconfig option */ + P_DEFAULT, /* default y */ + P_CHOICE, /* choice value */ + P_SELECT, /* select BAR */ + P_RANGE, /* range 7..100 (for a symbol) */ + P_ENV, /* value from environment variable */ + P_SYMBOL, /* where a symbol is defined */ +}; + +struct property { + struct property *next; /* next property - null if last */ + struct symbol *sym; /* the symbol for which the property is associated */ + enum prop_type type; /* type of property */ + const char *text; /* the prompt value - P_PROMPT, P_MENU, P_COMMENT */ + struct expr_value visible; + struct expr *expr; /* the optional conditional part of the property */ + struct menu *menu; /* the menu the property are associated with + * valid for: P_SELECT, P_RANGE, P_CHOICE, + * P_PROMPT, P_DEFAULT, P_MENU, P_COMMENT */ + struct file *file; /* what file was this property defined */ + int lineno; /* what lineno was this property defined */ +}; + +#define for_all_properties(sym, st, tok) \ + for (st = sym->prop; st; st = st->next) \ + if (st->type == (tok)) +#define for_all_defaults(sym, st) for_all_properties(sym, st, P_DEFAULT) +#define for_all_choices(sym, st) for_all_properties(sym, st, P_CHOICE) +#define for_all_prompts(sym, st) \ + for (st = sym->prop; st; st = st->next) \ + if (st->text) + +struct menu { + struct menu *next; + struct menu *parent; + struct menu *list; + struct symbol *sym; + struct property *prompt; + struct expr *visibility; + struct expr *dep; + unsigned int flags; + char *help; + struct file *file; + int lineno; + void *data; +}; + +#define MENU_CHANGED 0x0001 +#define MENU_ROOT 0x0002 + +struct jump_key { + struct list_head entries; + size_t offset; + struct menu *target; + int index; +}; + +#define JUMP_NB 9 + +extern struct file *file_list; +extern struct file *current_file; +struct file *lookup_file(const char *name); + +extern struct symbol symbol_yes, symbol_no, symbol_mod; +extern struct symbol *modules_sym; +extern struct symbol *sym_defconfig_list; +extern int cdebug; +struct expr *expr_alloc_symbol(struct symbol *sym); +struct expr *expr_alloc_one(enum expr_type type, struct expr *ce); +struct expr *expr_alloc_two(enum expr_type type, struct expr *e1, struct expr *e2); +struct expr *expr_alloc_comp(enum expr_type type, struct symbol *s1, struct symbol *s2); +struct expr *expr_alloc_and(struct expr *e1, struct expr *e2); +struct expr *expr_alloc_or(struct expr *e1, struct expr *e2); +struct expr *expr_copy(const struct expr *org); +void expr_free(struct expr *e); +void expr_eliminate_eq(struct expr **ep1, struct expr **ep2); +tristate expr_calc_value(struct expr *e); +struct expr *expr_trans_bool(struct expr *e); +struct expr *expr_eliminate_dups(struct expr *e); +struct expr *expr_transform(struct expr *e); +int expr_contains_symbol(struct expr *dep, struct symbol *sym); +bool expr_depends_symbol(struct expr *dep, struct symbol *sym); +struct expr *expr_trans_compare(struct expr *e, enum expr_type type, struct symbol *sym); +struct expr *expr_simplify_unmet_dep(struct expr *e1, struct expr *e2); + +void expr_fprint(struct expr *e, FILE *out); +struct gstr; /* forward */ +void expr_gstr_print(struct expr *e, struct gstr *gs); + +static inline int expr_is_yes(struct expr *e) +{ + return !e || (e->type == E_SYMBOL && e->left.sym == &symbol_yes); +} + +static inline int expr_is_no(struct expr *e) +{ + return e && (e->type == E_SYMBOL && e->left.sym == &symbol_no); +} + +#ifdef __cplusplus +} +#endif + +#endif /* EXPR_H */ diff --git a/scripts/kconfig/gconf.c b/scripts/kconfig/gconf.c new file mode 100644 index 0000000000..8b2653fed1 --- /dev/null +++ b/scripts/kconfig/gconf.c @@ -0,0 +1,1521 @@ +/* Hey EMACS -*- linux-c -*- */ +/* + * + * Copyright (C) 2002-2003 Romain Lievin + * Released under the terms of the GNU GPL v2.0. + * + */ + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include +#include "lkc.h" +#include "images.c" + +#include +#include +#include +#include + +#include +#include +#include +#include + +//#define DEBUG + +enum { + SINGLE_VIEW, SPLIT_VIEW, FULL_VIEW +}; + +enum { + OPT_NORMAL, OPT_ALL, OPT_PROMPT +}; + +static gint view_mode = FULL_VIEW; +static gboolean show_name = TRUE; +static gboolean show_range = TRUE; +static gboolean show_value = TRUE; +static gboolean resizeable = FALSE; +static int opt_mode = OPT_NORMAL; + +GtkWidget *main_wnd = NULL; +GtkWidget *tree1_w = NULL; // left frame +GtkWidget *tree2_w = NULL; // right frame +GtkWidget *text_w = NULL; +GtkWidget *hpaned = NULL; +GtkWidget *vpaned = NULL; +GtkWidget *back_btn = NULL; +GtkWidget *save_btn = NULL; +GtkWidget *save_menu_item = NULL; + +GtkTextTag *tag1, *tag2; +GdkColor color; + +GtkTreeStore *tree1, *tree2, *tree; +GtkTreeModel *model1, *model2; +static GtkTreeIter *parents[256]; +static gint indent; + +static struct menu *current; // current node for SINGLE view +static struct menu *browsed; // browsed node for SPLIT view + +enum { + COL_OPTION, COL_NAME, COL_NO, COL_MOD, COL_YES, COL_VALUE, + COL_MENU, COL_COLOR, COL_EDIT, COL_PIXBUF, + COL_PIXVIS, COL_BTNVIS, COL_BTNACT, COL_BTNINC, COL_BTNRAD, + COL_NUMBER +}; + +static void display_list(void); +static void display_tree(struct menu *menu); +static void display_tree_part(void); +static void update_tree(struct menu *src, GtkTreeIter * dst); +static void set_node(GtkTreeIter * node, struct menu *menu, gchar ** row); +static gchar **fill_row(struct menu *menu); +static void conf_changed(void); + +/* Helping/Debugging Functions */ + +const char *dbg_sym_flags(int val) +{ + static char buf[256]; + + bzero(buf, 256); + + if (val & SYMBOL_CONST) + strcat(buf, "const/"); + if (val & SYMBOL_CHECK) + strcat(buf, "check/"); + if (val & SYMBOL_CHOICE) + strcat(buf, "choice/"); + if (val & SYMBOL_CHOICEVAL) + strcat(buf, "choiceval/"); + if (val & SYMBOL_VALID) + strcat(buf, "valid/"); + if (val & SYMBOL_OPTIONAL) + strcat(buf, "optional/"); + if (val & SYMBOL_WRITE) + strcat(buf, "write/"); + if (val & SYMBOL_CHANGED) + strcat(buf, "changed/"); + if (val & SYMBOL_AUTO) + strcat(buf, "auto/"); + + buf[strlen(buf) - 1] = '\0'; + + return buf; +} + +void replace_button_icon(GladeXML * xml, GdkDrawable * window, + GtkStyle * style, gchar * btn_name, gchar ** xpm) +{ + GdkPixmap *pixmap; + GdkBitmap *mask; + GtkToolButton *button; + GtkWidget *image; + + pixmap = gdk_pixmap_create_from_xpm_d(window, &mask, + &style->bg[GTK_STATE_NORMAL], + xpm); + + button = GTK_TOOL_BUTTON(glade_xml_get_widget(xml, btn_name)); + image = gtk_image_new_from_pixmap(pixmap, mask); + gtk_widget_show(image); + gtk_tool_button_set_icon_widget(button, image); +} + +/* Main Window Initialization */ +void init_main_window(const gchar * glade_file) +{ + GladeXML *xml; + GtkWidget *widget; + GtkTextBuffer *txtbuf; + GtkStyle *style; + + xml = glade_xml_new(glade_file, "window1", NULL); + if (!xml) + g_error(_("GUI loading failed !\n")); + glade_xml_signal_autoconnect(xml); + + main_wnd = glade_xml_get_widget(xml, "window1"); + hpaned = glade_xml_get_widget(xml, "hpaned1"); + vpaned = glade_xml_get_widget(xml, "vpaned1"); + tree1_w = glade_xml_get_widget(xml, "treeview1"); + tree2_w = glade_xml_get_widget(xml, "treeview2"); + text_w = glade_xml_get_widget(xml, "textview3"); + + back_btn = glade_xml_get_widget(xml, "button1"); + gtk_widget_set_sensitive(back_btn, FALSE); + + widget = glade_xml_get_widget(xml, "show_name1"); + gtk_check_menu_item_set_active((GtkCheckMenuItem *) widget, + show_name); + + widget = glade_xml_get_widget(xml, "show_range1"); + gtk_check_menu_item_set_active((GtkCheckMenuItem *) widget, + show_range); + + widget = glade_xml_get_widget(xml, "show_data1"); + gtk_check_menu_item_set_active((GtkCheckMenuItem *) widget, + show_value); + + save_btn = glade_xml_get_widget(xml, "button3"); + save_menu_item = glade_xml_get_widget(xml, "save1"); + conf_set_changed_callback(conf_changed); + + style = gtk_widget_get_style(main_wnd); + widget = glade_xml_get_widget(xml, "toolbar1"); + + replace_button_icon(xml, main_wnd->window, style, + "button4", (gchar **) xpm_single_view); + replace_button_icon(xml, main_wnd->window, style, + "button5", (gchar **) xpm_split_view); + replace_button_icon(xml, main_wnd->window, style, + "button6", (gchar **) xpm_tree_view); + + txtbuf = gtk_text_view_get_buffer(GTK_TEXT_VIEW(text_w)); + tag1 = gtk_text_buffer_create_tag(txtbuf, "mytag1", + "foreground", "red", + "weight", PANGO_WEIGHT_BOLD, + NULL); + tag2 = gtk_text_buffer_create_tag(txtbuf, "mytag2", + /*"style", PANGO_STYLE_OBLIQUE, */ + NULL); + + gtk_window_set_title(GTK_WINDOW(main_wnd), rootmenu.prompt->text); + + gtk_widget_show(main_wnd); +} + +void init_tree_model(void) +{ + gint i; + + tree = tree2 = gtk_tree_store_new(COL_NUMBER, + G_TYPE_STRING, G_TYPE_STRING, + G_TYPE_STRING, G_TYPE_STRING, + G_TYPE_STRING, G_TYPE_STRING, + G_TYPE_POINTER, GDK_TYPE_COLOR, + G_TYPE_BOOLEAN, GDK_TYPE_PIXBUF, + G_TYPE_BOOLEAN, G_TYPE_BOOLEAN, + G_TYPE_BOOLEAN, G_TYPE_BOOLEAN, + G_TYPE_BOOLEAN); + model2 = GTK_TREE_MODEL(tree2); + + for (parents[0] = NULL, i = 1; i < 256; i++) + parents[i] = (GtkTreeIter *) g_malloc(sizeof(GtkTreeIter)); + + tree1 = gtk_tree_store_new(COL_NUMBER, + G_TYPE_STRING, G_TYPE_STRING, + G_TYPE_STRING, G_TYPE_STRING, + G_TYPE_STRING, G_TYPE_STRING, + G_TYPE_POINTER, GDK_TYPE_COLOR, + G_TYPE_BOOLEAN, GDK_TYPE_PIXBUF, + G_TYPE_BOOLEAN, G_TYPE_BOOLEAN, + G_TYPE_BOOLEAN, G_TYPE_BOOLEAN, + G_TYPE_BOOLEAN); + model1 = GTK_TREE_MODEL(tree1); +} + +void init_left_tree(void) +{ + GtkTreeView *view = GTK_TREE_VIEW(tree1_w); + GtkCellRenderer *renderer; + GtkTreeSelection *sel; + GtkTreeViewColumn *column; + + gtk_tree_view_set_model(view, model1); + gtk_tree_view_set_headers_visible(view, TRUE); + gtk_tree_view_set_rules_hint(view, TRUE); + + column = gtk_tree_view_column_new(); + gtk_tree_view_append_column(view, column); + gtk_tree_view_column_set_title(column, _("Options")); + + renderer = gtk_cell_renderer_toggle_new(); + gtk_tree_view_column_pack_start(GTK_TREE_VIEW_COLUMN(column), + renderer, FALSE); + gtk_tree_view_column_set_attributes(GTK_TREE_VIEW_COLUMN(column), + renderer, + "active", COL_BTNACT, + "inconsistent", COL_BTNINC, + "visible", COL_BTNVIS, + "radio", COL_BTNRAD, NULL); + renderer = gtk_cell_renderer_text_new(); + gtk_tree_view_column_pack_start(GTK_TREE_VIEW_COLUMN(column), + renderer, FALSE); + gtk_tree_view_column_set_attributes(GTK_TREE_VIEW_COLUMN(column), + renderer, + "text", COL_OPTION, + "foreground-gdk", + COL_COLOR, NULL); + + sel = gtk_tree_view_get_selection(view); + gtk_tree_selection_set_mode(sel, GTK_SELECTION_SINGLE); + gtk_widget_realize(tree1_w); +} + +static void renderer_edited(GtkCellRendererText * cell, + const gchar * path_string, + const gchar * new_text, gpointer user_data); + +void init_right_tree(void) +{ + GtkTreeView *view = GTK_TREE_VIEW(tree2_w); + GtkCellRenderer *renderer; + GtkTreeSelection *sel; + GtkTreeViewColumn *column; + gint i; + + gtk_tree_view_set_model(view, model2); + gtk_tree_view_set_headers_visible(view, TRUE); + gtk_tree_view_set_rules_hint(view, TRUE); + + column = gtk_tree_view_column_new(); + gtk_tree_view_append_column(view, column); + gtk_tree_view_column_set_title(column, _("Options")); + + renderer = gtk_cell_renderer_pixbuf_new(); + gtk_tree_view_column_pack_start(GTK_TREE_VIEW_COLUMN(column), + renderer, FALSE); + gtk_tree_view_column_set_attributes(GTK_TREE_VIEW_COLUMN(column), + renderer, + "pixbuf", COL_PIXBUF, + "visible", COL_PIXVIS, NULL); + renderer = gtk_cell_renderer_toggle_new(); + gtk_tree_view_column_pack_start(GTK_TREE_VIEW_COLUMN(column), + renderer, FALSE); + gtk_tree_view_column_set_attributes(GTK_TREE_VIEW_COLUMN(column), + renderer, + "active", COL_BTNACT, + "inconsistent", COL_BTNINC, + "visible", COL_BTNVIS, + "radio", COL_BTNRAD, NULL); + renderer = gtk_cell_renderer_text_new(); + gtk_tree_view_column_pack_start(GTK_TREE_VIEW_COLUMN(column), + renderer, FALSE); + gtk_tree_view_column_set_attributes(GTK_TREE_VIEW_COLUMN(column), + renderer, + "text", COL_OPTION, + "foreground-gdk", + COL_COLOR, NULL); + + renderer = gtk_cell_renderer_text_new(); + gtk_tree_view_insert_column_with_attributes(view, -1, + _("Name"), renderer, + "text", COL_NAME, + "foreground-gdk", + COL_COLOR, NULL); + renderer = gtk_cell_renderer_text_new(); + gtk_tree_view_insert_column_with_attributes(view, -1, + "N", renderer, + "text", COL_NO, + "foreground-gdk", + COL_COLOR, NULL); + renderer = gtk_cell_renderer_text_new(); + gtk_tree_view_insert_column_with_attributes(view, -1, + "M", renderer, + "text", COL_MOD, + "foreground-gdk", + COL_COLOR, NULL); + renderer = gtk_cell_renderer_text_new(); + gtk_tree_view_insert_column_with_attributes(view, -1, + "Y", renderer, + "text", COL_YES, + "foreground-gdk", + COL_COLOR, NULL); + renderer = gtk_cell_renderer_text_new(); + gtk_tree_view_insert_column_with_attributes(view, -1, + _("Value"), renderer, + "text", COL_VALUE, + "editable", + COL_EDIT, + "foreground-gdk", + COL_COLOR, NULL); + g_signal_connect(G_OBJECT(renderer), "edited", + G_CALLBACK(renderer_edited), NULL); + + column = gtk_tree_view_get_column(view, COL_NAME); + gtk_tree_view_column_set_visible(column, show_name); + column = gtk_tree_view_get_column(view, COL_NO); + gtk_tree_view_column_set_visible(column, show_range); + column = gtk_tree_view_get_column(view, COL_MOD); + gtk_tree_view_column_set_visible(column, show_range); + column = gtk_tree_view_get_column(view, COL_YES); + gtk_tree_view_column_set_visible(column, show_range); + column = gtk_tree_view_get_column(view, COL_VALUE); + gtk_tree_view_column_set_visible(column, show_value); + + if (resizeable) { + for (i = 0; i < COL_VALUE; i++) { + column = gtk_tree_view_get_column(view, i); + gtk_tree_view_column_set_resizable(column, TRUE); + } + } + + sel = gtk_tree_view_get_selection(view); + gtk_tree_selection_set_mode(sel, GTK_SELECTION_SINGLE); +} + + +/* Utility Functions */ + + +static void text_insert_help(struct menu *menu) +{ + GtkTextBuffer *buffer; + GtkTextIter start, end; + const char *prompt = _(menu_get_prompt(menu)); + struct gstr help = str_new(); + + menu_get_ext_help(menu, &help); + + buffer = gtk_text_view_get_buffer(GTK_TEXT_VIEW(text_w)); + gtk_text_buffer_get_bounds(buffer, &start, &end); + gtk_text_buffer_delete(buffer, &start, &end); + gtk_text_view_set_left_margin(GTK_TEXT_VIEW(text_w), 15); + + gtk_text_buffer_get_end_iter(buffer, &end); + gtk_text_buffer_insert_with_tags(buffer, &end, prompt, -1, tag1, + NULL); + gtk_text_buffer_insert_at_cursor(buffer, "\n\n", 2); + gtk_text_buffer_get_end_iter(buffer, &end); + gtk_text_buffer_insert_with_tags(buffer, &end, str_get(&help), -1, tag2, + NULL); + str_free(&help); +} + + +static void text_insert_msg(const char *title, const char *message) +{ + GtkTextBuffer *buffer; + GtkTextIter start, end; + const char *msg = message; + + buffer = gtk_text_view_get_buffer(GTK_TEXT_VIEW(text_w)); + gtk_text_buffer_get_bounds(buffer, &start, &end); + gtk_text_buffer_delete(buffer, &start, &end); + gtk_text_view_set_left_margin(GTK_TEXT_VIEW(text_w), 15); + + gtk_text_buffer_get_end_iter(buffer, &end); + gtk_text_buffer_insert_with_tags(buffer, &end, title, -1, tag1, + NULL); + gtk_text_buffer_insert_at_cursor(buffer, "\n\n", 2); + gtk_text_buffer_get_end_iter(buffer, &end); + gtk_text_buffer_insert_with_tags(buffer, &end, msg, -1, tag2, + NULL); +} + + +/* Main Windows Callbacks */ + +void on_save_activate(GtkMenuItem * menuitem, gpointer user_data); +gboolean on_window1_delete_event(GtkWidget * widget, GdkEvent * event, + gpointer user_data) +{ + GtkWidget *dialog, *label; + gint result; + + if (!conf_get_changed()) + return FALSE; + + dialog = gtk_dialog_new_with_buttons(_("Warning !"), + GTK_WINDOW(main_wnd), + (GtkDialogFlags) + (GTK_DIALOG_MODAL | + GTK_DIALOG_DESTROY_WITH_PARENT), + GTK_STOCK_OK, + GTK_RESPONSE_YES, + GTK_STOCK_NO, + GTK_RESPONSE_NO, + GTK_STOCK_CANCEL, + GTK_RESPONSE_CANCEL, NULL); + gtk_dialog_set_default_response(GTK_DIALOG(dialog), + GTK_RESPONSE_CANCEL); + + label = gtk_label_new(_("\nSave configuration ?\n")); + gtk_container_add(GTK_CONTAINER(GTK_DIALOG(dialog)->vbox), label); + gtk_widget_show(label); + + result = gtk_dialog_run(GTK_DIALOG(dialog)); + switch (result) { + case GTK_RESPONSE_YES: + on_save_activate(NULL, NULL); + return FALSE; + case GTK_RESPONSE_NO: + return FALSE; + case GTK_RESPONSE_CANCEL: + case GTK_RESPONSE_DELETE_EVENT: + default: + gtk_widget_destroy(dialog); + return TRUE; + } + + return FALSE; +} + + +void on_window1_destroy(GtkObject * object, gpointer user_data) +{ + gtk_main_quit(); +} + + +void +on_window1_size_request(GtkWidget * widget, + GtkRequisition * requisition, gpointer user_data) +{ + static gint old_h; + gint w, h; + + if (widget->window == NULL) + gtk_window_get_default_size(GTK_WINDOW(main_wnd), &w, &h); + else + gdk_window_get_size(widget->window, &w, &h); + + if (h == old_h) + return; + old_h = h; + + gtk_paned_set_position(GTK_PANED(vpaned), 2 * h / 3); +} + + +/* Menu & Toolbar Callbacks */ + + +static void +load_filename(GtkFileSelection * file_selector, gpointer user_data) +{ + const gchar *fn; + + fn = gtk_file_selection_get_filename(GTK_FILE_SELECTION + (user_data)); + + if (conf_read(fn)) + text_insert_msg(_("Error"), _("Unable to load configuration !")); + else + display_tree(&rootmenu); +} + +void on_load1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkWidget *fs; + + fs = gtk_file_selection_new(_("Load file...")); + g_signal_connect(GTK_OBJECT(GTK_FILE_SELECTION(fs)->ok_button), + "clicked", + G_CALLBACK(load_filename), (gpointer) fs); + g_signal_connect_swapped(GTK_OBJECT + (GTK_FILE_SELECTION(fs)->ok_button), + "clicked", G_CALLBACK(gtk_widget_destroy), + (gpointer) fs); + g_signal_connect_swapped(GTK_OBJECT + (GTK_FILE_SELECTION(fs)->cancel_button), + "clicked", G_CALLBACK(gtk_widget_destroy), + (gpointer) fs); + gtk_widget_show(fs); +} + + +void on_save_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + if (conf_write(NULL)) + text_insert_msg(_("Error"), _("Unable to save configuration !")); +} + + +static void +store_filename(GtkFileSelection * file_selector, gpointer user_data) +{ + const gchar *fn; + + fn = gtk_file_selection_get_filename(GTK_FILE_SELECTION + (user_data)); + + if (conf_write(fn)) + text_insert_msg(_("Error"), _("Unable to save configuration !")); + + gtk_widget_destroy(GTK_WIDGET(user_data)); +} + +void on_save_as1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkWidget *fs; + + fs = gtk_file_selection_new(_("Save file as...")); + g_signal_connect(GTK_OBJECT(GTK_FILE_SELECTION(fs)->ok_button), + "clicked", + G_CALLBACK(store_filename), (gpointer) fs); + g_signal_connect_swapped(GTK_OBJECT + (GTK_FILE_SELECTION(fs)->ok_button), + "clicked", G_CALLBACK(gtk_widget_destroy), + (gpointer) fs); + g_signal_connect_swapped(GTK_OBJECT + (GTK_FILE_SELECTION(fs)->cancel_button), + "clicked", G_CALLBACK(gtk_widget_destroy), + (gpointer) fs); + gtk_widget_show(fs); +} + + +void on_quit1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + if (!on_window1_delete_event(NULL, NULL, NULL)) + gtk_widget_destroy(GTK_WIDGET(main_wnd)); +} + + +void on_show_name1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkTreeViewColumn *col; + + show_name = GTK_CHECK_MENU_ITEM(menuitem)->active; + col = gtk_tree_view_get_column(GTK_TREE_VIEW(tree2_w), COL_NAME); + if (col) + gtk_tree_view_column_set_visible(col, show_name); +} + + +void on_show_range1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkTreeViewColumn *col; + + show_range = GTK_CHECK_MENU_ITEM(menuitem)->active; + col = gtk_tree_view_get_column(GTK_TREE_VIEW(tree2_w), COL_NO); + if (col) + gtk_tree_view_column_set_visible(col, show_range); + col = gtk_tree_view_get_column(GTK_TREE_VIEW(tree2_w), COL_MOD); + if (col) + gtk_tree_view_column_set_visible(col, show_range); + col = gtk_tree_view_get_column(GTK_TREE_VIEW(tree2_w), COL_YES); + if (col) + gtk_tree_view_column_set_visible(col, show_range); + +} + + +void on_show_data1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkTreeViewColumn *col; + + show_value = GTK_CHECK_MENU_ITEM(menuitem)->active; + col = gtk_tree_view_get_column(GTK_TREE_VIEW(tree2_w), COL_VALUE); + if (col) + gtk_tree_view_column_set_visible(col, show_value); +} + + +void +on_set_option_mode1_activate(GtkMenuItem *menuitem, gpointer user_data) +{ + opt_mode = OPT_NORMAL; + gtk_tree_store_clear(tree2); + display_tree(&rootmenu); /* instead of update_tree to speed-up */ +} + + +void +on_set_option_mode2_activate(GtkMenuItem *menuitem, gpointer user_data) +{ + opt_mode = OPT_ALL; + gtk_tree_store_clear(tree2); + display_tree(&rootmenu); /* instead of update_tree to speed-up */ +} + + +void +on_set_option_mode3_activate(GtkMenuItem *menuitem, gpointer user_data) +{ + opt_mode = OPT_PROMPT; + gtk_tree_store_clear(tree2); + display_tree(&rootmenu); /* instead of update_tree to speed-up */ +} + + +void on_introduction1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkWidget *dialog; + const gchar *intro_text = _( + "Welcome to gkc, the GTK+ graphical configuration tool\n" + "For each option, a blank box indicates the feature is disabled, a\n" + "check indicates it is enabled, and a dot indicates that it is to\n" + "be compiled as a module. Clicking on the box will cycle through the three states.\n" + "\n" + "If you do not see an option (e.g., a device driver) that you\n" + "believe should be present, try turning on Show All Options\n" + "under the Options menu.\n" + "Although there is no cross reference yet to help you figure out\n" + "what other options must be enabled to support the option you\n" + "are interested in, you can still view the help of a grayed-out\n" + "option.\n" + "\n" + "Toggling Show Debug Info under the Options menu will show \n" + "the dependencies, which you can then match by examining other options."); + + dialog = gtk_message_dialog_new(GTK_WINDOW(main_wnd), + GTK_DIALOG_DESTROY_WITH_PARENT, + GTK_MESSAGE_INFO, + GTK_BUTTONS_CLOSE, "%s", intro_text); + g_signal_connect_swapped(GTK_OBJECT(dialog), "response", + G_CALLBACK(gtk_widget_destroy), + GTK_OBJECT(dialog)); + gtk_widget_show_all(dialog); +} + + +void on_about1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkWidget *dialog; + const gchar *about_text = + _("gkc is copyright (c) 2002 Romain Lievin .\n" + "Based on the source code from Roman Zippel.\n"); + + dialog = gtk_message_dialog_new(GTK_WINDOW(main_wnd), + GTK_DIALOG_DESTROY_WITH_PARENT, + GTK_MESSAGE_INFO, + GTK_BUTTONS_CLOSE, "%s", about_text); + g_signal_connect_swapped(GTK_OBJECT(dialog), "response", + G_CALLBACK(gtk_widget_destroy), + GTK_OBJECT(dialog)); + gtk_widget_show_all(dialog); +} + + +void on_license1_activate(GtkMenuItem * menuitem, gpointer user_data) +{ + GtkWidget *dialog; + const gchar *license_text = + _("gkc is released under the terms of the GNU GPL v2.\n" + "For more information, please see the source code or\n" + "visit http://www.fsf.org/licenses/licenses.html\n"); + + dialog = gtk_message_dialog_new(GTK_WINDOW(main_wnd), + GTK_DIALOG_DESTROY_WITH_PARENT, + GTK_MESSAGE_INFO, + GTK_BUTTONS_CLOSE, "%s", license_text); + g_signal_connect_swapped(GTK_OBJECT(dialog), "response", + G_CALLBACK(gtk_widget_destroy), + GTK_OBJECT(dialog)); + gtk_widget_show_all(dialog); +} + + +void on_back_clicked(GtkButton * button, gpointer user_data) +{ + enum prop_type ptype; + + current = current->parent; + ptype = current->prompt ? current->prompt->type : P_UNKNOWN; + if (ptype != P_MENU) + current = current->parent; + display_tree_part(); + + if (current == &rootmenu) + gtk_widget_set_sensitive(back_btn, FALSE); +} + + +void on_load_clicked(GtkButton * button, gpointer user_data) +{ + on_load1_activate(NULL, user_data); +} + + +void on_single_clicked(GtkButton * button, gpointer user_data) +{ + view_mode = SINGLE_VIEW; + gtk_widget_hide(tree1_w); + current = &rootmenu; + display_tree_part(); +} + + +void on_split_clicked(GtkButton * button, gpointer user_data) +{ + gint w, h; + view_mode = SPLIT_VIEW; + gtk_widget_show(tree1_w); + gtk_window_get_default_size(GTK_WINDOW(main_wnd), &w, &h); + gtk_paned_set_position(GTK_PANED(hpaned), w / 2); + if (tree2) + gtk_tree_store_clear(tree2); + display_list(); + + /* Disable back btn, like in full mode. */ + gtk_widget_set_sensitive(back_btn, FALSE); +} + + +void on_full_clicked(GtkButton * button, gpointer user_data) +{ + view_mode = FULL_VIEW; + gtk_widget_hide(tree1_w); + if (tree2) + gtk_tree_store_clear(tree2); + display_tree(&rootmenu); + gtk_widget_set_sensitive(back_btn, FALSE); +} + + +void on_collapse_clicked(GtkButton * button, gpointer user_data) +{ + gtk_tree_view_collapse_all(GTK_TREE_VIEW(tree2_w)); +} + + +void on_expand_clicked(GtkButton * button, gpointer user_data) +{ + gtk_tree_view_expand_all(GTK_TREE_VIEW(tree2_w)); +} + + +/* CTree Callbacks */ + +/* Change hex/int/string value in the cell */ +static void renderer_edited(GtkCellRendererText * cell, + const gchar * path_string, + const gchar * new_text, gpointer user_data) +{ + GtkTreePath *path = gtk_tree_path_new_from_string(path_string); + GtkTreeIter iter; + const char *old_def, *new_def; + struct menu *menu; + struct symbol *sym; + + if (!gtk_tree_model_get_iter(model2, &iter, path)) + return; + + gtk_tree_model_get(model2, &iter, COL_MENU, &menu, -1); + sym = menu->sym; + + gtk_tree_model_get(model2, &iter, COL_VALUE, &old_def, -1); + new_def = new_text; + + sym_set_string_value(sym, new_def); + + update_tree(&rootmenu, NULL); + + gtk_tree_path_free(path); +} + +/* Change the value of a symbol and update the tree */ +static void change_sym_value(struct menu *menu, gint col) +{ + struct symbol *sym = menu->sym; + tristate newval; + + if (!sym) + return; + + if (col == COL_NO) + newval = no; + else if (col == COL_MOD) + newval = mod; + else if (col == COL_YES) + newval = yes; + else + return; + + switch (sym_get_type(sym)) { + case S_BOOLEAN: + case S_TRISTATE: + if (!sym_tristate_within_range(sym, newval)) + newval = yes; + sym_set_tristate_value(sym, newval); + if (view_mode == FULL_VIEW) + update_tree(&rootmenu, NULL); + else if (view_mode == SPLIT_VIEW) { + update_tree(browsed, NULL); + display_list(); + } + else if (view_mode == SINGLE_VIEW) + display_tree_part(); //fixme: keep exp/coll + break; + case S_INT: + case S_HEX: + case S_STRING: + default: + break; + } +} + +static void toggle_sym_value(struct menu *menu) +{ + if (!menu->sym) + return; + + sym_toggle_tristate_value(menu->sym); + if (view_mode == FULL_VIEW) + update_tree(&rootmenu, NULL); + else if (view_mode == SPLIT_VIEW) { + update_tree(browsed, NULL); + display_list(); + } + else if (view_mode == SINGLE_VIEW) + display_tree_part(); //fixme: keep exp/coll +} + +static gint column2index(GtkTreeViewColumn * column) +{ + gint i; + + for (i = 0; i < COL_NUMBER; i++) { + GtkTreeViewColumn *col; + + col = gtk_tree_view_get_column(GTK_TREE_VIEW(tree2_w), i); + if (col == column) + return i; + } + + return -1; +} + + +/* User click: update choice (full) or goes down (single) */ +gboolean +on_treeview2_button_press_event(GtkWidget * widget, + GdkEventButton * event, gpointer user_data) +{ + GtkTreeView *view = GTK_TREE_VIEW(widget); + GtkTreePath *path; + GtkTreeViewColumn *column; + GtkTreeIter iter; + struct menu *menu; + gint col; + +#if GTK_CHECK_VERSION(2,1,4) // bug in ctree with earlier version of GTK + gint tx = (gint) event->x; + gint ty = (gint) event->y; + gint cx, cy; + + gtk_tree_view_get_path_at_pos(view, tx, ty, &path, &column, &cx, + &cy); +#else + gtk_tree_view_get_cursor(view, &path, &column); +#endif + if (path == NULL) + return FALSE; + + if (!gtk_tree_model_get_iter(model2, &iter, path)) + return FALSE; + gtk_tree_model_get(model2, &iter, COL_MENU, &menu, -1); + + col = column2index(column); + if (event->type == GDK_2BUTTON_PRESS) { + enum prop_type ptype; + ptype = menu->prompt ? menu->prompt->type : P_UNKNOWN; + + if (ptype == P_MENU && view_mode != FULL_VIEW && col == COL_OPTION) { + // goes down into menu + current = menu; + display_tree_part(); + gtk_widget_set_sensitive(back_btn, TRUE); + } else if ((col == COL_OPTION)) { + toggle_sym_value(menu); + gtk_tree_view_expand_row(view, path, TRUE); + } + } else { + if (col == COL_VALUE) { + toggle_sym_value(menu); + gtk_tree_view_expand_row(view, path, TRUE); + } else if (col == COL_NO || col == COL_MOD + || col == COL_YES) { + change_sym_value(menu, col); + gtk_tree_view_expand_row(view, path, TRUE); + } + } + + return FALSE; +} + +/* Key pressed: update choice */ +gboolean +on_treeview2_key_press_event(GtkWidget * widget, + GdkEventKey * event, gpointer user_data) +{ + GtkTreeView *view = GTK_TREE_VIEW(widget); + GtkTreePath *path; + GtkTreeViewColumn *column; + GtkTreeIter iter; + struct menu *menu; + gint col; + + gtk_tree_view_get_cursor(view, &path, &column); + if (path == NULL) + return FALSE; + + if (event->keyval == GDK_space) { + if (gtk_tree_view_row_expanded(view, path)) + gtk_tree_view_collapse_row(view, path); + else + gtk_tree_view_expand_row(view, path, FALSE); + return TRUE; + } + if (event->keyval == GDK_KP_Enter) { + } + if (widget == tree1_w) + return FALSE; + + gtk_tree_model_get_iter(model2, &iter, path); + gtk_tree_model_get(model2, &iter, COL_MENU, &menu, -1); + + if (!strcasecmp(event->string, "n")) + col = COL_NO; + else if (!strcasecmp(event->string, "m")) + col = COL_MOD; + else if (!strcasecmp(event->string, "y")) + col = COL_YES; + else + col = -1; + change_sym_value(menu, col); + + return FALSE; +} + + +/* Row selection changed: update help */ +void +on_treeview2_cursor_changed(GtkTreeView * treeview, gpointer user_data) +{ + GtkTreeSelection *selection; + GtkTreeIter iter; + struct menu *menu; + + selection = gtk_tree_view_get_selection(treeview); + if (gtk_tree_selection_get_selected(selection, &model2, &iter)) { + gtk_tree_model_get(model2, &iter, COL_MENU, &menu, -1); + text_insert_help(menu); + } +} + + +/* User click: display sub-tree in the right frame. */ +gboolean +on_treeview1_button_press_event(GtkWidget * widget, + GdkEventButton * event, gpointer user_data) +{ + GtkTreeView *view = GTK_TREE_VIEW(widget); + GtkTreePath *path; + GtkTreeViewColumn *column; + GtkTreeIter iter; + struct menu *menu; + + gint tx = (gint) event->x; + gint ty = (gint) event->y; + gint cx, cy; + + gtk_tree_view_get_path_at_pos(view, tx, ty, &path, &column, &cx, + &cy); + if (path == NULL) + return FALSE; + + gtk_tree_model_get_iter(model1, &iter, path); + gtk_tree_model_get(model1, &iter, COL_MENU, &menu, -1); + + if (event->type == GDK_2BUTTON_PRESS) { + toggle_sym_value(menu); + current = menu; + display_tree_part(); + } else { + browsed = menu; + display_tree_part(); + } + + gtk_widget_realize(tree2_w); + gtk_tree_view_set_cursor(view, path, NULL, FALSE); + gtk_widget_grab_focus(tree2_w); + + return FALSE; +} + + +/* Fill a row of strings */ +static gchar **fill_row(struct menu *menu) +{ + static gchar *row[COL_NUMBER]; + struct symbol *sym = menu->sym; + const char *def; + int stype; + tristate val; + enum prop_type ptype; + int i; + + for (i = COL_OPTION; i <= COL_COLOR; i++) + g_free(row[i]); + bzero(row, sizeof(row)); + + row[COL_OPTION] = + g_strdup_printf("%s %s", _(menu_get_prompt(menu)), + sym && !sym_has_value(sym) ? "(NEW)" : ""); + + if (opt_mode == OPT_ALL && !menu_is_visible(menu)) + row[COL_COLOR] = g_strdup("DarkGray"); + else if (opt_mode == OPT_PROMPT && + menu_has_prompt(menu) && !menu_is_visible(menu)) + row[COL_COLOR] = g_strdup("DarkGray"); + else + row[COL_COLOR] = g_strdup("Black"); + + ptype = menu->prompt ? menu->prompt->type : P_UNKNOWN; + switch (ptype) { + case P_MENU: + row[COL_PIXBUF] = (gchar *) xpm_menu; + if (view_mode == SINGLE_VIEW) + row[COL_PIXVIS] = GINT_TO_POINTER(TRUE); + row[COL_BTNVIS] = GINT_TO_POINTER(FALSE); + break; + case P_COMMENT: + row[COL_PIXBUF] = (gchar *) xpm_void; + row[COL_PIXVIS] = GINT_TO_POINTER(FALSE); + row[COL_BTNVIS] = GINT_TO_POINTER(FALSE); + break; + default: + row[COL_PIXBUF] = (gchar *) xpm_void; + row[COL_PIXVIS] = GINT_TO_POINTER(FALSE); + row[COL_BTNVIS] = GINT_TO_POINTER(TRUE); + break; + } + + if (!sym) + return row; + row[COL_NAME] = g_strdup(sym->name); + + sym_calc_value(sym); + sym->flags &= ~SYMBOL_CHANGED; + + if (sym_is_choice(sym)) { // parse childs for getting final value + struct menu *child; + struct symbol *def_sym = sym_get_choice_value(sym); + struct menu *def_menu = NULL; + + row[COL_BTNVIS] = GINT_TO_POINTER(FALSE); + + for (child = menu->list; child; child = child->next) { + if (menu_is_visible(child) + && child->sym == def_sym) + def_menu = child; + } + + if (def_menu) + row[COL_VALUE] = + g_strdup(_(menu_get_prompt(def_menu))); + } + if (sym->flags & SYMBOL_CHOICEVAL) + row[COL_BTNRAD] = GINT_TO_POINTER(TRUE); + + stype = sym_get_type(sym); + switch (stype) { + case S_BOOLEAN: + if (GPOINTER_TO_INT(row[COL_PIXVIS]) == FALSE) + row[COL_BTNVIS] = GINT_TO_POINTER(TRUE); + if (sym_is_choice(sym)) + break; + /* fall through */ + case S_TRISTATE: + val = sym_get_tristate_value(sym); + switch (val) { + case no: + row[COL_NO] = g_strdup("N"); + row[COL_VALUE] = g_strdup("N"); + row[COL_BTNACT] = GINT_TO_POINTER(FALSE); + row[COL_BTNINC] = GINT_TO_POINTER(FALSE); + break; + case mod: + row[COL_MOD] = g_strdup("M"); + row[COL_VALUE] = g_strdup("M"); + row[COL_BTNINC] = GINT_TO_POINTER(TRUE); + break; + case yes: + row[COL_YES] = g_strdup("Y"); + row[COL_VALUE] = g_strdup("Y"); + row[COL_BTNACT] = GINT_TO_POINTER(TRUE); + row[COL_BTNINC] = GINT_TO_POINTER(FALSE); + break; + } + + if (val != no && sym_tristate_within_range(sym, no)) + row[COL_NO] = g_strdup("_"); + if (val != mod && sym_tristate_within_range(sym, mod)) + row[COL_MOD] = g_strdup("_"); + if (val != yes && sym_tristate_within_range(sym, yes)) + row[COL_YES] = g_strdup("_"); + break; + case S_INT: + case S_HEX: + case S_STRING: + def = sym_get_string_value(sym); + row[COL_VALUE] = g_strdup(def); + row[COL_EDIT] = GINT_TO_POINTER(TRUE); + row[COL_BTNVIS] = GINT_TO_POINTER(FALSE); + break; + } + + return row; +} + + +/* Set the node content with a row of strings */ +static void set_node(GtkTreeIter * node, struct menu *menu, gchar ** row) +{ + GdkColor color; + gboolean success; + GdkPixbuf *pix; + + pix = gdk_pixbuf_new_from_xpm_data((const char **) + row[COL_PIXBUF]); + + gdk_color_parse(row[COL_COLOR], &color); + gdk_colormap_alloc_colors(gdk_colormap_get_system(), &color, 1, + FALSE, FALSE, &success); + + gtk_tree_store_set(tree, node, + COL_OPTION, row[COL_OPTION], + COL_NAME, row[COL_NAME], + COL_NO, row[COL_NO], + COL_MOD, row[COL_MOD], + COL_YES, row[COL_YES], + COL_VALUE, row[COL_VALUE], + COL_MENU, (gpointer) menu, + COL_COLOR, &color, + COL_EDIT, GPOINTER_TO_INT(row[COL_EDIT]), + COL_PIXBUF, pix, + COL_PIXVIS, GPOINTER_TO_INT(row[COL_PIXVIS]), + COL_BTNVIS, GPOINTER_TO_INT(row[COL_BTNVIS]), + COL_BTNACT, GPOINTER_TO_INT(row[COL_BTNACT]), + COL_BTNINC, GPOINTER_TO_INT(row[COL_BTNINC]), + COL_BTNRAD, GPOINTER_TO_INT(row[COL_BTNRAD]), + -1); + + g_object_unref(pix); +} + + +/* Add a node to the tree */ +static void place_node(struct menu *menu, char **row) +{ + GtkTreeIter *parent = parents[indent - 1]; + GtkTreeIter *node = parents[indent]; + + gtk_tree_store_append(tree, node, parent); + set_node(node, menu, row); +} + + +/* Find a node in the GTK+ tree */ +static GtkTreeIter found; + +/* + * Find a menu in the GtkTree starting at parent. + */ +GtkTreeIter *gtktree_iter_find_node(GtkTreeIter * parent, + struct menu *tofind) +{ + GtkTreeIter iter; + GtkTreeIter *child = &iter; + gboolean valid; + GtkTreeIter *ret; + + valid = gtk_tree_model_iter_children(model2, child, parent); + while (valid) { + struct menu *menu; + + gtk_tree_model_get(model2, child, 6, &menu, -1); + + if (menu == tofind) { + memcpy(&found, child, sizeof(GtkTreeIter)); + return &found; + } + + ret = gtktree_iter_find_node(child, tofind); + if (ret) + return ret; + + valid = gtk_tree_model_iter_next(model2, child); + } + + return NULL; +} + + +/* + * Update the tree by adding/removing entries + * Does not change other nodes + */ +static void update_tree(struct menu *src, GtkTreeIter * dst) +{ + struct menu *child1; + GtkTreeIter iter, tmp; + GtkTreeIter *child2 = &iter; + gboolean valid; + GtkTreeIter *sibling; + struct symbol *sym; + struct menu *menu1, *menu2; + + if (src == &rootmenu) + indent = 1; + + valid = gtk_tree_model_iter_children(model2, child2, dst); + for (child1 = src->list; child1; child1 = child1->next) { + + sym = child1->sym; + + reparse: + menu1 = child1; + if (valid) + gtk_tree_model_get(model2, child2, COL_MENU, + &menu2, -1); + else + menu2 = NULL; // force adding of a first child + +#ifdef CONFIG_DEBUG + printf("%*c%s | %s\n", indent, ' ', + menu1 ? menu_get_prompt(menu1) : "nil", + menu2 ? menu_get_prompt(menu2) : "nil"); +#endif + + if ((opt_mode == OPT_NORMAL && !menu_is_visible(child1)) || + (opt_mode == OPT_PROMPT && !menu_has_prompt(child1)) || + (opt_mode == OPT_ALL && !menu_get_prompt(child1))) { + + /* remove node */ + if (gtktree_iter_find_node(dst, menu1) != NULL) { + memcpy(&tmp, child2, sizeof(GtkTreeIter)); + valid = gtk_tree_model_iter_next(model2, + child2); + gtk_tree_store_remove(tree2, &tmp); + if (!valid) + return; /* next parent */ + else + goto reparse; /* next child */ + } else + continue; + } + + if (menu1 != menu2) { + if (gtktree_iter_find_node(dst, menu1) == NULL) { // add node + if (!valid && !menu2) + sibling = NULL; + else + sibling = child2; + gtk_tree_store_insert_before(tree2, + child2, + dst, sibling); + set_node(child2, menu1, fill_row(menu1)); + if (menu2 == NULL) + valid = TRUE; + } else { // remove node + memcpy(&tmp, child2, sizeof(GtkTreeIter)); + valid = gtk_tree_model_iter_next(model2, + child2); + gtk_tree_store_remove(tree2, &tmp); + if (!valid) + return; // next parent + else + goto reparse; // next child + } + } else if (sym && (sym->flags & SYMBOL_CHANGED)) { + set_node(child2, menu1, fill_row(menu1)); + } + + indent++; + update_tree(child1, child2); + indent--; + + valid = gtk_tree_model_iter_next(model2, child2); + } +} + + +/* Display the whole tree (single/split/full view) */ +static void display_tree(struct menu *menu) +{ + struct symbol *sym; + struct property *prop; + struct menu *child; + enum prop_type ptype; + + if (menu == &rootmenu) { + indent = 1; + current = &rootmenu; + } + + for (child = menu->list; child; child = child->next) { + prop = child->prompt; + sym = child->sym; + ptype = prop ? prop->type : P_UNKNOWN; + + if (sym) + sym->flags &= ~SYMBOL_CHANGED; + + if ((view_mode == SPLIT_VIEW) + && !(child->flags & MENU_ROOT) && (tree == tree1)) + continue; + + if ((view_mode == SPLIT_VIEW) && (child->flags & MENU_ROOT) + && (tree == tree2)) + continue; + + if ((opt_mode == OPT_NORMAL && menu_is_visible(child)) || + (opt_mode == OPT_PROMPT && menu_has_prompt(child)) || + (opt_mode == OPT_ALL && menu_get_prompt(child))) + place_node(child, fill_row(child)); +#ifdef CONFIG_DEBUG + printf("%*c%s: ", indent, ' ', menu_get_prompt(child)); + printf("%s", child->flags & MENU_ROOT ? "rootmenu | " : ""); + printf("%s", prop_get_type_name(ptype)); + printf(" | "); + if (sym) { + printf("%s", sym_type_name(sym->type)); + printf(" | "); + printf("%s", dbg_sym_flags(sym->flags)); + printf("\n"); + } else + printf("\n"); +#endif + if ((view_mode != FULL_VIEW) && (ptype == P_MENU) + && (tree == tree2)) + continue; +/* + if (((menu != &rootmenu) && !(menu->flags & MENU_ROOT)) + || (view_mode == FULL_VIEW) + || (view_mode == SPLIT_VIEW))*/ + + /* Change paned position if the view is not in 'split mode' */ + if (view_mode == SINGLE_VIEW || view_mode == FULL_VIEW) { + gtk_paned_set_position(GTK_PANED(hpaned), 0); + } + + if (((view_mode == SINGLE_VIEW) && (menu->flags & MENU_ROOT)) + || (view_mode == FULL_VIEW) + || (view_mode == SPLIT_VIEW)) { + indent++; + display_tree(child); + indent--; + } + } +} + +/* Display a part of the tree starting at current node (single/split view) */ +static void display_tree_part(void) +{ + if (tree2) + gtk_tree_store_clear(tree2); + if (view_mode == SINGLE_VIEW) + display_tree(current); + else if (view_mode == SPLIT_VIEW) + display_tree(browsed); + gtk_tree_view_expand_all(GTK_TREE_VIEW(tree2_w)); +} + +/* Display the list in the left frame (split view) */ +static void display_list(void) +{ + if (tree1) + gtk_tree_store_clear(tree1); + + tree = tree1; + display_tree(&rootmenu); + gtk_tree_view_expand_all(GTK_TREE_VIEW(tree1_w)); + tree = tree2; +} + +void fixup_rootmenu(struct menu *menu) +{ + struct menu *child; + static int menu_cnt = 0; + + menu->flags |= MENU_ROOT; + for (child = menu->list; child; child = child->next) { + if (child->prompt && child->prompt->type == P_MENU) { + menu_cnt++; + fixup_rootmenu(child); + menu_cnt--; + } else if (!menu_cnt) + fixup_rootmenu(child); + } +} + + +/* Main */ +int main(int ac, char *av[]) +{ + const char *name; + char *env; + gchar *glade_file; + + bindtextdomain(PACKAGE, LOCALEDIR); + bind_textdomain_codeset(PACKAGE, "UTF-8"); + textdomain(PACKAGE); + + /* GTK stuffs */ + gtk_set_locale(); + gtk_init(&ac, &av); + glade_init(); + + //add_pixmap_directory (PACKAGE_DATA_DIR "/" PACKAGE "/pixmaps"); + //add_pixmap_directory (PACKAGE_SOURCE_DIR "/pixmaps"); + + /* Determine GUI path */ + env = getenv(SRCTREE); + if (env) + glade_file = g_strconcat(env, "/scripts/kconfig/gconf.glade", NULL); + else if (av[0][0] == '/') + glade_file = g_strconcat(av[0], ".glade", NULL); + else + glade_file = g_strconcat(g_get_current_dir(), "/", av[0], ".glade", NULL); + + /* Conf stuffs */ + if (ac > 1 && av[1][0] == '-') { + switch (av[1][1]) { + case 'a': + //showAll = 1; + break; + case 's': + conf_set_message_callback(NULL); + break; + case 'h': + case '?': + printf("%s [-s] \n", av[0]); + exit(0); + } + name = av[2]; + } else + name = av[1]; + + conf_parse(name); + fixup_rootmenu(&rootmenu); + conf_read(NULL); + + /* Load the interface and connect signals */ + init_main_window(glade_file); + init_tree_model(); + init_left_tree(); + init_right_tree(); + + switch (view_mode) { + case SINGLE_VIEW: + display_tree_part(); + break; + case SPLIT_VIEW: + display_list(); + break; + case FULL_VIEW: + display_tree(&rootmenu); + break; + } + + gtk_main(); + + return 0; +} + +static void conf_changed(void) +{ + bool changed = conf_get_changed(); + gtk_widget_set_sensitive(save_btn, changed); + gtk_widget_set_sensitive(save_menu_item, changed); +} diff --git a/scripts/kconfig/gconf.glade b/scripts/kconfig/gconf.glade new file mode 100644 index 0000000000..aa483cb327 --- /dev/null +++ b/scripts/kconfig/gconf.glade @@ -0,0 +1,661 @@ + + + + + + True + Gtk Kernel Configurator + GTK_WINDOW_TOPLEVEL + GTK_WIN_POS_NONE + False + 640 + 480 + True + False + True + False + False + GDK_WINDOW_TYPE_HINT_NORMAL + GDK_GRAVITY_NORTH_WEST + + + + + + + True + False + 0 + + + + True + + + + True + _File + True + + + + + + + True + Load a config file + _Load + True + + + + + + True + gtk-open + 1 + 0.5 + 0.5 + 0 + 0 + + + + + + + + True + Save the config in .config + _Save + True + + + + + + True + gtk-save + 1 + 0.5 + 0.5 + 0 + 0 + + + + + + + + True + Save the config in a file + Save _as + True + + + + + True + gtk-save-as + 1 + 0.5 + 0.5 + 0 + 0 + + + + + + + + True + + + + + + True + _Quit + True + + + + + + True + gtk-quit + 1 + 0.5 + 0.5 + 0 + 0 + + + + + + + + + + + + True + _Options + True + + + + + + + True + Show name + Show _name + True + False + + + + + + + True + Show range (Y/M/N) + Show _range + True + False + + + + + + + True + Show value of the option + Show _data + True + False + + + + + + + True + + + + + + True + Show normal options + Show normal options + True + True + + + + + + + True + Show all options + Show all _options + True + False + set_option_mode1 + + + + + + + True + Show all options with prompts + Show all prompt options + True + False + set_option_mode1 + + + + + + + + + + + + True + _Help + True + + + + + + + True + _Introduction + True + + + + + + True + gtk-dialog-question + 1 + 0.5 + 0.5 + 0 + 0 + + + + + + + + True + _About + True + + + + + + True + gtk-properties + 1 + 0.5 + 0.5 + 0 + 0 + + + + + + + + True + _License + True + + + + + True + gtk-justify-fill + 1 + 0.5 + 0.5 + 0 + 0 + + + + + + + + + + + 0 + False + False + + + + + + True + GTK_SHADOW_OUT + GTK_POS_LEFT + GTK_POS_TOP + + + + True + GTK_ORIENTATION_HORIZONTAL + GTK_TOOLBAR_BOTH + True + True + + + + True + Goes up of one level (single view) + Back + True + gtk-undo + True + True + False + + + + False + True + + + + + + True + True + True + False + + + + True + + + + + False + False + + + + + + True + Load a config file + Load + True + gtk-open + True + True + False + + + + False + True + + + + + + True + Save a config file + Save + True + gtk-save + True + True + False + + + + False + True + + + + + + True + True + True + False + + + + True + + + + + False + False + + + + + + True + Single view + Single + True + gtk-missing-image + True + True + False + + + + False + True + + + + + + True + Split view + Split + True + gtk-missing-image + True + True + False + + + + False + True + + + + + + True + Full view + Full + True + gtk-missing-image + True + True + False + + + + False + True + + + + + + True + True + True + False + + + + True + + + + + False + False + + + + + + True + Collapse the whole tree in the right frame + Collapse + True + gtk-remove + True + True + False + + + + False + True + + + + + + True + Expand the whole tree in the right frame + Expand + True + gtk-add + True + True + False + + + + False + True + + + + + + + 0 + False + False + + + + + + 1 + True + True + 0 + + + + True + GTK_POLICY_AUTOMATIC + GTK_POLICY_AUTOMATIC + GTK_SHADOW_IN + GTK_CORNER_TOP_LEFT + + + + True + True + True + False + False + False + + + + + + + + True + False + + + + + + True + True + 0 + + + + True + GTK_POLICY_AUTOMATIC + GTK_POLICY_AUTOMATIC + GTK_SHADOW_IN + GTK_CORNER_TOP_LEFT + + + + True + True + True + True + False + False + False + + + + + + + + True + False + + + + + + True + GTK_POLICY_NEVER + GTK_POLICY_AUTOMATIC + GTK_SHADOW_IN + GTK_CORNER_TOP_LEFT + + + + True + True + False + False + True + GTK_JUSTIFY_LEFT + GTK_WRAP_WORD + True + 0 + 0 + 0 + 0 + 0 + 0 + Sorry, no help available for this option yet. + + + + + True + True + + + + + True + True + + + + + 0 + True + True + + + + + + + diff --git a/scripts/kconfig/images.c b/scripts/kconfig/images.c new file mode 100644 index 0000000000..d4f84bd4a9 --- /dev/null +++ b/scripts/kconfig/images.c @@ -0,0 +1,326 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +static const char *xpm_load[] = { +"22 22 5 1", +". c None", +"# c #000000", +"c c #838100", +"a c #ffff00", +"b c #ffffff", +"......................", +"......................", +"......................", +"............####....#.", +"...........#....##.##.", +"..................###.", +".................####.", +".####...........#####.", +"#abab##########.......", +"#babababababab#.......", +"#ababababababa#.......", +"#babababababab#.......", +"#ababab###############", +"#babab##cccccccccccc##", +"#abab##cccccccccccc##.", +"#bab##cccccccccccc##..", +"#ab##cccccccccccc##...", +"#b##cccccccccccc##....", +"###cccccccccccc##.....", +"##cccccccccccc##......", +"###############.......", +"......................"}; + +static const char *xpm_save[] = { +"22 22 5 1", +". c None", +"# c #000000", +"a c #838100", +"b c #c5c2c5", +"c c #cdb6d5", +"......................", +".####################.", +".#aa#bbbbbbbbbbbb#bb#.", +".#aa#bbbbbbbbbbbb#bb#.", +".#aa#bbbbbbbbbcbb####.", +".#aa#bbbccbbbbbbb#aa#.", +".#aa#bbbccbbbbbbb#aa#.", +".#aa#bbbbbbbbbbbb#aa#.", +".#aa#bbbbbbbbbbbb#aa#.", +".#aa#bbbbbbbbbbbb#aa#.", +".#aa#bbbbbbbbbbbb#aa#.", +".#aaa############aaa#.", +".#aaaaaaaaaaaaaaaaaa#.", +".#aaaaaaaaaaaaaaaaaa#.", +".#aaa#############aa#.", +".#aaa#########bbb#aa#.", +".#aaa#########bbb#aa#.", +".#aaa#########bbb#aa#.", +".#aaa#########bbb#aa#.", +".#aaa#########bbb#aa#.", +"..##################..", +"......................"}; + +static const char *xpm_back[] = { +"22 22 3 1", +". c None", +"# c #000083", +"a c #838183", +"......................", +"......................", +"......................", +"......................", +"......................", +"...........######a....", +"..#......##########...", +"..##...####......##a..", +"..###.###.........##..", +"..######..........##..", +"..#####...........##..", +"..######..........##..", +"..#######.........##..", +"..########.......##a..", +"...............a###...", +"...............###....", +"......................", +"......................", +"......................", +"......................", +"......................", +"......................"}; + +static const char *xpm_tree_view[] = { +"22 22 2 1", +". c None", +"# c #000000", +"......................", +"......................", +"......#...............", +"......#...............", +"......#...............", +"......#...............", +"......#...............", +"......########........", +"......#...............", +"......#...............", +"......#...............", +"......#...............", +"......#...............", +"......########........", +"......#...............", +"......#...............", +"......#...............", +"......#...............", +"......#...............", +"......########........", +"......................", +"......................"}; + +static const char *xpm_single_view[] = { +"22 22 2 1", +". c None", +"# c #000000", +"......................", +"......................", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"..........#...........", +"......................", +"......................"}; + +static const char *xpm_split_view[] = { +"22 22 2 1", +". c None", +"# c #000000", +"......................", +"......................", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......#......#........", +"......................", +"......................"}; + +static const char *xpm_symbol_no[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .......... ", +" . . ", +" . . ", +" . . ", +" . . ", +" . . ", +" . . ", +" . . ", +" . . ", +" .......... ", +" "}; + +static const char *xpm_symbol_mod[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .......... ", +" . . ", +" . . ", +" . .. . ", +" . .... . ", +" . .... . ", +" . .. . ", +" . . ", +" . . ", +" .......... ", +" "}; + +static const char *xpm_symbol_yes[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .......... ", +" . . ", +" . . ", +" . . . ", +" . .. . ", +" . . .. . ", +" . .... . ", +" . .. . ", +" . . ", +" .......... ", +" "}; + +static const char *xpm_choice_no[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .... ", +" .. .. ", +" . . ", +" . . ", +" . . ", +" . . ", +" . . ", +" . . ", +" .. .. ", +" .... ", +" "}; + +static const char *xpm_choice_yes[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .... ", +" .. .. ", +" . . ", +" . .. . ", +" . .... . ", +" . .... . ", +" . .. . ", +" . . ", +" .. .. ", +" .... ", +" "}; + +static const char *xpm_menu[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .......... ", +" . . ", +" . .. . ", +" . .... . ", +" . ...... . ", +" . ...... . ", +" . .... . ", +" . .. . ", +" . . ", +" .......... ", +" "}; + +static const char *xpm_menu_inv[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .......... ", +" .......... ", +" .. ...... ", +" .. .... ", +" .. .. ", +" .. .. ", +" .. .... ", +" .. ...... ", +" .......... ", +" .......... ", +" "}; + +static const char *xpm_menuback[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" .......... ", +" . . ", +" . .. . ", +" . .... . ", +" . ...... . ", +" . ...... . ", +" . .... . ", +" . .. . ", +" . . ", +" .......... ", +" "}; + +static const char *xpm_void[] = { +"12 12 2 1", +" c white", +". c black", +" ", +" ", +" ", +" ", +" ", +" ", +" ", +" ", +" ", +" ", +" ", +" "}; diff --git a/scripts/kconfig/kxgettext.c b/scripts/kconfig/kxgettext.c new file mode 100644 index 0000000000..2858738b22 --- /dev/null +++ b/scripts/kconfig/kxgettext.c @@ -0,0 +1,235 @@ +/* + * Arnaldo Carvalho de Melo , 2005 + * + * Released under the terms of the GNU GPL v2.0 + */ + +#include +#include + +#include "lkc.h" + +static char *escape(const char* text, char *bf, int len) +{ + char *bfp = bf; + int multiline = strchr(text, '\n') != NULL; + int eol = 0; + int textlen = strlen(text); + + if ((textlen > 0) && (text[textlen-1] == '\n')) + eol = 1; + + *bfp++ = '"'; + --len; + + if (multiline) { + *bfp++ = '"'; + *bfp++ = '\n'; + *bfp++ = '"'; + len -= 3; + } + + while (*text != '\0' && len > 1) { + if (*text == '"') + *bfp++ = '\\'; + else if (*text == '\n') { + *bfp++ = '\\'; + *bfp++ = 'n'; + *bfp++ = '"'; + *bfp++ = '\n'; + *bfp++ = '"'; + len -= 5; + ++text; + goto next; + } + else if (*text == '\\') { + *bfp++ = '\\'; + len--; + } + *bfp++ = *text++; +next: + --len; + } + + if (multiline && eol) + bfp -= 3; + + *bfp++ = '"'; + *bfp = '\0'; + + return bf; +} + +struct file_line { + struct file_line *next; + const char *file; + int lineno; +}; + +static struct file_line *file_line__new(const char *file, int lineno) +{ + struct file_line *self = malloc(sizeof(*self)); + + if (self == NULL) + goto out; + + self->file = file; + self->lineno = lineno; + self->next = NULL; +out: + return self; +} + +struct message { + const char *msg; + const char *option; + struct message *next; + struct file_line *files; +}; + +static struct message *message__list; + +static struct message *message__new(const char *msg, char *option, + const char *file, int lineno) +{ + struct message *self = malloc(sizeof(*self)); + + if (self == NULL) + goto out; + + self->files = file_line__new(file, lineno); + if (self->files == NULL) + goto out_fail; + + self->msg = strdup(msg); + if (self->msg == NULL) + goto out_fail_msg; + + self->option = option; + self->next = NULL; +out: + return self; +out_fail_msg: + free(self->files); +out_fail: + free(self); + self = NULL; + goto out; +} + +static struct message *mesage__find(const char *msg) +{ + struct message *m = message__list; + + while (m != NULL) { + if (strcmp(m->msg, msg) == 0) + break; + m = m->next; + } + + return m; +} + +static int message__add_file_line(struct message *self, const char *file, + int lineno) +{ + int rc = -1; + struct file_line *fl = file_line__new(file, lineno); + + if (fl == NULL) + goto out; + + fl->next = self->files; + self->files = fl; + rc = 0; +out: + return rc; +} + +static int message__add(const char *msg, char *option, const char *file, + int lineno) +{ + int rc = 0; + char bf[16384]; + char *escaped = escape(msg, bf, sizeof(bf)); + struct message *m = mesage__find(escaped); + + if (m != NULL) + rc = message__add_file_line(m, file, lineno); + else { + m = message__new(escaped, option, file, lineno); + + if (m != NULL) { + m->next = message__list; + message__list = m; + } else + rc = -1; + } + return rc; +} + +static void menu_build_message_list(struct menu *menu) +{ + struct menu *child; + + message__add(menu_get_prompt(menu), NULL, + menu->file == NULL ? "Root Menu" : menu->file->name, + menu->lineno); + + if (menu->sym != NULL && menu_has_help(menu)) + message__add(menu_get_help(menu), menu->sym->name, + menu->file == NULL ? "Root Menu" : menu->file->name, + menu->lineno); + + for (child = menu->list; child != NULL; child = child->next) + if (child->prompt != NULL) + menu_build_message_list(child); +} + +static void message__print_file_lineno(struct message *self) +{ + struct file_line *fl = self->files; + + putchar('\n'); + if (self->option != NULL) + printf("# %s:00000\n", self->option); + + printf("#: %s:%d", fl->file, fl->lineno); + fl = fl->next; + + while (fl != NULL) { + printf(", %s:%d", fl->file, fl->lineno); + fl = fl->next; + } + + putchar('\n'); +} + +static void message__print_gettext_msgid_msgstr(struct message *self) +{ + message__print_file_lineno(self); + + printf("msgid %s\n" + "msgstr \"\"\n", self->msg); +} + +static void menu__xgettext(void) +{ + struct message *m = message__list; + + while (m != NULL) { + /* skip empty lines ("") */ + if (strlen(m->msg) > sizeof("\"\"")) + message__print_gettext_msgid_msgstr(m); + m = m->next; + } +} + +int main(int ac, char **av) +{ + conf_parse(av[1]); + + menu_build_message_list(menu_get_root_menu(NULL)); + menu__xgettext(); + return 0; +} diff --git a/scripts/kconfig/list.h b/scripts/kconfig/list.h new file mode 100644 index 0000000000..2cf23f002d --- /dev/null +++ b/scripts/kconfig/list.h @@ -0,0 +1,131 @@ +#ifndef LIST_H +#define LIST_H + +/* + * Copied from include/linux/... + */ + +#undef offsetof +#define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER) + +/** + * container_of - cast a member of a structure out to the containing structure + * @ptr: the pointer to the member. + * @type: the type of the container struct this is embedded in. + * @member: the name of the member within the struct. + * + */ +#define container_of(ptr, type, member) ({ \ + const typeof( ((type *)0)->member ) *__mptr = (ptr); \ + (type *)( (char *)__mptr - offsetof(type,member) );}) + + +struct list_head { + struct list_head *next, *prev; +}; + + +#define LIST_HEAD_INIT(name) { &(name), &(name) } + +#define LIST_HEAD(name) \ + struct list_head name = LIST_HEAD_INIT(name) + +/** + * list_entry - get the struct for this entry + * @ptr: the &struct list_head pointer. + * @type: the type of the struct this is embedded in. + * @member: the name of the list_head within the struct. + */ +#define list_entry(ptr, type, member) \ + container_of(ptr, type, member) + +/** + * list_for_each_entry - iterate over list of given type + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_head within the struct. + */ +#define list_for_each_entry(pos, head, member) \ + for (pos = list_entry((head)->next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.next, typeof(*pos), member)) + +/** + * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_head within the struct. + */ +#define list_for_each_entry_safe(pos, n, head, member) \ + for (pos = list_entry((head)->next, typeof(*pos), member), \ + n = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.next, typeof(*n), member)) + +/** + * list_empty - tests whether a list is empty + * @head: the list to test. + */ +static inline int list_empty(const struct list_head *head) +{ + return head->next == head; +} + +/* + * Insert a new entry between two known consecutive entries. + * + * This is only for internal list manipulation where we know + * the prev/next entries already! + */ +static inline void __list_add(struct list_head *_new, + struct list_head *prev, + struct list_head *next) +{ + next->prev = _new; + _new->next = next; + _new->prev = prev; + prev->next = _new; +} + +/** + * list_add_tail - add a new entry + * @new: new entry to be added + * @head: list head to add it before + * + * Insert a new entry before the specified head. + * This is useful for implementing queues. + */ +static inline void list_add_tail(struct list_head *_new, struct list_head *head) +{ + __list_add(_new, head->prev, head); +} + +/* + * Delete a list entry by making the prev/next entries + * point to each other. + * + * This is only for internal list manipulation where we know + * the prev/next entries already! + */ +static inline void __list_del(struct list_head *prev, struct list_head *next) +{ + next->prev = prev; + prev->next = next; +} + +#define LIST_POISON1 ((void *) 0x00100100) +#define LIST_POISON2 ((void *) 0x00200200) +/** + * list_del - deletes entry from list. + * @entry: the element to delete from the list. + * Note: list_empty() on entry does not return true after this, the entry is + * in an undefined state. + */ +static inline void list_del(struct list_head *entry) +{ + __list_del(entry->prev, entry->next); + entry->next = (struct list_head*)LIST_POISON1; + entry->prev = (struct list_head*)LIST_POISON2; +} +#endif diff --git a/scripts/kconfig/lkc.h b/scripts/kconfig/lkc.h new file mode 100644 index 0000000000..91ca126ea0 --- /dev/null +++ b/scripts/kconfig/lkc.h @@ -0,0 +1,186 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#ifndef LKC_H +#define LKC_H + +#include "expr.h" + +#ifndef KBUILD_NO_NLS +# include +#else +static inline const char *gettext(const char *txt) { return txt; } +static inline void textdomain(const char *domainname) {} +static inline void bindtextdomain(const char *name, const char *dir) {} +static inline char *bind_textdomain_codeset(const char *dn, char *c) { return c; } +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#include "lkc_proto.h" + +#define SRCTREE "srctree" + +#ifndef PACKAGE +#define PACKAGE "linux" +#endif + +#define LOCALEDIR "/usr/share/locale" + +#define _(text) gettext(text) +#define N_(text) (text) + +#ifndef CONFIG_ +#define CONFIG_ "CONFIG_" +#endif +static inline const char *CONFIG_prefix(void) +{ + return getenv( "CONFIG_" ) ?: CONFIG_; +} +#undef CONFIG_ +#define CONFIG_ CONFIG_prefix() + +#define TF_COMMAND 0x0001 +#define TF_PARAM 0x0002 +#define TF_OPTION 0x0004 + +enum conf_def_mode { + def_default, + def_yes, + def_mod, + def_no, + def_random +}; + +#define T_OPT_MODULES 1 +#define T_OPT_DEFCONFIG_LIST 2 +#define T_OPT_ENV 3 +#define T_OPT_ALLNOCONFIG_Y 4 + +struct kconf_id { + int name; + int token; + unsigned int flags; + enum symbol_type stype; +}; + +void zconfdump(FILE *out); +void zconf_starthelp(void); +FILE *zconf_fopen(const char *name); +void zconf_initscan(const char *name); +void zconf_nextfile(const char *name); +int zconf_lineno(void); +const char *zconf_curname(void); + +/* confdata.c */ +const char *conf_get_configname(void); +const char *conf_get_autoconfig_name(void); +char *conf_get_default_confname(void); +void sym_set_change_count(int count); +void sym_add_change_count(int count); +bool conf_set_all_new_symbols(enum conf_def_mode mode); +void set_all_choice_values(struct symbol *csym); + +/* confdata.c and expr.c */ +static inline void xfwrite(const void *str, size_t len, size_t count, FILE *out) +{ + assert(len != 0); + + if (fwrite(str, len, count, out) != count) + fprintf(stderr, "Error in writing or end of file.\n"); +} + +/* menu.c */ +void _menu_init(void); +void menu_warn(struct menu *menu, const char *fmt, ...); +struct menu *menu_add_menu(void); +void menu_end_menu(void); +void menu_add_entry(struct symbol *sym); +void menu_end_entry(void); +void menu_add_dep(struct expr *dep); +void menu_add_visibility(struct expr *dep); +struct property *menu_add_prompt(enum prop_type type, char *prompt, struct expr *dep); +void menu_add_expr(enum prop_type type, struct expr *expr, struct expr *dep); +void menu_add_symbol(enum prop_type type, struct symbol *sym, struct expr *dep); +void menu_add_option(int token, char *arg); +void menu_finalize(struct menu *parent); +void menu_set_type(int type); + +/* util.c */ +struct file *file_lookup(const char *name); +int file_write_dep(const char *name); +void *xmalloc(size_t size); +void *xcalloc(size_t nmemb, size_t size); + +struct gstr { + size_t len; + char *s; + /* + * when max_width is not zero long lines in string s (if any) get + * wrapped not to exceed the max_width value + */ + int max_width; +}; +struct gstr str_new(void); +void str_free(struct gstr *gs); +void str_append(struct gstr *gs, const char *s); +void str_printf(struct gstr *gs, const char *fmt, ...); +const char *str_get(struct gstr *gs); + +/* symbol.c */ +extern struct expr *sym_env_list; + +void sym_init(void); +void sym_clear_all_valid(void); +struct symbol *sym_choice_default(struct symbol *sym); +const char *sym_get_string_default(struct symbol *sym); +struct symbol *sym_check_deps(struct symbol *sym); +struct property *prop_alloc(enum prop_type type, struct symbol *sym); +struct symbol *prop_get_symbol(struct property *prop); +struct property *sym_get_env_prop(struct symbol *sym); + +static inline tristate sym_get_tristate_value(struct symbol *sym) +{ + return sym->curr.tri; +} + + +static inline struct symbol *sym_get_choice_value(struct symbol *sym) +{ + return (struct symbol *)sym->curr.val; +} + +static inline bool sym_set_choice_value(struct symbol *ch, struct symbol *chval) +{ + return sym_set_tristate_value(chval, yes); +} + +static inline bool sym_is_choice(struct symbol *sym) +{ + return sym->flags & SYMBOL_CHOICE ? true : false; +} + +static inline bool sym_is_choice_value(struct symbol *sym) +{ + return sym->flags & SYMBOL_CHOICEVAL ? true : false; +} + +static inline bool sym_is_optional(struct symbol *sym) +{ + return sym->flags & SYMBOL_OPTIONAL ? true : false; +} + +static inline bool sym_has_value(struct symbol *sym) +{ + return sym->flags & SYMBOL_DEF_USER ? true : false; +} + +#ifdef __cplusplus +} +#endif + +#endif /* LKC_H */ diff --git a/scripts/kconfig/lkc_proto.h b/scripts/kconfig/lkc_proto.h new file mode 100644 index 0000000000..d5398718ec --- /dev/null +++ b/scripts/kconfig/lkc_proto.h @@ -0,0 +1,52 @@ +#include + +/* confdata.c */ +void conf_parse(const char *name); +int conf_read(const char *name); +int conf_read_simple(const char *name, int); +int conf_write_defconfig(const char *name); +int conf_write(const char *name); +int conf_write_autoconf(void); +bool conf_get_changed(void); +void conf_set_changed_callback(void (*fn)(void)); +void conf_set_message_callback(void (*fn)(const char *fmt, va_list ap)); + +/* menu.c */ +extern struct menu rootmenu; + +bool menu_is_empty(struct menu *menu); +bool menu_is_visible(struct menu *menu); +bool menu_has_prompt(struct menu *menu); +const char * menu_get_prompt(struct menu *menu); +struct menu * menu_get_root_menu(struct menu *menu); +struct menu * menu_get_parent_menu(struct menu *menu); +bool menu_has_help(struct menu *menu); +const char * menu_get_help(struct menu *menu); +struct gstr get_relations_str(struct symbol **sym_arr, struct list_head *head); +void menu_get_ext_help(struct menu *menu, struct gstr *help); + +/* symbol.c */ +extern struct symbol * symbol_hash[SYMBOL_HASHSIZE]; + +struct symbol * sym_lookup(const char *name, int flags); +struct symbol * sym_find(const char *name); +const char * sym_expand_string_value(const char *in); +const char * sym_escape_string_value(const char *in); +struct symbol ** sym_re_search(const char *pattern); +const char * sym_type_name(enum symbol_type type); +void sym_calc_value(struct symbol *sym); +enum symbol_type sym_get_type(struct symbol *sym); +bool sym_tristate_within_range(struct symbol *sym,tristate tri); +bool sym_set_tristate_value(struct symbol *sym,tristate tri); +tristate sym_toggle_tristate_value(struct symbol *sym); +bool sym_string_valid(struct symbol *sym, const char *newval); +bool sym_string_within_range(struct symbol *sym, const char *str); +bool sym_set_string_value(struct symbol *sym, const char *newval); +bool sym_is_changable(struct symbol *sym); +struct property * sym_get_choice_prop(struct symbol *sym); +const char * sym_get_string_value(struct symbol *sym); + +const char * prop_get_type_name(enum prop_type type); + +/* expr.c */ +void expr_print(struct expr *e, void (*fn)(void *, struct symbol *, const char *), void *data, int prevtoken); diff --git a/scripts/kconfig/lxdialog/.gitignore b/scripts/kconfig/lxdialog/.gitignore new file mode 100644 index 0000000000..90b08ff025 --- /dev/null +++ b/scripts/kconfig/lxdialog/.gitignore @@ -0,0 +1,4 @@ +# +# Generated files +# +lxdialog diff --git a/scripts/kconfig/lxdialog/BIG.FAT.WARNING b/scripts/kconfig/lxdialog/BIG.FAT.WARNING new file mode 100644 index 0000000000..a8999d82bd --- /dev/null +++ b/scripts/kconfig/lxdialog/BIG.FAT.WARNING @@ -0,0 +1,4 @@ +This is NOT the official version of dialog. This version has been +significantly modified from the original. It is for use by the Linux +kernel configuration script. Please do not bother Savio Lam with +questions about this program. diff --git a/scripts/kconfig/lxdialog/check-lxdialog.sh b/scripts/kconfig/lxdialog/check-lxdialog.sh new file mode 100755 index 0000000000..5075ebf2d3 --- /dev/null +++ b/scripts/kconfig/lxdialog/check-lxdialog.sh @@ -0,0 +1,91 @@ +#!/bin/sh +# Check ncurses compatibility + +# What library to link +ldflags() +{ + pkg-config --libs ncursesw 2>/dev/null && exit + pkg-config --libs ncurses 2>/dev/null && exit + for ext in so a dll.a dylib ; do + for lib in ncursesw ncurses curses ; do + $cc -print-file-name=lib${lib}.${ext} | grep -q / + if [ $? -eq 0 ]; then + echo "-l${lib}" + exit + fi + done + done + exit 1 +} + +# Where is ncurses.h? +ccflags() +{ + if pkg-config --cflags ncursesw 2>/dev/null; then + echo '-DCURSES_LOC="" -DNCURSES_WIDECHAR=1' + elif pkg-config --cflags ncurses 2>/dev/null; then + echo '-DCURSES_LOC=""' + elif [ -f /usr/include/ncursesw/curses.h ]; then + echo '-I/usr/include/ncursesw -DCURSES_LOC=""' + echo ' -DNCURSES_WIDECHAR=1' + elif [ -f /usr/include/ncurses/ncurses.h ]; then + echo '-I/usr/include/ncurses -DCURSES_LOC=""' + elif [ -f /usr/include/ncurses/curses.h ]; then + echo '-I/usr/include/ncurses -DCURSES_LOC=""' + elif [ -f /usr/include/ncurses.h ]; then + echo '-DCURSES_LOC=""' + else + echo '-DCURSES_LOC=""' + fi +} + +# Temp file, try to clean up after us +tmp=.lxdialog.tmp +trap "rm -f $tmp" 0 1 2 3 15 + +# Check if we can link to ncurses +check() { + $cc -x c - -o $tmp 2>/dev/null <<'EOF' +#include CURSES_LOC +main() {} +EOF + if [ $? != 0 ]; then + echo " *** Unable to find the ncurses libraries or the" 1>&2 + echo " *** required header files." 1>&2 + echo " *** 'make menuconfig' requires the ncurses libraries." 1>&2 + echo " *** " 1>&2 + echo " *** Install ncurses (ncurses-devel) and try again." 1>&2 + echo " *** " 1>&2 + exit 1 + fi +} + +usage() { + printf "Usage: $0 [-check compiler options|-ccflags|-ldflags compiler options]\n" +} + +if [ $# -eq 0 ]; then + usage + exit 1 +fi + +cc="" +case "$1" in + "-check") + shift + cc="$@" + check + ;; + "-ccflags") + ccflags + ;; + "-ldflags") + shift + cc="$@" + ldflags + ;; + "*") + usage + exit 1 + ;; +esac diff --git a/scripts/kconfig/lxdialog/checklist.c b/scripts/kconfig/lxdialog/checklist.c new file mode 100644 index 0000000000..8d016faa28 --- /dev/null +++ b/scripts/kconfig/lxdialog/checklist.c @@ -0,0 +1,332 @@ +/* + * checklist.c -- implements the checklist box + * + * ORIGINAL AUTHOR: Savio Lam (lam836@cs.cuhk.hk) + * Stuart Herbert - S.Herbert@sheffield.ac.uk: radiolist extension + * Alessandro Rubini - rubini@ipvvis.unipv.it: merged the two + * MODIFIED FOR LINUX KERNEL CONFIG BY: William Roadcap (roadcap@cfw.com) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "dialog.h" + +static int list_width, check_x, item_x; + +/* + * Print list item + */ +static void print_item(WINDOW * win, int choice, int selected) +{ + int i; + char *list_item = malloc(list_width + 1); + + strncpy(list_item, item_str(), list_width - item_x); + list_item[list_width - item_x] = '\0'; + + /* Clear 'residue' of last item */ + wattrset(win, dlg.menubox.atr); + wmove(win, choice, 0); + for (i = 0; i < list_width; i++) + waddch(win, ' '); + + wmove(win, choice, check_x); + wattrset(win, selected ? dlg.check_selected.atr + : dlg.check.atr); + if (!item_is_tag(':')) + wprintw(win, "(%c)", item_is_tag('X') ? 'X' : ' '); + + wattrset(win, selected ? dlg.tag_selected.atr : dlg.tag.atr); + mvwaddch(win, choice, item_x, list_item[0]); + wattrset(win, selected ? dlg.item_selected.atr : dlg.item.atr); + waddstr(win, list_item + 1); + if (selected) { + wmove(win, choice, check_x + 1); + wrefresh(win); + } + free(list_item); +} + +/* + * Print the scroll indicators. + */ +static void print_arrows(WINDOW * win, int choice, int item_no, int scroll, + int y, int x, int height) +{ + wmove(win, y, x); + + if (scroll > 0) { + wattrset(win, dlg.uarrow.atr); + waddch(win, ACS_UARROW); + waddstr(win, "(-)"); + } else { + wattrset(win, dlg.menubox.atr); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + } + + y = y + height + 1; + wmove(win, y, x); + + if ((height < item_no) && (scroll + choice < item_no - 1)) { + wattrset(win, dlg.darrow.atr); + waddch(win, ACS_DARROW); + waddstr(win, "(+)"); + } else { + wattrset(win, dlg.menubox_border.atr); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + } +} + +/* + * Display the termination buttons + */ +static void print_buttons(WINDOW * dialog, int height, int width, int selected) +{ + int x = width / 2 - 11; + int y = height - 2; + + print_button(dialog, gettext("Select"), y, x, selected == 0); + print_button(dialog, gettext(" Help "), y, x + 14, selected == 1); + + wmove(dialog, y, x + 1 + 14 * selected); + wrefresh(dialog); +} + +/* + * Display a dialog box with a list of options that can be turned on or off + * in the style of radiolist (only one option turned on at a time). + */ +int dialog_checklist(const char *title, const char *prompt, int height, + int width, int list_height) +{ + int i, x, y, box_x, box_y; + int key = 0, button = 0, choice = 0, scroll = 0, max_choice; + WINDOW *dialog, *list; + + /* which item to highlight */ + item_foreach() { + if (item_is_tag('X')) + choice = item_n(); + if (item_is_selected()) { + choice = item_n(); + break; + } + } + +do_resize: + if (getmaxy(stdscr) < (height + CHECKLIST_HEIGTH_MIN)) + return -ERRDISPLAYTOOSMALL; + if (getmaxx(stdscr) < (width + CHECKLIST_WIDTH_MIN)) + return -ERRDISPLAYTOOSMALL; + + max_choice = MIN(list_height, item_count()); + + /* center dialog box on screen */ + x = (getmaxx(stdscr) - width) / 2; + y = (getmaxy(stdscr) - height) / 2; + + draw_shadow(stdscr, y, x, height, width); + + dialog = newwin(height, width, y, x); + keypad(dialog, TRUE); + + draw_box(dialog, 0, 0, height, width, + dlg.dialog.atr, dlg.border.atr); + wattrset(dialog, dlg.border.atr); + mvwaddch(dialog, height - 3, 0, ACS_LTEE); + for (i = 0; i < width - 2; i++) + waddch(dialog, ACS_HLINE); + wattrset(dialog, dlg.dialog.atr); + waddch(dialog, ACS_RTEE); + + print_title(dialog, title, width); + + wattrset(dialog, dlg.dialog.atr); + print_autowrap(dialog, prompt, width - 2, 1, 3); + + list_width = width - 6; + box_y = height - list_height - 5; + box_x = (width - list_width) / 2 - 1; + + /* create new window for the list */ + list = subwin(dialog, list_height, list_width, y + box_y + 1, + x + box_x + 1); + + keypad(list, TRUE); + + /* draw a box around the list items */ + draw_box(dialog, box_y, box_x, list_height + 2, list_width + 2, + dlg.menubox_border.atr, dlg.menubox.atr); + + /* Find length of longest item in order to center checklist */ + check_x = 0; + item_foreach() + check_x = MAX(check_x, strlen(item_str()) + 4); + check_x = MIN(check_x, list_width); + + check_x = (list_width - check_x) / 2; + item_x = check_x + 4; + + if (choice >= list_height) { + scroll = choice - list_height + 1; + choice -= scroll; + } + + /* Print the list */ + for (i = 0; i < max_choice; i++) { + item_set(scroll + i); + print_item(list, i, i == choice); + } + + print_arrows(dialog, choice, item_count(), scroll, + box_y, box_x + check_x + 5, list_height); + + print_buttons(dialog, height, width, 0); + + wnoutrefresh(dialog); + wnoutrefresh(list); + doupdate(); + + while (key != KEY_ESC) { + key = wgetch(dialog); + + for (i = 0; i < max_choice; i++) { + item_set(i + scroll); + if (toupper(key) == toupper(item_str()[0])) + break; + } + + if (i < max_choice || key == KEY_UP || key == KEY_DOWN || + key == '+' || key == '-') { + if (key == KEY_UP || key == '-') { + if (!choice) { + if (!scroll) + continue; + /* Scroll list down */ + if (list_height > 1) { + /* De-highlight current first item */ + item_set(scroll); + print_item(list, 0, FALSE); + scrollok(list, TRUE); + wscrl(list, -1); + scrollok(list, FALSE); + } + scroll--; + item_set(scroll); + print_item(list, 0, TRUE); + print_arrows(dialog, choice, item_count(), + scroll, box_y, box_x + check_x + 5, list_height); + + wnoutrefresh(dialog); + wrefresh(list); + + continue; /* wait for another key press */ + } else + i = choice - 1; + } else if (key == KEY_DOWN || key == '+') { + if (choice == max_choice - 1) { + if (scroll + choice >= item_count() - 1) + continue; + /* Scroll list up */ + if (list_height > 1) { + /* De-highlight current last item before scrolling up */ + item_set(scroll + max_choice - 1); + print_item(list, + max_choice - 1, + FALSE); + scrollok(list, TRUE); + wscrl(list, 1); + scrollok(list, FALSE); + } + scroll++; + item_set(scroll + max_choice - 1); + print_item(list, max_choice - 1, TRUE); + + print_arrows(dialog, choice, item_count(), + scroll, box_y, box_x + check_x + 5, list_height); + + wnoutrefresh(dialog); + wrefresh(list); + + continue; /* wait for another key press */ + } else + i = choice + 1; + } + if (i != choice) { + /* De-highlight current item */ + item_set(scroll + choice); + print_item(list, choice, FALSE); + /* Highlight new item */ + choice = i; + item_set(scroll + choice); + print_item(list, choice, TRUE); + wnoutrefresh(dialog); + wrefresh(list); + } + continue; /* wait for another key press */ + } + switch (key) { + case 'H': + case 'h': + case '?': + button = 1; + /* fall-through */ + case 'S': + case 's': + case ' ': + case '\n': + item_foreach() + item_set_selected(0); + item_set(scroll + choice); + item_set_selected(1); + delwin(list); + delwin(dialog); + return button; + case TAB: + case KEY_LEFT: + case KEY_RIGHT: + button = ((key == KEY_LEFT ? --button : ++button) < 0) + ? 1 : (button > 1 ? 0 : button); + + print_buttons(dialog, height, width, button); + wrefresh(dialog); + break; + case 'X': + case 'x': + key = KEY_ESC; + break; + case KEY_ESC: + key = on_key_esc(dialog); + break; + case KEY_RESIZE: + delwin(list); + delwin(dialog); + on_key_resize(); + goto do_resize; + } + + /* Now, update everything... */ + doupdate(); + } + delwin(list); + delwin(dialog); + return key; /* ESC pressed */ +} diff --git a/scripts/kconfig/lxdialog/dialog.h b/scripts/kconfig/lxdialog/dialog.h new file mode 100644 index 0000000000..fcffd5b41f --- /dev/null +++ b/scripts/kconfig/lxdialog/dialog.h @@ -0,0 +1,257 @@ +/* + * dialog.h -- common declarations for all dialog modules + * + * AUTHOR: Savio Lam (lam836@cs.cuhk.hk) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include +#include +#include +#include +#include +#include +#include + +#ifndef KBUILD_NO_NLS +# include +#else +# define gettext(Msgid) ((const char *) (Msgid)) +#endif + +#ifdef __sun__ +#define CURS_MACROS +#endif +#include CURSES_LOC + +/* + * Colors in ncurses 1.9.9e do not work properly since foreground and + * background colors are OR'd rather than separately masked. This version + * of dialog was hacked to work with ncurses 1.9.9e, making it incompatible + * with standard curses. The simplest fix (to make this work with standard + * curses) uses the wbkgdset() function, not used in the original hack. + * Turn it off if we're building with 1.9.9e, since it just confuses things. + */ +#if defined(NCURSES_VERSION) && defined(_NEED_WRAP) && !defined(GCC_PRINTFLIKE) +#define OLD_NCURSES 1 +#undef wbkgdset +#define wbkgdset(w,p) /*nothing */ +#else +#define OLD_NCURSES 0 +#endif + +#define TR(params) _tracef params + +#define KEY_ESC 27 +#define TAB 9 +#define MAX_LEN 2048 +#define BUF_SIZE (10*1024) +#define MIN(x,y) (x < y ? x : y) +#define MAX(x,y) (x > y ? x : y) + +#ifndef ACS_ULCORNER +#define ACS_ULCORNER '+' +#endif +#ifndef ACS_LLCORNER +#define ACS_LLCORNER '+' +#endif +#ifndef ACS_URCORNER +#define ACS_URCORNER '+' +#endif +#ifndef ACS_LRCORNER +#define ACS_LRCORNER '+' +#endif +#ifndef ACS_HLINE +#define ACS_HLINE '-' +#endif +#ifndef ACS_VLINE +#define ACS_VLINE '|' +#endif +#ifndef ACS_LTEE +#define ACS_LTEE '+' +#endif +#ifndef ACS_RTEE +#define ACS_RTEE '+' +#endif +#ifndef ACS_UARROW +#define ACS_UARROW '^' +#endif +#ifndef ACS_DARROW +#define ACS_DARROW 'v' +#endif + +/* error return codes */ +#define ERRDISPLAYTOOSMALL (KEY_MAX + 1) + +/* + * Color definitions + */ +struct dialog_color { + chtype atr; /* Color attribute */ + int fg; /* foreground */ + int bg; /* background */ + int hl; /* highlight this item */ +}; + +struct subtitle_list { + struct subtitle_list *next; + const char *text; +}; + +struct dialog_info { + const char *backtitle; + struct subtitle_list *subtitles; + struct dialog_color screen; + struct dialog_color shadow; + struct dialog_color dialog; + struct dialog_color title; + struct dialog_color border; + struct dialog_color button_active; + struct dialog_color button_inactive; + struct dialog_color button_key_active; + struct dialog_color button_key_inactive; + struct dialog_color button_label_active; + struct dialog_color button_label_inactive; + struct dialog_color inputbox; + struct dialog_color inputbox_border; + struct dialog_color searchbox; + struct dialog_color searchbox_title; + struct dialog_color searchbox_border; + struct dialog_color position_indicator; + struct dialog_color menubox; + struct dialog_color menubox_border; + struct dialog_color item; + struct dialog_color item_selected; + struct dialog_color tag; + struct dialog_color tag_selected; + struct dialog_color tag_key; + struct dialog_color tag_key_selected; + struct dialog_color check; + struct dialog_color check_selected; + struct dialog_color uarrow; + struct dialog_color darrow; +}; + +/* + * Global variables + */ +extern struct dialog_info dlg; +extern char dialog_input_result[]; +extern int saved_x, saved_y; /* Needed in signal handler in mconf.c */ + +/* + * Function prototypes + */ + +/* item list as used by checklist and menubox */ +void item_reset(void); +void item_make(const char *fmt, ...); +void item_add_str(const char *fmt, ...); +void item_set_tag(char tag); +void item_set_data(void *p); +void item_set_selected(int val); +int item_activate_selected(void); +void *item_data(void); +char item_tag(void); + +/* item list manipulation for lxdialog use */ +#define MAXITEMSTR 200 +struct dialog_item { + char str[MAXITEMSTR]; /* prompt displayed */ + char tag; + void *data; /* pointer to menu item - used by menubox+checklist */ + int selected; /* Set to 1 by dialog_*() function if selected. */ +}; + +/* list of lialog_items */ +struct dialog_list { + struct dialog_item node; + struct dialog_list *next; +}; + +extern struct dialog_list *item_cur; +extern struct dialog_list item_nil; +extern struct dialog_list *item_head; + +int item_count(void); +void item_set(int n); +int item_n(void); +const char *item_str(void); +int item_is_selected(void); +int item_is_tag(char tag); +#define item_foreach() \ + for (item_cur = item_head ? item_head: item_cur; \ + item_cur && (item_cur != &item_nil); item_cur = item_cur->next) + +/* generic key handlers */ +int on_key_esc(WINDOW *win); +int on_key_resize(void); + +/* minimum (re)size values */ +#define CHECKLIST_HEIGTH_MIN 6 /* For dialog_checklist() */ +#define CHECKLIST_WIDTH_MIN 6 +#define INPUTBOX_HEIGTH_MIN 2 /* For dialog_inputbox() */ +#define INPUTBOX_WIDTH_MIN 2 +#define MENUBOX_HEIGTH_MIN 15 /* For dialog_menu() */ +#define MENUBOX_WIDTH_MIN 65 +#define TEXTBOX_HEIGTH_MIN 8 /* For dialog_textbox() */ +#define TEXTBOX_WIDTH_MIN 8 +#define YESNO_HEIGTH_MIN 4 /* For dialog_yesno() */ +#define YESNO_WIDTH_MIN 4 +#define WINDOW_HEIGTH_MIN 19 /* For init_dialog() */ +#define WINDOW_WIDTH_MIN 80 + +int init_dialog(const char *backtitle); +void set_dialog_backtitle(const char *backtitle); +void set_dialog_subtitles(struct subtitle_list *subtitles); +void end_dialog(int x, int y); +void attr_clear(WINDOW * win, int height, int width, chtype attr); +void dialog_clear(void); +void print_autowrap(WINDOW * win, const char *prompt, int width, int y, int x); +void print_button(WINDOW * win, const char *label, int y, int x, int selected); +void print_title(WINDOW *dialog, const char *title, int width); +void draw_box(WINDOW * win, int y, int x, int height, int width, chtype box, + chtype border); +void draw_shadow(WINDOW * win, int y, int x, int height, int width); + +int first_alpha(const char *string, const char *exempt); +int dialog_yesno(const char *title, const char *prompt, int height, int width); +int dialog_msgbox(const char *title, const char *prompt, int height, + int width, int pause); + + +typedef void (*update_text_fn)(char *buf, size_t start, size_t end, void + *_data); +int dialog_textbox(const char *title, char *tbuf, int initial_height, + int initial_width, int *keys, int *_vscroll, int *_hscroll, + update_text_fn update_text, void *data); +int dialog_menu(const char *title, const char *prompt, + const void *selected, int *s_scroll); +int dialog_checklist(const char *title, const char *prompt, int height, + int width, int list_height); +int dialog_inputbox(const char *title, const char *prompt, int height, + int width, const char *init); + +/* + * This is the base for fictitious keys, which activate + * the buttons. + * + * Mouse-generated keys are the following: + * -- the first 32 are used as numbers, in addition to '0'-'9' + * -- the lowercase are used to signal mouse-enter events (M_EVENT + 'o') + * -- uppercase chars are used to invoke the button (M_EVENT + 'O') + */ +#define M_EVENT (KEY_MAX+1) diff --git a/scripts/kconfig/lxdialog/inputbox.c b/scripts/kconfig/lxdialog/inputbox.c new file mode 100644 index 0000000000..d58de1dc53 --- /dev/null +++ b/scripts/kconfig/lxdialog/inputbox.c @@ -0,0 +1,301 @@ +/* + * inputbox.c -- implements the input box + * + * ORIGINAL AUTHOR: Savio Lam (lam836@cs.cuhk.hk) + * MODIFIED FOR LINUX KERNEL CONFIG BY: William Roadcap (roadcap@cfw.com) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "dialog.h" + +char dialog_input_result[MAX_LEN + 1]; + +/* + * Print the termination buttons + */ +static void print_buttons(WINDOW * dialog, int height, int width, int selected) +{ + int x = width / 2 - 11; + int y = height - 2; + + print_button(dialog, gettext(" Ok "), y, x, selected == 0); + print_button(dialog, gettext(" Help "), y, x + 14, selected == 1); + + wmove(dialog, y, x + 1 + 14 * selected); + wrefresh(dialog); +} + +/* + * Display a dialog box for inputing a string + */ +int dialog_inputbox(const char *title, const char *prompt, int height, int width, + const char *init) +{ + int i, x, y, box_y, box_x, box_width; + int input_x = 0, key = 0, button = -1; + int show_x, len, pos; + char *instr = dialog_input_result; + WINDOW *dialog; + + if (!init) + instr[0] = '\0'; + else + strcpy(instr, init); + +do_resize: + if (getmaxy(stdscr) <= (height - INPUTBOX_HEIGTH_MIN)) + return -ERRDISPLAYTOOSMALL; + if (getmaxx(stdscr) <= (width - INPUTBOX_WIDTH_MIN)) + return -ERRDISPLAYTOOSMALL; + + /* center dialog box on screen */ + x = (getmaxx(stdscr) - width) / 2; + y = (getmaxy(stdscr) - height) / 2; + + draw_shadow(stdscr, y, x, height, width); + + dialog = newwin(height, width, y, x); + keypad(dialog, TRUE); + + draw_box(dialog, 0, 0, height, width, + dlg.dialog.atr, dlg.border.atr); + wattrset(dialog, dlg.border.atr); + mvwaddch(dialog, height - 3, 0, ACS_LTEE); + for (i = 0; i < width - 2; i++) + waddch(dialog, ACS_HLINE); + wattrset(dialog, dlg.dialog.atr); + waddch(dialog, ACS_RTEE); + + print_title(dialog, title, width); + + wattrset(dialog, dlg.dialog.atr); + print_autowrap(dialog, prompt, width - 2, 1, 3); + + /* Draw the input field box */ + box_width = width - 6; + getyx(dialog, y, x); + box_y = y + 2; + box_x = (width - box_width) / 2; + draw_box(dialog, y + 1, box_x - 1, 3, box_width + 2, + dlg.dialog.atr, dlg.border.atr); + + print_buttons(dialog, height, width, 0); + + /* Set up the initial value */ + wmove(dialog, box_y, box_x); + wattrset(dialog, dlg.inputbox.atr); + + len = strlen(instr); + pos = len; + + if (len >= box_width) { + show_x = len - box_width + 1; + input_x = box_width - 1; + for (i = 0; i < box_width - 1; i++) + waddch(dialog, instr[show_x + i]); + } else { + show_x = 0; + input_x = len; + waddstr(dialog, instr); + } + + wmove(dialog, box_y, box_x + input_x); + + wrefresh(dialog); + + while (key != KEY_ESC) { + key = wgetch(dialog); + + if (button == -1) { /* Input box selected */ + switch (key) { + case TAB: + case KEY_UP: + case KEY_DOWN: + break; + case KEY_BACKSPACE: + case 127: + if (pos) { + wattrset(dialog, dlg.inputbox.atr); + if (input_x == 0) { + show_x--; + } else + input_x--; + + if (pos < len) { + for (i = pos - 1; i < len; i++) { + instr[i] = instr[i+1]; + } + } + + pos--; + len--; + instr[len] = '\0'; + wmove(dialog, box_y, box_x); + for (i = 0; i < box_width; i++) { + if (!instr[show_x + i]) { + waddch(dialog, ' '); + break; + } + waddch(dialog, instr[show_x + i]); + } + wmove(dialog, box_y, input_x + box_x); + wrefresh(dialog); + } + continue; + case KEY_LEFT: + if (pos > 0) { + if (input_x > 0) { + wmove(dialog, box_y, --input_x + box_x); + } else if (input_x == 0) { + show_x--; + wmove(dialog, box_y, box_x); + for (i = 0; i < box_width; i++) { + if (!instr[show_x + i]) { + waddch(dialog, ' '); + break; + } + waddch(dialog, instr[show_x + i]); + } + wmove(dialog, box_y, box_x); + } + pos--; + } + continue; + case KEY_RIGHT: + if (pos < len) { + if (input_x < box_width - 1) { + wmove(dialog, box_y, ++input_x + box_x); + } else if (input_x == box_width - 1) { + show_x++; + wmove(dialog, box_y, box_x); + for (i = 0; i < box_width; i++) { + if (!instr[show_x + i]) { + waddch(dialog, ' '); + break; + } + waddch(dialog, instr[show_x + i]); + } + wmove(dialog, box_y, input_x + box_x); + } + pos++; + } + continue; + default: + if (key < 0x100 && isprint(key)) { + if (len < MAX_LEN) { + wattrset(dialog, dlg.inputbox.atr); + if (pos < len) { + for (i = len; i > pos; i--) + instr[i] = instr[i-1]; + instr[pos] = key; + } else { + instr[len] = key; + } + pos++; + len++; + instr[len] = '\0'; + + if (input_x == box_width - 1) { + show_x++; + } else { + input_x++; + } + + wmove(dialog, box_y, box_x); + for (i = 0; i < box_width; i++) { + if (!instr[show_x + i]) { + waddch(dialog, ' '); + break; + } + waddch(dialog, instr[show_x + i]); + } + wmove(dialog, box_y, input_x + box_x); + wrefresh(dialog); + } else + flash(); /* Alarm user about overflow */ + continue; + } + } + } + switch (key) { + case 'O': + case 'o': + delwin(dialog); + return 0; + case 'H': + case 'h': + delwin(dialog); + return 1; + case KEY_UP: + case KEY_LEFT: + switch (button) { + case -1: + button = 1; /* Indicates "Help" button is selected */ + print_buttons(dialog, height, width, 1); + break; + case 0: + button = -1; /* Indicates input box is selected */ + print_buttons(dialog, height, width, 0); + wmove(dialog, box_y, box_x + input_x); + wrefresh(dialog); + break; + case 1: + button = 0; /* Indicates "OK" button is selected */ + print_buttons(dialog, height, width, 0); + break; + } + break; + case TAB: + case KEY_DOWN: + case KEY_RIGHT: + switch (button) { + case -1: + button = 0; /* Indicates "OK" button is selected */ + print_buttons(dialog, height, width, 0); + break; + case 0: + button = 1; /* Indicates "Help" button is selected */ + print_buttons(dialog, height, width, 1); + break; + case 1: + button = -1; /* Indicates input box is selected */ + print_buttons(dialog, height, width, 0); + wmove(dialog, box_y, box_x + input_x); + wrefresh(dialog); + break; + } + break; + case ' ': + case '\n': + delwin(dialog); + return (button == -1 ? 0 : button); + case 'X': + case 'x': + key = KEY_ESC; + break; + case KEY_ESC: + key = on_key_esc(dialog); + break; + case KEY_RESIZE: + delwin(dialog); + on_key_resize(); + goto do_resize; + } + } + + delwin(dialog); + return KEY_ESC; /* ESC pressed */ +} diff --git a/scripts/kconfig/lxdialog/menubox.c b/scripts/kconfig/lxdialog/menubox.c new file mode 100644 index 0000000000..11ae9ad7ac --- /dev/null +++ b/scripts/kconfig/lxdialog/menubox.c @@ -0,0 +1,437 @@ +/* + * menubox.c -- implements the menu box + * + * ORIGINAL AUTHOR: Savio Lam (lam836@cs.cuhk.hk) + * MODIFIED FOR LINUX KERNEL CONFIG BY: William Roadcap (roadcapw@cfw.com) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +/* + * Changes by Clifford Wolf (god@clifford.at) + * + * [ 1998-06-13 ] + * + * *) A bugfix for the Page-Down problem + * + * *) Formerly when I used Page Down and Page Up, the cursor would be set + * to the first position in the menu box. Now lxdialog is a bit + * smarter and works more like other menu systems (just have a look at + * it). + * + * *) Formerly if I selected something my scrolling would be broken because + * lxdialog is re-invoked by the Menuconfig shell script, can't + * remember the last scrolling position, and just sets it so that the + * cursor is at the bottom of the box. Now it writes the temporary file + * lxdialog.scrltmp which contains this information. The file is + * deleted by lxdialog if the user leaves a submenu or enters a new + * one, but it would be nice if Menuconfig could make another "rm -f" + * just to be sure. Just try it out - you will recognise a difference! + * + * [ 1998-06-14 ] + * + * *) Now lxdialog is crash-safe against broken "lxdialog.scrltmp" files + * and menus change their size on the fly. + * + * *) If for some reason the last scrolling position is not saved by + * lxdialog, it sets the scrolling so that the selected item is in the + * middle of the menu box, not at the bottom. + * + * 02 January 1999, Michael Elizabeth Chastain (mec@shout.net) + * Reset 'scroll' to 0 if the value from lxdialog.scrltmp is bogus. + * This fixes a bug in Menuconfig where using ' ' to descend into menus + * would leave mis-synchronized lxdialog.scrltmp files lying around, + * fscanf would read in 'scroll', and eventually that value would get used. + */ + +#include "dialog.h" + +static int menu_width, item_x; + +/* + * Print menu item + */ +static void do_print_item(WINDOW * win, const char *item, int line_y, + int selected, int hotkey) +{ + int j; + char *menu_item = malloc(menu_width + 1); + + strncpy(menu_item, item, menu_width - item_x); + menu_item[menu_width - item_x] = '\0'; + j = first_alpha(menu_item, "YyNnMmHh"); + + /* Clear 'residue' of last item */ + wattrset(win, dlg.menubox.atr); + wmove(win, line_y, 0); +#if OLD_NCURSES + { + int i; + for (i = 0; i < menu_width; i++) + waddch(win, ' '); + } +#else + wclrtoeol(win); +#endif + wattrset(win, selected ? dlg.item_selected.atr : dlg.item.atr); + mvwaddstr(win, line_y, item_x, menu_item); + if (hotkey) { + wattrset(win, selected ? dlg.tag_key_selected.atr + : dlg.tag_key.atr); + mvwaddch(win, line_y, item_x + j, menu_item[j]); + } + if (selected) { + wmove(win, line_y, item_x + 1); + } + free(menu_item); + wrefresh(win); +} + +#define print_item(index, choice, selected) \ +do { \ + item_set(index); \ + do_print_item(menu, item_str(), choice, selected, !item_is_tag(':')); \ +} while (0) + +/* + * Print the scroll indicators. + */ +static void print_arrows(WINDOW * win, int item_no, int scroll, int y, int x, + int height) +{ + int cur_y, cur_x; + + getyx(win, cur_y, cur_x); + + wmove(win, y, x); + + if (scroll > 0) { + wattrset(win, dlg.uarrow.atr); + waddch(win, ACS_UARROW); + waddstr(win, "(-)"); + } else { + wattrset(win, dlg.menubox.atr); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + } + + y = y + height + 1; + wmove(win, y, x); + wrefresh(win); + + if ((height < item_no) && (scroll + height < item_no)) { + wattrset(win, dlg.darrow.atr); + waddch(win, ACS_DARROW); + waddstr(win, "(+)"); + } else { + wattrset(win, dlg.menubox_border.atr); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + waddch(win, ACS_HLINE); + } + + wmove(win, cur_y, cur_x); + wrefresh(win); +} + +/* + * Display the termination buttons. + */ +static void print_buttons(WINDOW * win, int height, int width, int selected) +{ + int x = width / 2 - 28; + int y = height - 2; + + print_button(win, gettext("Select"), y, x, selected == 0); + print_button(win, gettext(" Exit "), y, x + 12, selected == 1); + print_button(win, gettext(" Help "), y, x + 24, selected == 2); + print_button(win, gettext(" Save "), y, x + 36, selected == 3); + print_button(win, gettext(" Load "), y, x + 48, selected == 4); + + wmove(win, y, x + 1 + 12 * selected); + wrefresh(win); +} + +/* scroll up n lines (n may be negative) */ +static void do_scroll(WINDOW *win, int *scroll, int n) +{ + /* Scroll menu up */ + scrollok(win, TRUE); + wscrl(win, n); + scrollok(win, FALSE); + *scroll = *scroll + n; + wrefresh(win); +} + +/* + * Display a menu for choosing among a number of options + */ +int dialog_menu(const char *title, const char *prompt, + const void *selected, int *s_scroll) +{ + int i, j, x, y, box_x, box_y; + int height, width, menu_height; + int key = 0, button = 0, scroll = 0, choice = 0; + int first_item = 0, max_choice; + WINDOW *dialog, *menu; + +do_resize: + height = getmaxy(stdscr); + width = getmaxx(stdscr); + if (height < MENUBOX_HEIGTH_MIN || width < MENUBOX_WIDTH_MIN) + return -ERRDISPLAYTOOSMALL; + + height -= 4; + width -= 5; + menu_height = height - 10; + + max_choice = MIN(menu_height, item_count()); + + /* center dialog box on screen */ + x = (getmaxx(stdscr) - width) / 2; + y = (getmaxy(stdscr) - height) / 2; + + draw_shadow(stdscr, y, x, height, width); + + dialog = newwin(height, width, y, x); + keypad(dialog, TRUE); + + draw_box(dialog, 0, 0, height, width, + dlg.dialog.atr, dlg.border.atr); + wattrset(dialog, dlg.border.atr); + mvwaddch(dialog, height - 3, 0, ACS_LTEE); + for (i = 0; i < width - 2; i++) + waddch(dialog, ACS_HLINE); + wattrset(dialog, dlg.dialog.atr); + wbkgdset(dialog, dlg.dialog.atr & A_COLOR); + waddch(dialog, ACS_RTEE); + + print_title(dialog, title, width); + + wattrset(dialog, dlg.dialog.atr); + print_autowrap(dialog, prompt, width - 2, 1, 3); + + menu_width = width - 6; + box_y = height - menu_height - 5; + box_x = (width - menu_width) / 2 - 1; + + /* create new window for the menu */ + menu = subwin(dialog, menu_height, menu_width, + y + box_y + 1, x + box_x + 1); + keypad(menu, TRUE); + + /* draw a box around the menu items */ + draw_box(dialog, box_y, box_x, menu_height + 2, menu_width + 2, + dlg.menubox_border.atr, dlg.menubox.atr); + + if (menu_width >= 80) + item_x = (menu_width - 70) / 2; + else + item_x = 4; + + /* Set choice to default item */ + item_foreach() + if (selected && (selected == item_data())) + choice = item_n(); + /* get the saved scroll info */ + scroll = *s_scroll; + if ((scroll <= choice) && (scroll + max_choice > choice) && + (scroll >= 0) && (scroll + max_choice <= item_count())) { + first_item = scroll; + choice = choice - scroll; + } else { + scroll = 0; + } + if ((choice >= max_choice)) { + if (choice >= item_count() - max_choice / 2) + scroll = first_item = item_count() - max_choice; + else + scroll = first_item = choice - max_choice / 2; + choice = choice - scroll; + } + + /* Print the menu */ + for (i = 0; i < max_choice; i++) { + print_item(first_item + i, i, i == choice); + } + + wnoutrefresh(menu); + + print_arrows(dialog, item_count(), scroll, + box_y, box_x + item_x + 1, menu_height); + + print_buttons(dialog, height, width, 0); + wmove(menu, choice, item_x + 1); + wrefresh(menu); + + while (key != KEY_ESC) { + key = wgetch(menu); + + if (key < 256 && isalpha(key)) + key = tolower(key); + + if (strchr("ynmh", key)) + i = max_choice; + else { + for (i = choice + 1; i < max_choice; i++) { + item_set(scroll + i); + j = first_alpha(item_str(), "YyNnMmHh"); + if (key == tolower(item_str()[j])) + break; + } + if (i == max_choice) + for (i = 0; i < max_choice; i++) { + item_set(scroll + i); + j = first_alpha(item_str(), "YyNnMmHh"); + if (key == tolower(item_str()[j])) + break; + } + } + + if (item_count() != 0 && + (i < max_choice || + key == KEY_UP || key == KEY_DOWN || + key == '-' || key == '+' || + key == KEY_PPAGE || key == KEY_NPAGE)) { + /* Remove highligt of current item */ + print_item(scroll + choice, choice, FALSE); + + if (key == KEY_UP || key == '-') { + if (choice < 2 && scroll) { + /* Scroll menu down */ + do_scroll(menu, &scroll, -1); + + print_item(scroll, 0, FALSE); + } else + choice = MAX(choice - 1, 0); + + } else if (key == KEY_DOWN || key == '+') { + print_item(scroll+choice, choice, FALSE); + + if ((choice > max_choice - 3) && + (scroll + max_choice < item_count())) { + /* Scroll menu up */ + do_scroll(menu, &scroll, 1); + + print_item(scroll+max_choice - 1, + max_choice - 1, FALSE); + } else + choice = MIN(choice + 1, max_choice - 1); + + } else if (key == KEY_PPAGE) { + scrollok(menu, TRUE); + for (i = 0; (i < max_choice); i++) { + if (scroll > 0) { + do_scroll(menu, &scroll, -1); + print_item(scroll, 0, FALSE); + } else { + if (choice > 0) + choice--; + } + } + + } else if (key == KEY_NPAGE) { + for (i = 0; (i < max_choice); i++) { + if (scroll + max_choice < item_count()) { + do_scroll(menu, &scroll, 1); + print_item(scroll+max_choice-1, + max_choice - 1, FALSE); + } else { + if (choice + 1 < max_choice) + choice++; + } + } + } else + choice = i; + + print_item(scroll + choice, choice, TRUE); + + print_arrows(dialog, item_count(), scroll, + box_y, box_x + item_x + 1, menu_height); + + wnoutrefresh(dialog); + wrefresh(menu); + + continue; /* wait for another key press */ + } + + switch (key) { + case KEY_LEFT: + case TAB: + case KEY_RIGHT: + button = ((key == KEY_LEFT ? --button : ++button) < 0) + ? 4 : (button > 4 ? 0 : button); + + print_buttons(dialog, height, width, button); + wrefresh(menu); + break; + case ' ': + case 's': + case 'y': + case 'n': + case 'm': + case '/': + case 'h': + case '?': + case 'z': + case '\n': + /* save scroll info */ + *s_scroll = scroll; + delwin(menu); + delwin(dialog); + item_set(scroll + choice); + item_set_selected(1); + switch (key) { + case 'h': + case '?': + return 2; + case 's': + case 'y': + return 5; + case 'n': + return 6; + case 'm': + return 7; + case ' ': + return 8; + case '/': + return 9; + case 'z': + return 10; + case '\n': + return button; + } + return 0; + case 'e': + case 'x': + key = KEY_ESC; + break; + case KEY_ESC: + key = on_key_esc(menu); + break; + case KEY_RESIZE: + on_key_resize(); + delwin(menu); + delwin(dialog); + goto do_resize; + } + } + delwin(menu); + delwin(dialog); + return key; /* ESC pressed */ +} diff --git a/scripts/kconfig/lxdialog/textbox.c b/scripts/kconfig/lxdialog/textbox.c new file mode 100644 index 0000000000..1773319b95 --- /dev/null +++ b/scripts/kconfig/lxdialog/textbox.c @@ -0,0 +1,408 @@ +/* + * textbox.c -- implements the text box + * + * ORIGINAL AUTHOR: Savio Lam (lam836@cs.cuhk.hk) + * MODIFIED FOR LINUX KERNEL CONFIG BY: William Roadcap (roadcap@cfw.com) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "dialog.h" + +static void back_lines(int n); +static void print_page(WINDOW *win, int height, int width, update_text_fn + update_text, void *data); +static void print_line(WINDOW *win, int row, int width); +static char *get_line(void); +static void print_position(WINDOW * win); + +static int hscroll; +static int begin_reached, end_reached, page_length; +static char *buf; +static char *page; + +/* + * refresh window content + */ +static void refresh_text_box(WINDOW *dialog, WINDOW *box, int boxh, int boxw, + int cur_y, int cur_x, update_text_fn update_text, + void *data) +{ + print_page(box, boxh, boxw, update_text, data); + print_position(dialog); + wmove(dialog, cur_y, cur_x); /* Restore cursor position */ + wrefresh(dialog); +} + + +/* + * Display text from a file in a dialog box. + * + * keys is a null-terminated array + * update_text() may not add or remove any '\n' or '\0' in tbuf + */ +int dialog_textbox(const char *title, char *tbuf, int initial_height, + int initial_width, int *keys, int *_vscroll, int *_hscroll, + update_text_fn update_text, void *data) +{ + int i, x, y, cur_x, cur_y, key = 0; + int height, width, boxh, boxw; + WINDOW *dialog, *box; + bool done = false; + + begin_reached = 1; + end_reached = 0; + page_length = 0; + hscroll = 0; + buf = tbuf; + page = buf; /* page is pointer to start of page to be displayed */ + + if (_vscroll && *_vscroll) { + begin_reached = 0; + + for (i = 0; i < *_vscroll; i++) + get_line(); + } + if (_hscroll) + hscroll = *_hscroll; + +do_resize: + getmaxyx(stdscr, height, width); + if (height < TEXTBOX_HEIGTH_MIN || width < TEXTBOX_WIDTH_MIN) + return -ERRDISPLAYTOOSMALL; + if (initial_height != 0) + height = initial_height; + else + if (height > 4) + height -= 4; + else + height = 0; + if (initial_width != 0) + width = initial_width; + else + if (width > 5) + width -= 5; + else + width = 0; + + /* center dialog box on screen */ + x = (getmaxx(stdscr) - width) / 2; + y = (getmaxy(stdscr) - height) / 2; + + draw_shadow(stdscr, y, x, height, width); + + dialog = newwin(height, width, y, x); + keypad(dialog, TRUE); + + /* Create window for box region, used for scrolling text */ + boxh = height - 4; + boxw = width - 2; + box = subwin(dialog, boxh, boxw, y + 1, x + 1); + wattrset(box, dlg.dialog.atr); + wbkgdset(box, dlg.dialog.atr & A_COLOR); + + keypad(box, TRUE); + + /* register the new window, along with its borders */ + draw_box(dialog, 0, 0, height, width, + dlg.dialog.atr, dlg.border.atr); + + wattrset(dialog, dlg.border.atr); + mvwaddch(dialog, height - 3, 0, ACS_LTEE); + for (i = 0; i < width - 2; i++) + waddch(dialog, ACS_HLINE); + wattrset(dialog, dlg.dialog.atr); + wbkgdset(dialog, dlg.dialog.atr & A_COLOR); + waddch(dialog, ACS_RTEE); + + print_title(dialog, title, width); + + print_button(dialog, gettext(" Exit "), height - 2, width / 2 - 4, TRUE); + wnoutrefresh(dialog); + getyx(dialog, cur_y, cur_x); /* Save cursor position */ + + /* Print first page of text */ + attr_clear(box, boxh, boxw, dlg.dialog.atr); + refresh_text_box(dialog, box, boxh, boxw, cur_y, cur_x, update_text, + data); + + while (!done) { + key = wgetch(dialog); + switch (key) { + case 'E': /* Exit */ + case 'e': + case 'X': + case 'x': + case 'q': + case '\n': + done = true; + break; + case 'g': /* First page */ + case KEY_HOME: + if (!begin_reached) { + begin_reached = 1; + page = buf; + refresh_text_box(dialog, box, boxh, boxw, + cur_y, cur_x, update_text, + data); + } + break; + case 'G': /* Last page */ + case KEY_END: + + end_reached = 1; + /* point to last char in buf */ + page = buf + strlen(buf); + back_lines(boxh); + refresh_text_box(dialog, box, boxh, boxw, cur_y, + cur_x, update_text, data); + break; + case 'K': /* Previous line */ + case 'k': + case KEY_UP: + if (begin_reached) + break; + + back_lines(page_length + 1); + refresh_text_box(dialog, box, boxh, boxw, cur_y, + cur_x, update_text, data); + break; + case 'B': /* Previous page */ + case 'b': + case 'u': + case KEY_PPAGE: + if (begin_reached) + break; + back_lines(page_length + boxh); + refresh_text_box(dialog, box, boxh, boxw, cur_y, + cur_x, update_text, data); + break; + case 'J': /* Next line */ + case 'j': + case KEY_DOWN: + if (end_reached) + break; + + back_lines(page_length - 1); + refresh_text_box(dialog, box, boxh, boxw, cur_y, + cur_x, update_text, data); + break; + case KEY_NPAGE: /* Next page */ + case ' ': + case 'd': + if (end_reached) + break; + + begin_reached = 0; + refresh_text_box(dialog, box, boxh, boxw, cur_y, + cur_x, update_text, data); + break; + case '0': /* Beginning of line */ + case 'H': /* Scroll left */ + case 'h': + case KEY_LEFT: + if (hscroll <= 0) + break; + + if (key == '0') + hscroll = 0; + else + hscroll--; + /* Reprint current page to scroll horizontally */ + back_lines(page_length); + refresh_text_box(dialog, box, boxh, boxw, cur_y, + cur_x, update_text, data); + break; + case 'L': /* Scroll right */ + case 'l': + case KEY_RIGHT: + if (hscroll >= MAX_LEN) + break; + hscroll++; + /* Reprint current page to scroll horizontally */ + back_lines(page_length); + refresh_text_box(dialog, box, boxh, boxw, cur_y, + cur_x, update_text, data); + break; + case KEY_ESC: + if (on_key_esc(dialog) == KEY_ESC) + done = true; + break; + case KEY_RESIZE: + back_lines(height); + delwin(box); + delwin(dialog); + on_key_resize(); + goto do_resize; + default: + for (i = 0; keys[i]; i++) { + if (key == keys[i]) { + done = true; + break; + } + } + } + } + delwin(box); + delwin(dialog); + if (_vscroll) { + const char *s; + + s = buf; + *_vscroll = 0; + back_lines(page_length); + while (s < page && (s = strchr(s, '\n'))) { + (*_vscroll)++; + s++; + } + } + if (_hscroll) + *_hscroll = hscroll; + return key; +} + +/* + * Go back 'n' lines in text. Called by dialog_textbox(). + * 'page' will be updated to point to the desired line in 'buf'. + */ +static void back_lines(int n) +{ + int i; + + begin_reached = 0; + /* Go back 'n' lines */ + for (i = 0; i < n; i++) { + if (*page == '\0') { + if (end_reached) { + end_reached = 0; + continue; + } + } + if (page == buf) { + begin_reached = 1; + return; + } + page--; + do { + if (page == buf) { + begin_reached = 1; + return; + } + page--; + } while (*page != '\n'); + page++; + } +} + +/* + * Print a new page of text. + */ +static void print_page(WINDOW *win, int height, int width, update_text_fn + update_text, void *data) +{ + int i, passed_end = 0; + + if (update_text) { + char *end; + + for (i = 0; i < height; i++) + get_line(); + end = page; + back_lines(height); + update_text(buf, page - buf, end - buf, data); + } + + page_length = 0; + for (i = 0; i < height; i++) { + print_line(win, i, width); + if (!passed_end) + page_length++; + if (end_reached && !passed_end) + passed_end = 1; + } + wnoutrefresh(win); +} + +/* + * Print a new line of text. + */ +static void print_line(WINDOW * win, int row, int width) +{ + char *line; + + line = get_line(); + line += MIN(strlen(line), hscroll); /* Scroll horizontally */ + wmove(win, row, 0); /* move cursor to correct line */ + waddch(win, ' '); + waddnstr(win, line, MIN(strlen(line), width - 2)); + + /* Clear 'residue' of previous line */ +#if OLD_NCURSES + { + int x = getcurx(win); + int i; + for (i = 0; i < width - x; i++) + waddch(win, ' '); + } +#else + wclrtoeol(win); +#endif +} + +/* + * Return current line of text. Called by dialog_textbox() and print_line(). + * 'page' should point to start of current line before calling, and will be + * updated to point to start of next line. + */ +static char *get_line(void) +{ + int i = 0; + static char line[MAX_LEN + 1]; + + end_reached = 0; + while (*page != '\n') { + if (*page == '\0') { + end_reached = 1; + break; + } else if (i < MAX_LEN) + line[i++] = *(page++); + else { + /* Truncate lines longer than MAX_LEN characters */ + if (i == MAX_LEN) + line[i++] = '\0'; + page++; + } + } + if (i <= MAX_LEN) + line[i] = '\0'; + if (!end_reached) + page++; /* move past '\n' */ + + return line; +} + +/* + * Print current position + */ +static void print_position(WINDOW * win) +{ + int percent; + + wattrset(win, dlg.position_indicator.atr); + wbkgdset(win, dlg.position_indicator.atr & A_COLOR); + percent = (page - buf) * 100 / strlen(buf); + wmove(win, getmaxy(win) - 3, getmaxx(win) - 9); + wprintw(win, "(%3d%%)", percent); +} diff --git a/scripts/kconfig/lxdialog/util.c b/scripts/kconfig/lxdialog/util.c new file mode 100644 index 0000000000..f7abdeb92a --- /dev/null +++ b/scripts/kconfig/lxdialog/util.c @@ -0,0 +1,713 @@ +/* + * util.c + * + * ORIGINAL AUTHOR: Savio Lam (lam836@cs.cuhk.hk) + * MODIFIED FOR LINUX KERNEL CONFIG BY: William Roadcap (roadcap@cfw.com) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include + +#include "dialog.h" + +/* Needed in signal handler in mconf.c */ +int saved_x, saved_y; + +struct dialog_info dlg; + +static void set_mono_theme(void) +{ + dlg.screen.atr = A_NORMAL; + dlg.shadow.atr = A_NORMAL; + dlg.dialog.atr = A_NORMAL; + dlg.title.atr = A_BOLD; + dlg.border.atr = A_NORMAL; + dlg.button_active.atr = A_REVERSE; + dlg.button_inactive.atr = A_DIM; + dlg.button_key_active.atr = A_REVERSE; + dlg.button_key_inactive.atr = A_BOLD; + dlg.button_label_active.atr = A_REVERSE; + dlg.button_label_inactive.atr = A_NORMAL; + dlg.inputbox.atr = A_NORMAL; + dlg.inputbox_border.atr = A_NORMAL; + dlg.searchbox.atr = A_NORMAL; + dlg.searchbox_title.atr = A_BOLD; + dlg.searchbox_border.atr = A_NORMAL; + dlg.position_indicator.atr = A_BOLD; + dlg.menubox.atr = A_NORMAL; + dlg.menubox_border.atr = A_NORMAL; + dlg.item.atr = A_NORMAL; + dlg.item_selected.atr = A_REVERSE; + dlg.tag.atr = A_BOLD; + dlg.tag_selected.atr = A_REVERSE; + dlg.tag_key.atr = A_BOLD; + dlg.tag_key_selected.atr = A_REVERSE; + dlg.check.atr = A_BOLD; + dlg.check_selected.atr = A_REVERSE; + dlg.uarrow.atr = A_BOLD; + dlg.darrow.atr = A_BOLD; +} + +#define DLG_COLOR(dialog, f, b, h) \ +do { \ + dlg.dialog.fg = (f); \ + dlg.dialog.bg = (b); \ + dlg.dialog.hl = (h); \ +} while (0) + +static void set_classic_theme(void) +{ + DLG_COLOR(screen, COLOR_CYAN, COLOR_BLUE, true); + DLG_COLOR(shadow, COLOR_BLACK, COLOR_BLACK, true); + DLG_COLOR(dialog, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(title, COLOR_YELLOW, COLOR_WHITE, true); + DLG_COLOR(border, COLOR_WHITE, COLOR_WHITE, true); + DLG_COLOR(button_active, COLOR_WHITE, COLOR_BLUE, true); + DLG_COLOR(button_inactive, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(button_key_active, COLOR_WHITE, COLOR_BLUE, true); + DLG_COLOR(button_key_inactive, COLOR_RED, COLOR_WHITE, false); + DLG_COLOR(button_label_active, COLOR_YELLOW, COLOR_BLUE, true); + DLG_COLOR(button_label_inactive, COLOR_BLACK, COLOR_WHITE, true); + DLG_COLOR(inputbox, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(inputbox_border, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(searchbox, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(searchbox_title, COLOR_YELLOW, COLOR_WHITE, true); + DLG_COLOR(searchbox_border, COLOR_WHITE, COLOR_WHITE, true); + DLG_COLOR(position_indicator, COLOR_YELLOW, COLOR_WHITE, true); + DLG_COLOR(menubox, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(menubox_border, COLOR_WHITE, COLOR_WHITE, true); + DLG_COLOR(item, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(item_selected, COLOR_WHITE, COLOR_BLUE, true); + DLG_COLOR(tag, COLOR_YELLOW, COLOR_WHITE, true); + DLG_COLOR(tag_selected, COLOR_YELLOW, COLOR_BLUE, true); + DLG_COLOR(tag_key, COLOR_YELLOW, COLOR_WHITE, true); + DLG_COLOR(tag_key_selected, COLOR_YELLOW, COLOR_BLUE, true); + DLG_COLOR(check, COLOR_BLACK, COLOR_WHITE, false); + DLG_COLOR(check_selected, COLOR_WHITE, COLOR_BLUE, true); + DLG_COLOR(uarrow, COLOR_GREEN, COLOR_WHITE, true); + DLG_COLOR(darrow, COLOR_GREEN, COLOR_WHITE, true); +} + +static void set_blackbg_theme(void) +{ + DLG_COLOR(screen, COLOR_RED, COLOR_BLACK, true); + DLG_COLOR(shadow, COLOR_BLACK, COLOR_BLACK, false); + DLG_COLOR(dialog, COLOR_WHITE, COLOR_BLACK, false); + DLG_COLOR(title, COLOR_RED, COLOR_BLACK, false); + DLG_COLOR(border, COLOR_BLACK, COLOR_BLACK, true); + + DLG_COLOR(button_active, COLOR_YELLOW, COLOR_RED, false); + DLG_COLOR(button_inactive, COLOR_YELLOW, COLOR_BLACK, false); + DLG_COLOR(button_key_active, COLOR_YELLOW, COLOR_RED, true); + DLG_COLOR(button_key_inactive, COLOR_RED, COLOR_BLACK, false); + DLG_COLOR(button_label_active, COLOR_WHITE, COLOR_RED, false); + DLG_COLOR(button_label_inactive, COLOR_BLACK, COLOR_BLACK, true); + + DLG_COLOR(inputbox, COLOR_YELLOW, COLOR_BLACK, false); + DLG_COLOR(inputbox_border, COLOR_YELLOW, COLOR_BLACK, false); + + DLG_COLOR(searchbox, COLOR_YELLOW, COLOR_BLACK, false); + DLG_COLOR(searchbox_title, COLOR_YELLOW, COLOR_BLACK, true); + DLG_COLOR(searchbox_border, COLOR_BLACK, COLOR_BLACK, true); + + DLG_COLOR(position_indicator, COLOR_RED, COLOR_BLACK, false); + + DLG_COLOR(menubox, COLOR_YELLOW, COLOR_BLACK, false); + DLG_COLOR(menubox_border, COLOR_BLACK, COLOR_BLACK, true); + + DLG_COLOR(item, COLOR_WHITE, COLOR_BLACK, false); + DLG_COLOR(item_selected, COLOR_WHITE, COLOR_RED, false); + + DLG_COLOR(tag, COLOR_RED, COLOR_BLACK, false); + DLG_COLOR(tag_selected, COLOR_YELLOW, COLOR_RED, true); + DLG_COLOR(tag_key, COLOR_RED, COLOR_BLACK, false); + DLG_COLOR(tag_key_selected, COLOR_YELLOW, COLOR_RED, true); + + DLG_COLOR(check, COLOR_YELLOW, COLOR_BLACK, false); + DLG_COLOR(check_selected, COLOR_YELLOW, COLOR_RED, true); + + DLG_COLOR(uarrow, COLOR_RED, COLOR_BLACK, false); + DLG_COLOR(darrow, COLOR_RED, COLOR_BLACK, false); +} + +static void set_bluetitle_theme(void) +{ + set_classic_theme(); + DLG_COLOR(title, COLOR_BLUE, COLOR_WHITE, true); + DLG_COLOR(button_key_active, COLOR_YELLOW, COLOR_BLUE, true); + DLG_COLOR(button_label_active, COLOR_WHITE, COLOR_BLUE, true); + DLG_COLOR(searchbox_title, COLOR_BLUE, COLOR_WHITE, true); + DLG_COLOR(position_indicator, COLOR_BLUE, COLOR_WHITE, true); + DLG_COLOR(tag, COLOR_BLUE, COLOR_WHITE, true); + DLG_COLOR(tag_key, COLOR_BLUE, COLOR_WHITE, true); + +} + +/* + * Select color theme + */ +static int set_theme(const char *theme) +{ + int use_color = 1; + if (!theme) + set_bluetitle_theme(); + else if (strcmp(theme, "classic") == 0) + set_classic_theme(); + else if (strcmp(theme, "bluetitle") == 0) + set_bluetitle_theme(); + else if (strcmp(theme, "blackbg") == 0) + set_blackbg_theme(); + else if (strcmp(theme, "mono") == 0) + use_color = 0; + + return use_color; +} + +static void init_one_color(struct dialog_color *color) +{ + static int pair = 0; + + pair++; + init_pair(pair, color->fg, color->bg); + if (color->hl) + color->atr = A_BOLD | COLOR_PAIR(pair); + else + color->atr = COLOR_PAIR(pair); +} + +static void init_dialog_colors(void) +{ + init_one_color(&dlg.screen); + init_one_color(&dlg.shadow); + init_one_color(&dlg.dialog); + init_one_color(&dlg.title); + init_one_color(&dlg.border); + init_one_color(&dlg.button_active); + init_one_color(&dlg.button_inactive); + init_one_color(&dlg.button_key_active); + init_one_color(&dlg.button_key_inactive); + init_one_color(&dlg.button_label_active); + init_one_color(&dlg.button_label_inactive); + init_one_color(&dlg.inputbox); + init_one_color(&dlg.inputbox_border); + init_one_color(&dlg.searchbox); + init_one_color(&dlg.searchbox_title); + init_one_color(&dlg.searchbox_border); + init_one_color(&dlg.position_indicator); + init_one_color(&dlg.menubox); + init_one_color(&dlg.menubox_border); + init_one_color(&dlg.item); + init_one_color(&dlg.item_selected); + init_one_color(&dlg.tag); + init_one_color(&dlg.tag_selected); + init_one_color(&dlg.tag_key); + init_one_color(&dlg.tag_key_selected); + init_one_color(&dlg.check); + init_one_color(&dlg.check_selected); + init_one_color(&dlg.uarrow); + init_one_color(&dlg.darrow); +} + +/* + * Setup for color display + */ +static void color_setup(const char *theme) +{ + int use_color; + + use_color = set_theme(theme); + if (use_color && has_colors()) { + start_color(); + init_dialog_colors(); + } else + set_mono_theme(); +} + +/* + * Set window to attribute 'attr' + */ +void attr_clear(WINDOW * win, int height, int width, chtype attr) +{ + int i, j; + + wattrset(win, attr); + for (i = 0; i < height; i++) { + wmove(win, i, 0); + for (j = 0; j < width; j++) + waddch(win, ' '); + } + touchwin(win); +} + +void dialog_clear(void) +{ + int lines, columns; + + lines = getmaxy(stdscr); + columns = getmaxx(stdscr); + + attr_clear(stdscr, lines, columns, dlg.screen.atr); + /* Display background title if it exists ... - SLH */ + if (dlg.backtitle != NULL) { + int i, len = 0, skip = 0; + struct subtitle_list *pos; + + wattrset(stdscr, dlg.screen.atr); + mvwaddstr(stdscr, 0, 1, (char *)dlg.backtitle); + + for (pos = dlg.subtitles; pos != NULL; pos = pos->next) { + /* 3 is for the arrow and spaces */ + len += strlen(pos->text) + 3; + } + + wmove(stdscr, 1, 1); + if (len > columns - 2) { + const char *ellipsis = "[...] "; + waddstr(stdscr, ellipsis); + skip = len - (columns - 2 - strlen(ellipsis)); + } + + for (pos = dlg.subtitles; pos != NULL; pos = pos->next) { + if (skip == 0) + waddch(stdscr, ACS_RARROW); + else + skip--; + + if (skip == 0) + waddch(stdscr, ' '); + else + skip--; + + if (skip < strlen(pos->text)) { + waddstr(stdscr, pos->text + skip); + skip = 0; + } else + skip -= strlen(pos->text); + + if (skip == 0) + waddch(stdscr, ' '); + else + skip--; + } + + for (i = len + 1; i < columns - 1; i++) + waddch(stdscr, ACS_HLINE); + } + wnoutrefresh(stdscr); +} + +/* + * Do some initialization for dialog + */ +int init_dialog(const char *backtitle) +{ + int height, width; + + initscr(); /* Init curses */ + + /* Get current cursor position for signal handler in mconf.c */ + getyx(stdscr, saved_y, saved_x); + + getmaxyx(stdscr, height, width); + if (height < WINDOW_HEIGTH_MIN || width < WINDOW_WIDTH_MIN) { + endwin(); + return -ERRDISPLAYTOOSMALL; + } + + dlg.backtitle = backtitle; + color_setup(getenv("MENUCONFIG_COLOR")); + + keypad(stdscr, TRUE); + cbreak(); + noecho(); + dialog_clear(); + + return 0; +} + +void set_dialog_backtitle(const char *backtitle) +{ + dlg.backtitle = backtitle; +} + +void set_dialog_subtitles(struct subtitle_list *subtitles) +{ + dlg.subtitles = subtitles; +} + +/* + * End using dialog functions. + */ +void end_dialog(int x, int y) +{ + /* move cursor back to original position */ + move(y, x); + refresh(); + endwin(); +} + +/* Print the title of the dialog. Center the title and truncate + * tile if wider than dialog (- 2 chars). + **/ +void print_title(WINDOW *dialog, const char *title, int width) +{ + if (title) { + int tlen = MIN(width - 2, strlen(title)); + wattrset(dialog, dlg.title.atr); + mvwaddch(dialog, 0, (width - tlen) / 2 - 1, ' '); + mvwaddnstr(dialog, 0, (width - tlen)/2, title, tlen); + waddch(dialog, ' '); + } +} + +/* + * Print a string of text in a window, automatically wrap around to the + * next line if the string is too long to fit on one line. Newline + * characters '\n' are propperly processed. We start on a new line + * if there is no room for at least 4 nonblanks following a double-space. + */ +void print_autowrap(WINDOW * win, const char *prompt, int width, int y, int x) +{ + int newl, cur_x, cur_y; + int prompt_len, room, wlen; + char tempstr[MAX_LEN + 1], *word, *sp, *sp2, *newline_separator = 0; + + strcpy(tempstr, prompt); + + prompt_len = strlen(tempstr); + + if (prompt_len <= width - x * 2) { /* If prompt is short */ + wmove(win, y, (width - prompt_len) / 2); + waddstr(win, tempstr); + } else { + cur_x = x; + cur_y = y; + newl = 1; + word = tempstr; + while (word && *word) { + sp = strpbrk(word, "\n "); + if (sp && *sp == '\n') + newline_separator = sp; + + if (sp) + *sp++ = 0; + + /* Wrap to next line if either the word does not fit, + or it is the first word of a new sentence, and it is + short, and the next word does not fit. */ + room = width - cur_x; + wlen = strlen(word); + if (wlen > room || + (newl && wlen < 4 && sp + && wlen + 1 + strlen(sp) > room + && (!(sp2 = strpbrk(sp, "\n ")) + || wlen + 1 + (sp2 - sp) > room))) { + cur_y++; + cur_x = x; + } + wmove(win, cur_y, cur_x); + waddstr(win, word); + getyx(win, cur_y, cur_x); + + /* Move to the next line if the word separator was a newline */ + if (newline_separator) { + cur_y++; + cur_x = x; + newline_separator = 0; + } else + cur_x++; + + if (sp && *sp == ' ') { + cur_x++; /* double space */ + while (*++sp == ' ') ; + newl = 1; + } else + newl = 0; + word = sp; + } + } +} + +/* + * Print a button + */ +void print_button(WINDOW * win, const char *label, int y, int x, int selected) +{ + int i, temp; + + wmove(win, y, x); + wattrset(win, selected ? dlg.button_active.atr + : dlg.button_inactive.atr); + waddstr(win, "<"); + temp = strspn(label, " "); + label += temp; + wattrset(win, selected ? dlg.button_label_active.atr + : dlg.button_label_inactive.atr); + for (i = 0; i < temp; i++) + waddch(win, ' '); + wattrset(win, selected ? dlg.button_key_active.atr + : dlg.button_key_inactive.atr); + waddch(win, label[0]); + wattrset(win, selected ? dlg.button_label_active.atr + : dlg.button_label_inactive.atr); + waddstr(win, (char *)label + 1); + wattrset(win, selected ? dlg.button_active.atr + : dlg.button_inactive.atr); + waddstr(win, ">"); + wmove(win, y, x + temp + 1); +} + +/* + * Draw a rectangular box with line drawing characters + */ +void +draw_box(WINDOW * win, int y, int x, int height, int width, + chtype box, chtype border) +{ + int i, j; + + wattrset(win, 0); + for (i = 0; i < height; i++) { + wmove(win, y + i, x); + for (j = 0; j < width; j++) + if (!i && !j) + waddch(win, border | ACS_ULCORNER); + else if (i == height - 1 && !j) + waddch(win, border | ACS_LLCORNER); + else if (!i && j == width - 1) + waddch(win, box | ACS_URCORNER); + else if (i == height - 1 && j == width - 1) + waddch(win, box | ACS_LRCORNER); + else if (!i) + waddch(win, border | ACS_HLINE); + else if (i == height - 1) + waddch(win, box | ACS_HLINE); + else if (!j) + waddch(win, border | ACS_VLINE); + else if (j == width - 1) + waddch(win, box | ACS_VLINE); + else + waddch(win, box | ' '); + } +} + +/* + * Draw shadows along the right and bottom edge to give a more 3D look + * to the boxes + */ +void draw_shadow(WINDOW * win, int y, int x, int height, int width) +{ + int i; + + if (has_colors()) { /* Whether terminal supports color? */ + wattrset(win, dlg.shadow.atr); + wmove(win, y + height, x + 2); + for (i = 0; i < width; i++) + waddch(win, winch(win) & A_CHARTEXT); + for (i = y + 1; i < y + height + 1; i++) { + wmove(win, i, x + width); + waddch(win, winch(win) & A_CHARTEXT); + waddch(win, winch(win) & A_CHARTEXT); + } + wnoutrefresh(win); + } +} + +/* + * Return the position of the first alphabetic character in a string. + */ +int first_alpha(const char *string, const char *exempt) +{ + int i, in_paren = 0, c; + + for (i = 0; i < strlen(string); i++) { + c = tolower(string[i]); + + if (strchr("<[(", c)) + ++in_paren; + if (strchr(">])", c) && in_paren > 0) + --in_paren; + + if ((!in_paren) && isalpha(c) && strchr(exempt, c) == 0) + return i; + } + + return 0; +} + +/* + * ncurses uses ESC to detect escaped char sequences. This resutl in + * a small timeout before ESC is actually delivered to the application. + * lxdialog suggest which is correctly translated to two + * times esc. But then we need to ignore the second esc to avoid stepping + * out one menu too much. Filter away all escaped key sequences since + * keypad(FALSE) turn off ncurses support for escape sequences - and thats + * needed to make notimeout() do as expected. + */ +int on_key_esc(WINDOW *win) +{ + int key; + int key2; + int key3; + + nodelay(win, TRUE); + keypad(win, FALSE); + key = wgetch(win); + key2 = wgetch(win); + do { + key3 = wgetch(win); + } while (key3 != ERR); + nodelay(win, FALSE); + keypad(win, TRUE); + if (key == KEY_ESC && key2 == ERR) + return KEY_ESC; + else if (key != ERR && key != KEY_ESC && key2 == ERR) + ungetch(key); + + return -1; +} + +/* redraw screen in new size */ +int on_key_resize(void) +{ + dialog_clear(); + return KEY_RESIZE; +} + +struct dialog_list *item_cur; +struct dialog_list item_nil; +struct dialog_list *item_head; + +void item_reset(void) +{ + struct dialog_list *p, *next; + + for (p = item_head; p; p = next) { + next = p->next; + free(p); + } + item_head = NULL; + item_cur = &item_nil; +} + +void item_make(const char *fmt, ...) +{ + va_list ap; + struct dialog_list *p = malloc(sizeof(*p)); + + if (item_head) + item_cur->next = p; + else + item_head = p; + item_cur = p; + memset(p, 0, sizeof(*p)); + + va_start(ap, fmt); + vsnprintf(item_cur->node.str, sizeof(item_cur->node.str), fmt, ap); + va_end(ap); +} + +void item_add_str(const char *fmt, ...) +{ + va_list ap; + size_t avail; + + avail = sizeof(item_cur->node.str) - strlen(item_cur->node.str); + + va_start(ap, fmt); + vsnprintf(item_cur->node.str + strlen(item_cur->node.str), + avail, fmt, ap); + item_cur->node.str[sizeof(item_cur->node.str) - 1] = '\0'; + va_end(ap); +} + +void item_set_tag(char tag) +{ + item_cur->node.tag = tag; +} +void item_set_data(void *ptr) +{ + item_cur->node.data = ptr; +} + +void item_set_selected(int val) +{ + item_cur->node.selected = val; +} + +int item_activate_selected(void) +{ + item_foreach() + if (item_is_selected()) + return 1; + return 0; +} + +void *item_data(void) +{ + return item_cur->node.data; +} + +char item_tag(void) +{ + return item_cur->node.tag; +} + +int item_count(void) +{ + int n = 0; + struct dialog_list *p; + + for (p = item_head; p; p = p->next) + n++; + return n; +} + +void item_set(int n) +{ + int i = 0; + item_foreach() + if (i++ == n) + return; +} + +int item_n(void) +{ + int n = 0; + struct dialog_list *p; + + for (p = item_head; p; p = p->next) { + if (p == item_cur) + return n; + n++; + } + return 0; +} + +const char *item_str(void) +{ + return item_cur->node.str; +} + +int item_is_selected(void) +{ + return (item_cur->node.selected != 0); +} + +int item_is_tag(char tag) +{ + return (item_cur->node.tag == tag); +} diff --git a/scripts/kconfig/lxdialog/yesno.c b/scripts/kconfig/lxdialog/yesno.c new file mode 100644 index 0000000000..676fb2f824 --- /dev/null +++ b/scripts/kconfig/lxdialog/yesno.c @@ -0,0 +1,114 @@ +/* + * yesno.c -- implements the yes/no box + * + * ORIGINAL AUTHOR: Savio Lam (lam836@cs.cuhk.hk) + * MODIFIED FOR LINUX KERNEL CONFIG BY: William Roadcap (roadcap@cfw.com) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "dialog.h" + +/* + * Display termination buttons + */ +static void print_buttons(WINDOW * dialog, int height, int width, int selected) +{ + int x = width / 2 - 10; + int y = height - 2; + + print_button(dialog, gettext(" Yes "), y, x, selected == 0); + print_button(dialog, gettext(" No "), y, x + 13, selected == 1); + + wmove(dialog, y, x + 1 + 13 * selected); + wrefresh(dialog); +} + +/* + * Display a dialog box with two buttons - Yes and No + */ +int dialog_yesno(const char *title, const char *prompt, int height, int width) +{ + int i, x, y, key = 0, button = 0; + WINDOW *dialog; + +do_resize: + if (getmaxy(stdscr) < (height + YESNO_HEIGTH_MIN)) + return -ERRDISPLAYTOOSMALL; + if (getmaxx(stdscr) < (width + YESNO_WIDTH_MIN)) + return -ERRDISPLAYTOOSMALL; + + /* center dialog box on screen */ + x = (getmaxx(stdscr) - width) / 2; + y = (getmaxy(stdscr) - height) / 2; + + draw_shadow(stdscr, y, x, height, width); + + dialog = newwin(height, width, y, x); + keypad(dialog, TRUE); + + draw_box(dialog, 0, 0, height, width, + dlg.dialog.atr, dlg.border.atr); + wattrset(dialog, dlg.border.atr); + mvwaddch(dialog, height - 3, 0, ACS_LTEE); + for (i = 0; i < width - 2; i++) + waddch(dialog, ACS_HLINE); + wattrset(dialog, dlg.dialog.atr); + waddch(dialog, ACS_RTEE); + + print_title(dialog, title, width); + + wattrset(dialog, dlg.dialog.atr); + print_autowrap(dialog, prompt, width - 2, 1, 3); + + print_buttons(dialog, height, width, 0); + + while (key != KEY_ESC) { + key = wgetch(dialog); + switch (key) { + case 'Y': + case 'y': + delwin(dialog); + return 0; + case 'N': + case 'n': + delwin(dialog); + return 1; + + case TAB: + case KEY_LEFT: + case KEY_RIGHT: + button = ((key == KEY_LEFT ? --button : ++button) < 0) ? 1 : (button > 1 ? 0 : button); + + print_buttons(dialog, height, width, button); + wrefresh(dialog); + break; + case ' ': + case '\n': + delwin(dialog); + return button; + case KEY_ESC: + key = on_key_esc(dialog); + break; + case KEY_RESIZE: + delwin(dialog); + on_key_resize(); + goto do_resize; + } + } + + delwin(dialog); + return key; /* ESC pressed */ +} diff --git a/scripts/kconfig/mconf.c b/scripts/kconfig/mconf.c new file mode 100644 index 0000000000..315ce2c7cb --- /dev/null +++ b/scripts/kconfig/mconf.c @@ -0,0 +1,1047 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + * + * Introduced single menu mode (show all sub-menus in one large tree). + * 2002-11-06 Petr Baudis + * + * i18n, 2005, Arnaldo Carvalho de Melo + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lkc.h" +#include "lxdialog/dialog.h" + +static const char mconf_readme[] = N_( +"Overview\n" +"--------\n" +"This interface lets you select features and parameters for the build.\n" +"Features can either be built-in, modularized, or ignored. Parameters\n" +"must be entered in as decimal or hexadecimal numbers or text.\n" +"\n" +"Menu items beginning with following braces represent features that\n" +" [ ] can be built in or removed\n" +" < > can be built in, modularized or removed\n" +" { } can be built in or modularized (selected by other feature)\n" +" - - are selected by other feature,\n" +"while *, M or whitespace inside braces means to build in, build as\n" +"a module or to exclude the feature respectively.\n" +"\n" +"To change any of these features, highlight it with the cursor\n" +"keys and press to build it in, to make it a module or\n" +" to remove it. You may also press the to cycle\n" +"through the available options (i.e. Y->N->M->Y).\n" +"\n" +"Some additional keyboard hints:\n" +"\n" +"Menus\n" +"----------\n" +"o Use the Up/Down arrow keys (cursor keys) to highlight the item you\n" +" wish to change or the submenu you wish to select and press .\n" +" Submenus are designated by \"--->\", empty ones by \"----\".\n" +"\n" +" Shortcut: Press the option's highlighted letter (hotkey).\n" +" Pressing a hotkey more than once will sequence\n" +" through all visible items which use that hotkey.\n" +"\n" +" You may also use the and keys to scroll\n" +" unseen options into view.\n" +"\n" +"o To exit a menu use the cursor keys to highlight the button\n" +" and press .\n" +"\n" +" Shortcut: Press or or if there is no hotkey\n" +" using those letters. You may press a single , but\n" +" there is a delayed response which you may find annoying.\n" +"\n" +" Also, the and cursor keys will cycle between and\n" +" \n" +"\n" +"\n" +"Data Entry\n" +"-----------\n" +"o Enter the requested information and press \n" +" If you are entering hexadecimal values, it is not necessary to\n" +" add the '0x' prefix to the entry.\n" +"\n" +"o For help, use the or cursor keys to highlight the help option\n" +" and press . You can try as well.\n" +"\n" +"\n" +"Text Box (Help Window)\n" +"--------\n" +"o Use the cursor keys to scroll up/down/left/right. The VI editor\n" +" keys h,j,k,l function here as do , , and for\n" +" those who are familiar with less and lynx.\n" +"\n" +"o Press , , , or to exit.\n" +"\n" +"\n" +"Alternate Configuration Files\n" +"-----------------------------\n" +"Menuconfig supports the use of alternate configuration files for\n" +"those who, for various reasons, find it necessary to switch\n" +"between different configurations.\n" +"\n" +"The button will let you save the current configuration to\n" +"a file of your choosing. Use the button to load a previously\n" +"saved alternate configuration.\n" +"\n" +"Even if you don't use alternate configuration files, but you find\n" +"during a Menuconfig session that you have completely messed up your\n" +"settings, you may use the button to restore your previously\n" +"saved settings from \".config\" without restarting Menuconfig.\n" +"\n" +"Other information\n" +"-----------------\n" +"If you use Menuconfig in an XTERM window, make sure you have your\n" +"$TERM variable set to point to an xterm definition which supports\n" +"color. Otherwise, Menuconfig will look rather bad. Menuconfig will\n" +"not display correctly in an RXVT window because rxvt displays only one\n" +"intensity of color, bright.\n" +"\n" +"Menuconfig will display larger menus on screens or xterms which are\n" +"set to display more than the standard 25 row by 80 column geometry.\n" +"In order for this to work, the \"stty size\" command must be able to\n" +"display the screen's current row and column geometry. I STRONGLY\n" +"RECOMMEND that you make sure you do NOT have the shell variables\n" +"LINES and COLUMNS exported into your environment. Some distributions\n" +"export those variables via /etc/profile. Some ncurses programs can\n" +"become confused when those variables (LINES & COLUMNS) don't reflect\n" +"the true screen size.\n" +"\n" +"Optional personality available\n" +"------------------------------\n" +"If you prefer to have all of the options listed in a single menu,\n" +"rather than the default multimenu hierarchy, run the menuconfig with\n" +"MENUCONFIG_MODE environment variable set to single_menu. Example:\n" +"\n" +"make MENUCONFIG_MODE=single_menu menuconfig\n" +"\n" +" will then unroll the appropriate category, or enfold it if it\n" +"is already unrolled.\n" +"\n" +"Note that this mode can eventually be a little more CPU expensive\n" +"(especially with a larger number of unrolled categories) than the\n" +"default mode.\n" +"\n" +"Different color themes available\n" +"--------------------------------\n" +"It is possible to select different color themes using the variable\n" +"MENUCONFIG_COLOR. To select a theme use:\n" +"\n" +"make MENUCONFIG_COLOR= menuconfig\n" +"\n" +"Available themes are\n" +" mono => selects colors suitable for monochrome displays\n" +" blackbg => selects a color scheme with black background\n" +" classic => theme with blue background. The classic look\n" +" bluetitle => an LCD friendly version of classic. (default)\n" +"\n"), +menu_instructions[] = N_( + "Arrow keys navigate the menu. " + " selects submenus ---> (or empty submenus ----). " + "Highlighted letters are hotkeys. " + "Pressing includes, excludes, modularizes features. " + "Press to exit, for Help, for Search. " + "Legend: [*] built-in [ ] excluded module < > module capable"), +radiolist_instructions[] = N_( + "Use the arrow keys to navigate this window or " + "press the hotkey of the item you wish to select " + "followed by the . " + "Press for additional information about this option."), +inputbox_instructions_int[] = N_( + "Please enter a decimal value. " + "Fractions will not be accepted. " + "Use the key to move from the input field to the buttons below it."), +inputbox_instructions_hex[] = N_( + "Please enter a hexadecimal value. " + "Use the key to move from the input field to the buttons below it."), +inputbox_instructions_string[] = N_( + "Please enter a string value. " + "Use the key to move from the input field to the buttons below it."), +setmod_text[] = N_( + "This feature depends on another which has been configured as a module.\n" + "As a result, this feature will be built as a module."), +load_config_text[] = N_( + "Enter the name of the configuration file you wish to load. " + "Accept the name shown to restore the configuration you " + "last retrieved. Leave blank to abort."), +load_config_help[] = N_( + "\n" + "For various reasons, one may wish to keep several different\n" + "configurations available on a single machine.\n" + "\n" + "If you have saved a previous configuration in a file other than the\n" + "default one, entering its name here will allow you to modify that\n" + "configuration.\n" + "\n" + "If you are uncertain, then you have probably never used alternate\n" + "configuration files. You should therefore leave this blank to abort.\n"), +save_config_text[] = N_( + "Enter a filename to which this configuration should be saved " + "as an alternate. Leave blank to abort."), +save_config_help[] = N_( + "\n" + "For various reasons, one may wish to keep different configurations\n" + "available on a single machine.\n" + "\n" + "Entering a file name here will allow you to later retrieve, modify\n" + "and use the current configuration as an alternate to whatever\n" + "configuration options you have selected at that time.\n" + "\n" + "If you are uncertain what all this means then you should probably\n" + "leave this blank.\n"), +search_help[] = N_( + "\n" + "Search for symbols and display their relations.\n" + "Regular expressions are allowed.\n" + "Example: search for \"^FOO\"\n" + "Result:\n" + "-----------------------------------------------------------------\n" + "Symbol: FOO [=m]\n" + "Type : tristate\n" + "Prompt: Foo bus is used to drive the bar HW\n" + " Location:\n" + " -> Bus options (PCI, PCMCIA, EISA, ISA)\n" + " -> PCI support (PCI [=y])\n" + "(1) -> PCI access mode ( [=y])\n" + " Defined at drivers/pci/Kconfig:47\n" + " Depends on: X86_LOCAL_APIC && X86_IO_APIC || IA64\n" + " Selects: LIBCRC32\n" + " Selected by: BAR [=n]\n" + "-----------------------------------------------------------------\n" + "o The line 'Type:' shows the type of the configuration option for\n" + " this symbol (boolean, tristate, string, ...)\n" + "o The line 'Prompt:' shows the text used in the menu structure for\n" + " this symbol\n" + "o The 'Defined at' line tells at what file / line number the symbol\n" + " is defined\n" + "o The 'Depends on:' line tells what symbols need to be defined for\n" + " this symbol to be visible in the menu (selectable)\n" + "o The 'Location:' lines tells where in the menu structure this symbol\n" + " is located\n" + " A location followed by a [=y] indicates that this is a\n" + " selectable menu item - and the current value is displayed inside\n" + " brackets.\n" + " Press the key in the (#) prefix to jump directly to that\n" + " location. You will be returned to the current search results\n" + " after exiting this new menu.\n" + "o The 'Selects:' line tells what symbols will be automatically\n" + " selected if this symbol is selected (y or m)\n" + "o The 'Selected by' line tells what symbol has selected this symbol\n" + "\n" + "Only relevant lines are shown.\n" + "\n\n" + "Search examples:\n" + "Examples: USB => find all symbols containing USB\n" + " ^USB => find all symbols starting with USB\n" + " USB$ => find all symbols ending with USB\n" + "\n"); + +static int indent; +static struct menu *current_menu; +static int child_count; +static int single_menu_mode; +static int show_all_options; +static int save_and_exit; +static int silent; + +static void conf(struct menu *menu, struct menu *active_menu); +static void conf_choice(struct menu *menu); +static void conf_string(struct menu *menu); +static void conf_load(void); +static void conf_save(void); +static int show_textbox_ext(const char *title, char *text, int r, int c, + int *keys, int *vscroll, int *hscroll, + update_text_fn update_text, void *data); +static void show_textbox(const char *title, const char *text, int r, int c); +static void show_helptext(const char *title, const char *text); +static void show_help(struct menu *menu); + +static char filename[PATH_MAX+1]; +static void set_config_filename(const char *config_filename) +{ + static char menu_backtitle[PATH_MAX+128]; + int size; + + size = snprintf(menu_backtitle, sizeof(menu_backtitle), + "%s - %s", config_filename, rootmenu.prompt->text); + if (size >= sizeof(menu_backtitle)) + menu_backtitle[sizeof(menu_backtitle)-1] = '\0'; + set_dialog_backtitle(menu_backtitle); + + size = snprintf(filename, sizeof(filename), "%s", config_filename); + if (size >= sizeof(filename)) + filename[sizeof(filename)-1] = '\0'; +} + +struct subtitle_part { + struct list_head entries; + const char *text; +}; +static LIST_HEAD(trail); + +static struct subtitle_list *subtitles; +static void set_subtitle(void) +{ + struct subtitle_part *sp; + struct subtitle_list *pos, *tmp; + + for (pos = subtitles; pos != NULL; pos = tmp) { + tmp = pos->next; + free(pos); + } + + subtitles = NULL; + list_for_each_entry(sp, &trail, entries) { + if (sp->text) { + if (pos) { + pos->next = xcalloc(1, sizeof(*pos)); + pos = pos->next; + } else { + subtitles = pos = xcalloc(1, sizeof(*pos)); + } + pos->text = sp->text; + } + } + + set_dialog_subtitles(subtitles); +} + +static void reset_subtitle(void) +{ + struct subtitle_list *pos, *tmp; + + for (pos = subtitles; pos != NULL; pos = tmp) { + tmp = pos->next; + free(pos); + } + subtitles = NULL; + set_dialog_subtitles(subtitles); +} + +struct search_data { + struct list_head *head; + struct menu **targets; + int *keys; +}; + +static void update_text(char *buf, size_t start, size_t end, void *_data) +{ + struct search_data *data = _data; + struct jump_key *pos; + int k = 0; + + list_for_each_entry(pos, data->head, entries) { + if (pos->offset >= start && pos->offset < end) { + char header[4]; + + if (k < JUMP_NB) { + int key = '0' + (pos->index % JUMP_NB) + 1; + + sprintf(header, "(%c)", key); + data->keys[k] = key; + data->targets[k] = pos->target; + k++; + } else { + sprintf(header, " "); + } + + memcpy(buf + pos->offset, header, sizeof(header) - 1); + } + } + data->keys[k] = 0; +} + +static void search_conf(void) +{ + struct symbol **sym_arr; + struct gstr res; + struct gstr title; + char *dialog_input; + int dres, vscroll = 0, hscroll = 0; + bool again; + struct gstr sttext; + struct subtitle_part stpart; + + title = str_new(); + str_printf( &title, _("Enter (sub)string or regexp to search for " + "(with or without \"%s\")"), CONFIG_); + +again: + dialog_clear(); + dres = dialog_inputbox(_("Search Configuration Parameter"), + str_get(&title), + 10, 75, ""); + switch (dres) { + case 0: + break; + case 1: + show_helptext(_("Search Configuration"), search_help); + goto again; + default: + str_free(&title); + return; + } + + /* strip the prefix if necessary */ + dialog_input = dialog_input_result; + if (strncasecmp(dialog_input_result, CONFIG_, strlen(CONFIG_)) == 0) + dialog_input += strlen(CONFIG_); + + sttext = str_new(); + str_printf(&sttext, "Search (%s)", dialog_input_result); + stpart.text = str_get(&sttext); + list_add_tail(&stpart.entries, &trail); + + sym_arr = sym_re_search(dialog_input); + do { + LIST_HEAD(head); + struct menu *targets[JUMP_NB]; + int keys[JUMP_NB + 1], i; + struct search_data data = { + .head = &head, + .targets = targets, + .keys = keys, + }; + struct jump_key *pos, *tmp; + + res = get_relations_str(sym_arr, &head); + set_subtitle(); + dres = show_textbox_ext(_("Search Results"), (char *) + str_get(&res), 0, 0, keys, &vscroll, + &hscroll, &update_text, (void *) + &data); + again = false; + for (i = 0; i < JUMP_NB && keys[i]; i++) + if (dres == keys[i]) { + conf(targets[i]->parent, targets[i]); + again = true; + } + str_free(&res); + list_for_each_entry_safe(pos, tmp, &head, entries) + free(pos); + } while (again); + free(sym_arr); + str_free(&title); + list_del(trail.prev); + str_free(&sttext); +} + +static void build_conf(struct menu *menu) +{ + struct symbol *sym; + struct property *prop; + struct menu *child; + int type, tmp, doint = 2; + tristate val; + char ch; + bool visible; + + /* + * note: menu_is_visible() has side effect that it will + * recalc the value of the symbol. + */ + visible = menu_is_visible(menu); + if (show_all_options && !menu_has_prompt(menu)) + return; + else if (!show_all_options && !visible) + return; + + sym = menu->sym; + prop = menu->prompt; + if (!sym) { + if (prop && menu != current_menu) { + const char *prompt = menu_get_prompt(menu); + switch (prop->type) { + case P_MENU: + child_count++; + prompt = _(prompt); + if (single_menu_mode) { + item_make("%s%*c%s", + menu->data ? "-->" : "++>", + indent + 1, ' ', prompt); + } else + item_make(" %*c%s %s", + indent + 1, ' ', prompt, + menu_is_empty(menu) ? "----" : "--->"); + item_set_tag('m'); + item_set_data(menu); + if (single_menu_mode && menu->data) + goto conf_childs; + return; + case P_COMMENT: + if (prompt) { + child_count++; + item_make(" %*c*** %s ***", indent + 1, ' ', _(prompt)); + item_set_tag(':'); + item_set_data(menu); + } + break; + default: + if (prompt) { + child_count++; + item_make("---%*c%s", indent + 1, ' ', _(prompt)); + item_set_tag(':'); + item_set_data(menu); + } + } + } else + doint = 0; + goto conf_childs; + } + + type = sym_get_type(sym); + if (sym_is_choice(sym)) { + struct symbol *def_sym = sym_get_choice_value(sym); + struct menu *def_menu = NULL; + + child_count++; + for (child = menu->list; child; child = child->next) { + if (menu_is_visible(child) && child->sym == def_sym) + def_menu = child; + } + + val = sym_get_tristate_value(sym); + if (sym_is_changable(sym)) { + switch (type) { + case S_BOOLEAN: + item_make("[%c]", val == no ? ' ' : '*'); + break; + case S_TRISTATE: + switch (val) { + case yes: ch = '*'; break; + case mod: ch = 'M'; break; + default: ch = ' '; break; + } + item_make("<%c>", ch); + break; + } + item_set_tag('t'); + item_set_data(menu); + } else { + item_make(" "); + item_set_tag(def_menu ? 't' : ':'); + item_set_data(menu); + } + + item_add_str("%*c%s", indent + 1, ' ', _(menu_get_prompt(menu))); + if (val == yes) { + if (def_menu) { + item_add_str(" (%s)", _(menu_get_prompt(def_menu))); + item_add_str(" --->"); + if (def_menu->list) { + indent += 2; + build_conf(def_menu); + indent -= 2; + } + } + return; + } + } else { + if (menu == current_menu) { + item_make("---%*c%s", indent + 1, ' ', _(menu_get_prompt(menu))); + item_set_tag(':'); + item_set_data(menu); + goto conf_childs; + } + child_count++; + val = sym_get_tristate_value(sym); + if (sym_is_choice_value(sym) && val == yes) { + item_make(" "); + item_set_tag(':'); + item_set_data(menu); + } else { + switch (type) { + case S_BOOLEAN: + if (sym_is_changable(sym)) + item_make("[%c]", val == no ? ' ' : '*'); + else + item_make("-%c-", val == no ? ' ' : '*'); + item_set_tag('t'); + item_set_data(menu); + break; + case S_TRISTATE: + switch (val) { + case yes: ch = '*'; break; + case mod: ch = 'M'; break; + default: ch = ' '; break; + } + if (sym_is_changable(sym)) { + if (sym->rev_dep.tri == mod) + item_make("{%c}", ch); + else + item_make("<%c>", ch); + } else + item_make("-%c-", ch); + item_set_tag('t'); + item_set_data(menu); + break; + default: + tmp = 2 + strlen(sym_get_string_value(sym)); /* () = 2 */ + item_make("(%s)", sym_get_string_value(sym)); + tmp = indent - tmp + 4; + if (tmp < 0) + tmp = 0; + item_add_str("%*c%s%s", tmp, ' ', _(menu_get_prompt(menu)), + (sym_has_value(sym) || !sym_is_changable(sym)) ? + "" : _(" (NEW)")); + item_set_tag('s'); + item_set_data(menu); + goto conf_childs; + } + } + item_add_str("%*c%s%s", indent + 1, ' ', _(menu_get_prompt(menu)), + (sym_has_value(sym) || !sym_is_changable(sym)) ? + "" : _(" (NEW)")); + if (menu->prompt->type == P_MENU) { + item_add_str(" %s", menu_is_empty(menu) ? "----" : "--->"); + return; + } + } + +conf_childs: + indent += doint; + for (child = menu->list; child; child = child->next) + build_conf(child); + indent -= doint; +} + +static void conf(struct menu *menu, struct menu *active_menu) +{ + struct menu *submenu; + const char *prompt = menu_get_prompt(menu); + struct subtitle_part stpart; + struct symbol *sym; + int res; + int s_scroll = 0; + + if (menu != &rootmenu) + stpart.text = menu_get_prompt(menu); + else + stpart.text = NULL; + list_add_tail(&stpart.entries, &trail); + + while (1) { + item_reset(); + current_menu = menu; + build_conf(menu); + if (!child_count) + break; + set_subtitle(); + dialog_clear(); + res = dialog_menu(prompt ? _(prompt) : _("Main Menu"), + _(menu_instructions), + active_menu, &s_scroll); + if (res == 1 || res == KEY_ESC || res == -ERRDISPLAYTOOSMALL) + break; + if (item_count() != 0) { + if (!item_activate_selected()) + continue; + if (!item_tag()) + continue; + } + submenu = item_data(); + active_menu = item_data(); + if (submenu) + sym = submenu->sym; + else + sym = NULL; + + switch (res) { + case 0: + switch (item_tag()) { + case 'm': + if (single_menu_mode) + submenu->data = (void *) (long) !submenu->data; + else + conf(submenu, NULL); + break; + case 't': + if (sym_is_choice(sym) && sym_get_tristate_value(sym) == yes) + conf_choice(submenu); + else if (submenu->prompt->type == P_MENU) + conf(submenu, NULL); + break; + case 's': + conf_string(submenu); + break; + } + break; + case 2: + if (sym) + show_help(submenu); + else { + reset_subtitle(); + show_helptext(_("README"), _(mconf_readme)); + } + break; + case 3: + reset_subtitle(); + conf_save(); + break; + case 4: + reset_subtitle(); + conf_load(); + break; + case 5: + if (item_is_tag('t')) { + if (sym_set_tristate_value(sym, yes)) + break; + if (sym_set_tristate_value(sym, mod)) + show_textbox(NULL, setmod_text, 6, 74); + } + break; + case 6: + if (item_is_tag('t')) + sym_set_tristate_value(sym, no); + break; + case 7: + if (item_is_tag('t')) + sym_set_tristate_value(sym, mod); + break; + case 8: + if (item_is_tag('t')) + sym_toggle_tristate_value(sym); + else if (item_is_tag('m')) + conf(submenu, NULL); + break; + case 9: + search_conf(); + break; + case 10: + show_all_options = !show_all_options; + break; + } + } + + list_del(trail.prev); +} + +static int show_textbox_ext(const char *title, char *text, int r, int c, int + *keys, int *vscroll, int *hscroll, update_text_fn + update_text, void *data) +{ + dialog_clear(); + return dialog_textbox(title, text, r, c, keys, vscroll, hscroll, + update_text, data); +} + +static void show_textbox(const char *title, const char *text, int r, int c) +{ + show_textbox_ext(title, (char *) text, r, c, (int []) {0}, NULL, NULL, + NULL, NULL); +} + +static void show_helptext(const char *title, const char *text) +{ + show_textbox(title, text, 0, 0); +} + +static void conf_message_callback(const char *fmt, va_list ap) +{ + char buf[PATH_MAX+1]; + + vsnprintf(buf, sizeof(buf), fmt, ap); + if (save_and_exit) { + if (!silent) + printf("%s", buf); + } else { + show_textbox(NULL, buf, 6, 60); + } +} + +static void show_help(struct menu *menu) +{ + struct gstr help = str_new(); + + help.max_width = getmaxx(stdscr) - 10; + menu_get_ext_help(menu, &help); + + show_helptext(_(menu_get_prompt(menu)), str_get(&help)); + str_free(&help); +} + +static void conf_choice(struct menu *menu) +{ + const char *prompt = _(menu_get_prompt(menu)); + struct menu *child; + struct symbol *active; + + active = sym_get_choice_value(menu->sym); + while (1) { + int res; + int selected; + item_reset(); + + current_menu = menu; + for (child = menu->list; child; child = child->next) { + if (!menu_is_visible(child)) + continue; + if (child->sym) + item_make("%s", _(menu_get_prompt(child))); + else { + item_make("*** %s ***", _(menu_get_prompt(child))); + item_set_tag(':'); + } + item_set_data(child); + if (child->sym == active) + item_set_selected(1); + if (child->sym == sym_get_choice_value(menu->sym)) + item_set_tag('X'); + } + dialog_clear(); + res = dialog_checklist(prompt ? _(prompt) : _("Main Menu"), + _(radiolist_instructions), + MENUBOX_HEIGTH_MIN, + MENUBOX_WIDTH_MIN, + CHECKLIST_HEIGTH_MIN); + selected = item_activate_selected(); + switch (res) { + case 0: + if (selected) { + child = item_data(); + if (!child->sym) + break; + + sym_set_tristate_value(child->sym, yes); + } + return; + case 1: + if (selected) { + child = item_data(); + show_help(child); + active = child->sym; + } else + show_help(menu); + break; + case KEY_ESC: + return; + case -ERRDISPLAYTOOSMALL: + return; + } + } +} + +static void conf_string(struct menu *menu) +{ + const char *prompt = menu_get_prompt(menu); + + while (1) { + int res; + const char *heading; + + switch (sym_get_type(menu->sym)) { + case S_INT: + heading = _(inputbox_instructions_int); + break; + case S_HEX: + heading = _(inputbox_instructions_hex); + break; + case S_STRING: + heading = _(inputbox_instructions_string); + break; + default: + heading = _("Internal mconf error!"); + } + dialog_clear(); + res = dialog_inputbox(prompt ? _(prompt) : _("Main Menu"), + heading, 10, 75, + sym_get_string_value(menu->sym)); + switch (res) { + case 0: + if (sym_set_string_value(menu->sym, dialog_input_result)) + return; + show_textbox(NULL, _("You have made an invalid entry."), 5, 43); + break; + case 1: + show_help(menu); + break; + case KEY_ESC: + return; + } + } +} + +static void conf_load(void) +{ + + while (1) { + int res; + dialog_clear(); + res = dialog_inputbox(NULL, load_config_text, + 11, 55, filename); + switch(res) { + case 0: + if (!dialog_input_result[0]) + return; + if (!conf_read(dialog_input_result)) { + set_config_filename(dialog_input_result); + sym_set_change_count(1); + return; + } + show_textbox(NULL, _("File does not exist!"), 5, 38); + break; + case 1: + show_helptext(_("Load Alternate Configuration"), load_config_help); + break; + case KEY_ESC: + return; + } + } +} + +static void conf_save(void) +{ + while (1) { + int res; + dialog_clear(); + res = dialog_inputbox(NULL, save_config_text, + 11, 55, filename); + switch(res) { + case 0: + if (!dialog_input_result[0]) + return; + if (!conf_write(dialog_input_result)) { + set_config_filename(dialog_input_result); + return; + } + show_textbox(NULL, _("Can't create file! Probably a nonexistent directory."), 5, 60); + break; + case 1: + show_helptext(_("Save Alternate Configuration"), save_config_help); + break; + case KEY_ESC: + return; + } + } +} + +static int handle_exit(void) +{ + int res; + + save_and_exit = 1; + reset_subtitle(); + dialog_clear(); + if (conf_get_changed()) + res = dialog_yesno(NULL, + _("Do you wish to save your new configuration?\n" + "(Press to continue kernel configuration.)"), + 6, 60); + else + res = -1; + + end_dialog(saved_x, saved_y); + + switch (res) { + case 0: + if (conf_write(filename)) { + fprintf(stderr, _("\n\n" + "Error while writing of the configuration.\n" + "Your configuration changes were NOT saved." + "\n\n")); + return 1; + } + /* fall through */ + case -1: + if (!silent) + printf(_("\n\n" + "*** End of the configuration.\n" + "*** Execute 'make' to start the build or try 'make help'." + "\n\n")); + res = 0; + break; + default: + if (!silent) + fprintf(stderr, _("\n\n" + "Your configuration changes were NOT saved." + "\n\n")); + if (res != KEY_ESC) + res = 0; + } + + return res; +} + +static void sig_handler(int signo) +{ + exit(handle_exit()); +} + +int main(int ac, char **av) +{ + char *mode; + int res; + + setlocale(LC_ALL, ""); + bindtextdomain(PACKAGE, LOCALEDIR); + textdomain(PACKAGE); + + signal(SIGINT, sig_handler); + + if (ac > 1 && strcmp(av[1], "-s") == 0) { + silent = 1; + /* Silence conf_read() until the real callback is set up */ + conf_set_message_callback(NULL); + av++; + } + conf_parse(av[1]); + conf_read(NULL); + + mode = getenv("MENUCONFIG_MODE"); + if (mode) { + if (!strcasecmp(mode, "single_menu")) + single_menu_mode = 1; + } + + if (init_dialog(NULL)) { + fprintf(stderr, N_("Your display is too small to run Menuconfig!\n")); + fprintf(stderr, N_("It must be at least 19 lines by 80 columns.\n")); + return 1; + } + + set_config_filename(conf_get_configname()); + conf_set_message_callback(conf_message_callback); + do { + conf(&rootmenu, NULL); + res = handle_exit(); + } while (res == KEY_ESC); + + return res; +} diff --git a/scripts/kconfig/menu.c b/scripts/kconfig/menu.c new file mode 100644 index 0000000000..aed678e8a7 --- /dev/null +++ b/scripts/kconfig/menu.c @@ -0,0 +1,697 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include + +#include "lkc.h" + +static const char nohelp_text[] = "There is no help available for this option."; + +struct menu rootmenu; +static struct menu **last_entry_ptr; + +struct file *file_list; +struct file *current_file; + +void menu_warn(struct menu *menu, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + fprintf(stderr, "%s:%d:warning: ", menu->file->name, menu->lineno); + vfprintf(stderr, fmt, ap); + fprintf(stderr, "\n"); + va_end(ap); +} + +static void prop_warn(struct property *prop, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + fprintf(stderr, "%s:%d:warning: ", prop->file->name, prop->lineno); + vfprintf(stderr, fmt, ap); + fprintf(stderr, "\n"); + va_end(ap); +} + +void _menu_init(void) +{ + current_entry = current_menu = &rootmenu; + last_entry_ptr = &rootmenu.list; +} + +void menu_add_entry(struct symbol *sym) +{ + struct menu *menu; + + menu = xmalloc(sizeof(*menu)); + memset(menu, 0, sizeof(*menu)); + menu->sym = sym; + menu->parent = current_menu; + menu->file = current_file; + menu->lineno = zconf_lineno(); + + *last_entry_ptr = menu; + last_entry_ptr = &menu->next; + current_entry = menu; + if (sym) + menu_add_symbol(P_SYMBOL, sym, NULL); +} + +void menu_end_entry(void) +{ +} + +struct menu *menu_add_menu(void) +{ + menu_end_entry(); + last_entry_ptr = ¤t_entry->list; + return current_menu = current_entry; +} + +void menu_end_menu(void) +{ + last_entry_ptr = ¤t_menu->next; + current_menu = current_menu->parent; +} + +static struct expr *menu_check_dep(struct expr *e) +{ + if (!e) + return e; + + switch (e->type) { + case E_NOT: + e->left.expr = menu_check_dep(e->left.expr); + break; + case E_OR: + case E_AND: + e->left.expr = menu_check_dep(e->left.expr); + e->right.expr = menu_check_dep(e->right.expr); + break; + case E_SYMBOL: + /* change 'm' into 'm' && MODULES */ + if (e->left.sym == &symbol_mod) + return expr_alloc_and(e, expr_alloc_symbol(modules_sym)); + break; + default: + break; + } + return e; +} + +void menu_add_dep(struct expr *dep) +{ + current_entry->dep = expr_alloc_and(current_entry->dep, menu_check_dep(dep)); +} + +void menu_set_type(int type) +{ + struct symbol *sym = current_entry->sym; + + if (sym->type == type) + return; + if (sym->type == S_UNKNOWN) { + sym->type = type; + return; + } + menu_warn(current_entry, + "ignoring type redefinition of '%s' from '%s' to '%s'", + sym->name ? sym->name : "", + sym_type_name(sym->type), sym_type_name(type)); +} + +static struct property *menu_add_prop(enum prop_type type, char *prompt, struct expr *expr, struct expr *dep) +{ + struct property *prop = prop_alloc(type, current_entry->sym); + + prop->menu = current_entry; + prop->expr = expr; + prop->visible.expr = menu_check_dep(dep); + + if (prompt) { + if (isspace(*prompt)) { + prop_warn(prop, "leading whitespace ignored"); + while (isspace(*prompt)) + prompt++; + } + if (current_entry->prompt && current_entry != &rootmenu) + prop_warn(prop, "prompt redefined"); + + /* Apply all upper menus' visibilities to actual prompts. */ + if(type == P_PROMPT) { + struct menu *menu = current_entry; + + while ((menu = menu->parent) != NULL) { + struct expr *dup_expr; + + if (!menu->visibility) + continue; + /* + * Do not add a reference to the + * menu's visibility expression but + * use a copy of it. Otherwise the + * expression reduction functions + * will modify expressions that have + * multiple references which can + * cause unwanted side effects. + */ + dup_expr = expr_copy(menu->visibility); + + prop->visible.expr + = expr_alloc_and(prop->visible.expr, + dup_expr); + } + } + + current_entry->prompt = prop; + } + prop->text = prompt; + + return prop; +} + +struct property *menu_add_prompt(enum prop_type type, char *prompt, struct expr *dep) +{ + return menu_add_prop(type, prompt, NULL, dep); +} + +void menu_add_visibility(struct expr *expr) +{ + current_entry->visibility = expr_alloc_and(current_entry->visibility, + expr); +} + +void menu_add_expr(enum prop_type type, struct expr *expr, struct expr *dep) +{ + menu_add_prop(type, NULL, expr, dep); +} + +void menu_add_symbol(enum prop_type type, struct symbol *sym, struct expr *dep) +{ + menu_add_prop(type, NULL, expr_alloc_symbol(sym), dep); +} + +void menu_add_option(int token, char *arg) +{ + switch (token) { + case T_OPT_MODULES: + if (modules_sym) + zconf_error("symbol '%s' redefines option 'modules'" + " already defined by symbol '%s'", + current_entry->sym->name, + modules_sym->name + ); + modules_sym = current_entry->sym; + break; + case T_OPT_DEFCONFIG_LIST: + if (!sym_defconfig_list) + sym_defconfig_list = current_entry->sym; + else if (sym_defconfig_list != current_entry->sym) + zconf_error("trying to redefine defconfig symbol"); + break; + case T_OPT_ENV: + prop_add_env(arg); + break; + case T_OPT_ALLNOCONFIG_Y: + current_entry->sym->flags |= SYMBOL_ALLNOCONFIG_Y; + break; + } +} + +static int menu_validate_number(struct symbol *sym, struct symbol *sym2) +{ + return sym2->type == S_INT || sym2->type == S_HEX || + (sym2->type == S_UNKNOWN && sym_string_valid(sym, sym2->name)); +} + +static void sym_check_prop(struct symbol *sym) +{ + struct property *prop; + struct symbol *sym2; + for (prop = sym->prop; prop; prop = prop->next) { + switch (prop->type) { + case P_DEFAULT: + if ((sym->type == S_STRING || sym->type == S_INT || sym->type == S_HEX) && + prop->expr->type != E_SYMBOL) + prop_warn(prop, + "default for config symbol '%s'" + " must be a single symbol", sym->name); + if (prop->expr->type != E_SYMBOL) + break; + sym2 = prop_get_symbol(prop); + if (sym->type == S_HEX || sym->type == S_INT) { + if (!menu_validate_number(sym, sym2)) + prop_warn(prop, + "'%s': number is invalid", + sym->name); + } + break; + case P_SELECT: + sym2 = prop_get_symbol(prop); + if (sym->type != S_BOOLEAN && sym->type != S_TRISTATE) + prop_warn(prop, + "config symbol '%s' uses select, but is " + "not boolean or tristate", sym->name); + else if (sym2->type != S_UNKNOWN && + sym2->type != S_BOOLEAN && + sym2->type != S_TRISTATE) + prop_warn(prop, + "'%s' has wrong type. 'select' only " + "accept arguments of boolean and " + "tristate type", sym2->name); + break; + case P_RANGE: + if (sym->type != S_INT && sym->type != S_HEX) + prop_warn(prop, "range is only allowed " + "for int or hex symbols"); + if (!menu_validate_number(sym, prop->expr->left.sym) || + !menu_validate_number(sym, prop->expr->right.sym)) + prop_warn(prop, "range is invalid"); + break; + default: + ; + } + } +} + +void menu_finalize(struct menu *parent) +{ + struct menu *menu, *last_menu; + struct symbol *sym; + struct property *prop; + struct expr *parentdep, *basedep, *dep, *dep2, **ep; + + sym = parent->sym; + if (parent->list) { + if (sym && sym_is_choice(sym)) { + if (sym->type == S_UNKNOWN) { + /* find the first choice value to find out choice type */ + current_entry = parent; + for (menu = parent->list; menu; menu = menu->next) { + if (menu->sym && menu->sym->type != S_UNKNOWN) { + menu_set_type(menu->sym->type); + break; + } + } + } + /* set the type of the remaining choice values */ + for (menu = parent->list; menu; menu = menu->next) { + current_entry = menu; + if (menu->sym && menu->sym->type == S_UNKNOWN) + menu_set_type(sym->type); + } + parentdep = expr_alloc_symbol(sym); + } else if (parent->prompt) + parentdep = parent->prompt->visible.expr; + else + parentdep = parent->dep; + + for (menu = parent->list; menu; menu = menu->next) { + basedep = expr_transform(menu->dep); + basedep = expr_alloc_and(expr_copy(parentdep), basedep); + basedep = expr_eliminate_dups(basedep); + menu->dep = basedep; + if (menu->sym) + prop = menu->sym->prop; + else + prop = menu->prompt; + for (; prop; prop = prop->next) { + if (prop->menu != menu) + continue; + dep = expr_transform(prop->visible.expr); + dep = expr_alloc_and(expr_copy(basedep), dep); + dep = expr_eliminate_dups(dep); + if (menu->sym && menu->sym->type != S_TRISTATE) + dep = expr_trans_bool(dep); + prop->visible.expr = dep; + if (prop->type == P_SELECT) { + struct symbol *es = prop_get_symbol(prop); + es->rev_dep.expr = expr_alloc_or(es->rev_dep.expr, + expr_alloc_and(expr_alloc_symbol(menu->sym), expr_copy(dep))); + } + } + } + for (menu = parent->list; menu; menu = menu->next) + menu_finalize(menu); + } else if (sym) { + basedep = parent->prompt ? parent->prompt->visible.expr : NULL; + basedep = expr_trans_compare(basedep, E_UNEQUAL, &symbol_no); + basedep = expr_eliminate_dups(expr_transform(basedep)); + last_menu = NULL; + for (menu = parent->next; menu; menu = menu->next) { + dep = menu->prompt ? menu->prompt->visible.expr : menu->dep; + if (!expr_contains_symbol(dep, sym)) + break; + if (expr_depends_symbol(dep, sym)) + goto next; + dep = expr_trans_compare(dep, E_UNEQUAL, &symbol_no); + dep = expr_eliminate_dups(expr_transform(dep)); + dep2 = expr_copy(basedep); + expr_eliminate_eq(&dep, &dep2); + expr_free(dep); + if (!expr_is_yes(dep2)) { + expr_free(dep2); + break; + } + expr_free(dep2); + next: + menu_finalize(menu); + menu->parent = parent; + last_menu = menu; + } + if (last_menu) { + parent->list = parent->next; + parent->next = last_menu->next; + last_menu->next = NULL; + } + + sym->dir_dep.expr = expr_alloc_or(sym->dir_dep.expr, parent->dep); + } + for (menu = parent->list; menu; menu = menu->next) { + if (sym && sym_is_choice(sym) && + menu->sym && !sym_is_choice_value(menu->sym)) { + current_entry = menu; + menu->sym->flags |= SYMBOL_CHOICEVAL; + if (!menu->prompt) + menu_warn(menu, "choice value must have a prompt"); + for (prop = menu->sym->prop; prop; prop = prop->next) { + if (prop->type == P_DEFAULT) + prop_warn(prop, "defaults for choice " + "values not supported"); + if (prop->menu == menu) + continue; + if (prop->type == P_PROMPT && + prop->menu->parent->sym != sym) + prop_warn(prop, "choice value used outside its choice group"); + } + /* Non-tristate choice values of tristate choices must + * depend on the choice being set to Y. The choice + * values' dependencies were propagated to their + * properties above, so the change here must be re- + * propagated. + */ + if (sym->type == S_TRISTATE && menu->sym->type != S_TRISTATE) { + basedep = expr_alloc_comp(E_EQUAL, sym, &symbol_yes); + menu->dep = expr_alloc_and(basedep, menu->dep); + for (prop = menu->sym->prop; prop; prop = prop->next) { + if (prop->menu != menu) + continue; + prop->visible.expr = expr_alloc_and(expr_copy(basedep), + prop->visible.expr); + } + } + menu_add_symbol(P_CHOICE, sym, NULL); + prop = sym_get_choice_prop(sym); + for (ep = &prop->expr; *ep; ep = &(*ep)->left.expr) + ; + *ep = expr_alloc_one(E_LIST, NULL); + (*ep)->right.sym = menu->sym; + } + if (menu->list && (!menu->prompt || !menu->prompt->text)) { + for (last_menu = menu->list; ; last_menu = last_menu->next) { + last_menu->parent = parent; + if (!last_menu->next) + break; + } + last_menu->next = menu->next; + menu->next = menu->list; + menu->list = NULL; + } + } + + if (sym && !(sym->flags & SYMBOL_WARNED)) { + if (sym->type == S_UNKNOWN) + menu_warn(parent, "config symbol defined without type"); + + if (sym_is_choice(sym) && !parent->prompt) + menu_warn(parent, "choice must have a prompt"); + + /* Check properties connected to this symbol */ + sym_check_prop(sym); + sym->flags |= SYMBOL_WARNED; + } + + if (sym && !sym_is_optional(sym) && parent->prompt) { + sym->rev_dep.expr = expr_alloc_or(sym->rev_dep.expr, + expr_alloc_and(parent->prompt->visible.expr, + expr_alloc_symbol(&symbol_mod))); + } +} + +bool menu_has_prompt(struct menu *menu) +{ + if (!menu->prompt) + return false; + return true; +} + +/* + * Determine if a menu is empty. + * A menu is considered empty if it contains no or only + * invisible entries. + */ +bool menu_is_empty(struct menu *menu) +{ + struct menu *child; + + for (child = menu->list; child; child = child->next) { + if (menu_is_visible(child)) + return(false); + } + return(true); +} + +bool menu_is_visible(struct menu *menu) +{ + struct menu *child; + struct symbol *sym; + tristate visible; + + if (!menu->prompt) + return false; + + if (menu->visibility) { + if (expr_calc_value(menu->visibility) == no) + return false; + } + + sym = menu->sym; + if (sym) { + sym_calc_value(sym); + visible = menu->prompt->visible.tri; + } else + visible = menu->prompt->visible.tri = expr_calc_value(menu->prompt->visible.expr); + + if (visible != no) + return true; + + if (!sym || sym_get_tristate_value(menu->sym) == no) + return false; + + for (child = menu->list; child; child = child->next) { + if (menu_is_visible(child)) { + if (sym) + sym->flags |= SYMBOL_DEF_USER; + return true; + } + } + + return false; +} + +const char *menu_get_prompt(struct menu *menu) +{ + if (menu->prompt) + return menu->prompt->text; + else if (menu->sym) + return menu->sym->name; + return NULL; +} + +struct menu *menu_get_root_menu(struct menu *menu) +{ + return &rootmenu; +} + +struct menu *menu_get_parent_menu(struct menu *menu) +{ + enum prop_type type; + + for (; menu != &rootmenu; menu = menu->parent) { + type = menu->prompt ? menu->prompt->type : 0; + if (type == P_MENU) + break; + } + return menu; +} + +bool menu_has_help(struct menu *menu) +{ + return menu->help != NULL; +} + +const char *menu_get_help(struct menu *menu) +{ + if (menu->help) + return menu->help; + else + return ""; +} + +static void get_prompt_str(struct gstr *r, struct property *prop, + struct list_head *head) +{ + int i, j; + struct menu *submenu[8], *menu, *location = NULL; + struct jump_key *jump = NULL; + + str_printf(r, _("Prompt: %s\n"), _(prop->text)); + menu = prop->menu->parent; + for (i = 0; menu != &rootmenu && i < 8; menu = menu->parent) { + bool accessible = menu_is_visible(menu); + + submenu[i++] = menu; + if (location == NULL && accessible) + location = menu; + } + if (head && location) { + jump = xmalloc(sizeof(struct jump_key)); + + if (menu_is_visible(prop->menu)) { + /* + * There is not enough room to put the hint at the + * beginning of the "Prompt" line. Put the hint on the + * last "Location" line even when it would belong on + * the former. + */ + jump->target = prop->menu; + } else + jump->target = location; + + if (list_empty(head)) + jump->index = 0; + else + jump->index = list_entry(head->prev, struct jump_key, + entries)->index + 1; + + list_add_tail(&jump->entries, head); + } + + if (i > 0) { + str_printf(r, _(" Location:\n")); + for (j = 4; --i >= 0; j += 2) { + menu = submenu[i]; + if (jump && menu == location) + jump->offset = strlen(r->s); + str_printf(r, "%*c-> %s", j, ' ', + _(menu_get_prompt(menu))); + if (menu->sym) { + str_printf(r, " (%s [=%s])", menu->sym->name ? + menu->sym->name : _(""), + sym_get_string_value(menu->sym)); + } + str_append(r, "\n"); + } + } +} + +/* + * get property of type P_SYMBOL + */ +static struct property *get_symbol_prop(struct symbol *sym) +{ + struct property *prop = NULL; + + for_all_properties(sym, prop, P_SYMBOL) + break; + return prop; +} + +/* + * head is optional and may be NULL + */ +static void get_symbol_str(struct gstr *r, struct symbol *sym, + struct list_head *head) +{ + bool hit; + struct property *prop; + + if (sym && sym->name) { + str_printf(r, "Symbol: %s [=%s]\n", sym->name, + sym_get_string_value(sym)); + str_printf(r, "Type : %s\n", sym_type_name(sym->type)); + if (sym->type == S_INT || sym->type == S_HEX) { + prop = sym_get_range_prop(sym); + if (prop) { + str_printf(r, "Range : "); + expr_gstr_print(prop->expr, r); + str_append(r, "\n"); + } + } + } + for_all_prompts(sym, prop) + get_prompt_str(r, prop, head); + + prop = get_symbol_prop(sym); + if (prop) { + str_printf(r, _(" Defined at %s:%d\n"), prop->menu->file->name, + prop->menu->lineno); + if (!expr_is_yes(prop->visible.expr)) { + str_append(r, _(" Depends on: ")); + expr_gstr_print(prop->visible.expr, r); + str_append(r, "\n"); + } + } + + hit = false; + for_all_properties(sym, prop, P_SELECT) { + if (!hit) { + str_append(r, " Selects: "); + hit = true; + } else + str_printf(r, " && "); + expr_gstr_print(prop->expr, r); + } + if (hit) + str_append(r, "\n"); + if (sym->rev_dep.expr) { + str_append(r, _(" Selected by: ")); + expr_gstr_print(sym->rev_dep.expr, r); + str_append(r, "\n"); + } + str_append(r, "\n\n"); +} + +struct gstr get_relations_str(struct symbol **sym_arr, struct list_head *head) +{ + struct symbol *sym; + struct gstr res = str_new(); + int i; + + for (i = 0; sym_arr && (sym = sym_arr[i]); i++) + get_symbol_str(&res, sym, head); + if (!i) + str_append(&res, _("No matches found.\n")); + return res; +} + + +void menu_get_ext_help(struct menu *menu, struct gstr *help) +{ + struct symbol *sym = menu->sym; + const char *help_text = nohelp_text; + + if (menu_has_help(menu)) { + if (sym->name) + str_printf(help, "%s%s:\n\n", CONFIG_, sym->name); + help_text = menu_get_help(menu); + } + str_printf(help, "%s\n", _(help_text)); + if (sym) + get_symbol_str(help, sym, NULL); +} diff --git a/scripts/kconfig/merge_config.sh b/scripts/kconfig/merge_config.sh new file mode 100755 index 0000000000..67d1314476 --- /dev/null +++ b/scripts/kconfig/merge_config.sh @@ -0,0 +1,170 @@ +#!/bin/sh +# merge_config.sh - Takes a list of config fragment values, and merges +# them one by one. Provides warnings on overridden values, and specified +# values that did not make it to the resulting .config file (due to missed +# dependencies or config symbol removal). +# +# Portions reused from kconf_check and generate_cfg: +# http://git.yoctoproject.org/cgit/cgit.cgi/yocto-kernel-tools/tree/tools/kconf_check +# http://git.yoctoproject.org/cgit/cgit.cgi/yocto-kernel-tools/tree/tools/generate_cfg +# +# Copyright (c) 2009-2010 Wind River Systems, Inc. +# Copyright 2011 Linaro +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License version 2 as +# published by the Free Software Foundation. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the GNU General Public License for more details. + +clean_up() { + rm -f $TMP_FILE + exit +} +trap clean_up HUP INT TERM + +usage() { + echo "Usage: $0 [OPTIONS] [CONFIG [...]]" + echo " -h display this help text" + echo " -m only merge the fragments, do not execute the make command" + echo " -n use allnoconfig instead of alldefconfig" + echo " -r list redundant entries when merging fragments" + echo " -O dir to put generated output files. Consider setting \$KCONFIG_CONFIG instead." +} + +RUNMAKE=true +ALLTARGET=alldefconfig +WARNREDUN=false +OUTPUT=. + +while true; do + case $1 in + "-n") + ALLTARGET=allnoconfig + shift + continue + ;; + "-m") + RUNMAKE=false + shift + continue + ;; + "-h") + usage + exit + ;; + "-r") + WARNREDUN=true + shift + continue + ;; + "-O") + if [ -d $2 ];then + OUTPUT=$(echo $2 | sed 's/\/*$//') + else + echo "output directory $2 does not exist" 1>&2 + exit 1 + fi + shift 2 + continue + ;; + *) + break + ;; + esac +done + +if [ "$#" -lt 1 ] ; then + usage + exit +fi + +if [ -z "$KCONFIG_CONFIG" ]; then + if [ "$OUTPUT" != . ]; then + KCONFIG_CONFIG=$(readlink -m -- "$OUTPUT/.config") + else + KCONFIG_CONFIG=.config + fi +fi + +INITFILE=$1 +shift; + +if [ ! -r "$INITFILE" ]; then + echo "The base file '$INITFILE' does not exist. Exit." >&2 + exit 1 +fi + +MERGE_LIST=$* +SED_CONFIG_EXP="s/^\(# \)\{0,1\}\(CONFIG_[a-zA-Z0-9_]*\)[= ].*/\2/p" +TMP_FILE=$(mktemp ./.tmp.config.XXXXXXXXXX) + +echo "Using $INITFILE as base" +cat $INITFILE > $TMP_FILE + +# Merge files, printing warnings on overridden values +for MERGE_FILE in $MERGE_LIST ; do + echo "Merging $MERGE_FILE" + if [ ! -r "$MERGE_FILE" ]; then + echo "The merge file '$MERGE_FILE' does not exist. Exit." >&2 + exit 1 + fi + CFG_LIST=$(sed -n "$SED_CONFIG_EXP" $MERGE_FILE) + + for CFG in $CFG_LIST ; do + grep -q -w $CFG $TMP_FILE || continue + PREV_VAL=$(grep -w $CFG $TMP_FILE) + NEW_VAL=$(grep -w $CFG $MERGE_FILE) + if [ "x$PREV_VAL" != "x$NEW_VAL" ] ; then + echo Value of $CFG is redefined by fragment $MERGE_FILE: + echo Previous value: $PREV_VAL + echo New value: $NEW_VAL + echo + elif [ "$WARNREDUN" = "true" ]; then + echo Value of $CFG is redundant by fragment $MERGE_FILE: + fi + sed -i "/$CFG[ =]/d" $TMP_FILE + done + cat $MERGE_FILE >> $TMP_FILE +done + +if [ "$RUNMAKE" = "false" ]; then + cp -T -- "$TMP_FILE" "$KCONFIG_CONFIG" + echo "#" + echo "# merged configuration written to $KCONFIG_CONFIG (needs make)" + echo "#" + clean_up + exit +fi + +# If we have an output dir, setup the O= argument, otherwise leave +# it blank, since O=. will create an unnecessary ./source softlink +OUTPUT_ARG="" +if [ "$OUTPUT" != "." ] ; then + OUTPUT_ARG="O=$OUTPUT" +fi + + +# Use the merged file as the starting point for: +# alldefconfig: Fills in any missing symbols with Kconfig default +# allnoconfig: Fills in any missing symbols with # CONFIG_* is not set +make KCONFIG_ALLCONFIG=$TMP_FILE $OUTPUT_ARG $ALLTARGET + + +# Check all specified config values took (might have missed-dependency issues) +for CFG in $(sed -n "$SED_CONFIG_EXP" $TMP_FILE); do + + REQUESTED_VAL=$(grep -w -e "$CFG" $TMP_FILE) + ACTUAL_VAL=$(grep -w -e "$CFG" "$KCONFIG_CONFIG") + if [ "x$REQUESTED_VAL" != "x$ACTUAL_VAL" ] ; then + echo "Value requested for $CFG not in final .config" + echo "Requested value: $REQUESTED_VAL" + echo "Actual value: $ACTUAL_VAL" + echo "" + fi +done + +clean_up diff --git a/scripts/kconfig/nconf.c b/scripts/kconfig/nconf.c new file mode 100644 index 0000000000..d42d534a66 --- /dev/null +++ b/scripts/kconfig/nconf.c @@ -0,0 +1,1561 @@ +/* + * Copyright (C) 2008 Nir Tzachar +#include + +#include "lkc.h" +#include "nconf.h" +#include + +static const char nconf_global_help[] = N_( +"Help windows\n" +"------------\n" +"o Global help: Unless in a data entry window, pressing will give \n" +" you the global help window, which you are just reading.\n" +"\n" +"o A short version of the global help is available by pressing .\n" +"\n" +"o Local help: To get help related to the current menu entry, use any\n" +" of , or if in a data entry window then press .\n" +"\n" +"\n" +"Menu entries\n" +"------------\n" +"This interface lets you select features and parameters for the kernel\n" +"build. Kernel features can either be built-in, modularized, or removed.\n" +"Parameters must be entered as text or decimal or hexadecimal numbers.\n" +"\n" +"Menu entries beginning with following braces represent features that\n" +" [ ] can be built in or removed\n" +" < > can be built in, modularized or removed\n" +" { } can be built in or modularized, are selected by another feature\n" +" - - are selected by another feature\n" +" XXX cannot be selected. Symbol Info tells you why.\n" +"*, M or whitespace inside braces means to build in, build as a module\n" +"or to exclude the feature respectively.\n" +"\n" +"To change any of these features, highlight it with the movement keys\n" +"listed below and press to build it in, to make it a module or\n" +" to remove it. You may press the key to cycle through the\n" +"available options.\n" +"\n" +"A trailing \"--->\" designates a submenu, a trailing \"----\" an\n" +"empty submenu.\n" +"\n" +"Menu navigation keys\n" +"----------------------------------------------------------------------\n" +"Linewise up \n" +"Linewise down \n" +"Pagewise up \n" +"Pagewise down \n" +"First entry \n" +"Last entry \n" +"Enter a submenu \n" +"Go back to parent menu \n" +"Close a help window \n" +"Close entry window, apply \n" +"Close entry window, forget \n" +"Start incremental, case-insensitive search for STRING in menu entries,\n" +" no regex support, STRING is displayed in upper left corner\n" +" STRING\n" +" Remove last character \n" +" Jump to next hit \n" +" Jump to previous hit \n" +"Exit menu search mode \n" +"Search for configuration variables with or without leading CONFIG_\n" +" RegExpr\n" +"Verbose search help \n" +"----------------------------------------------------------------------\n" +"\n" +"Unless in a data entry window, key <1> may be used instead of ,\n" +"<2> instead of , etc.\n" +"\n" +"\n" +"Radiolist (Choice list)\n" +"-----------------------\n" +"Use the movement keys listed above to select the option you wish to set\n" +"and press .\n" +"\n" +"\n" +"Data entry\n" +"----------\n" +"Enter the requested information and press . Hexadecimal values\n" +"may be entered without the \"0x\" prefix.\n" +"\n" +"\n" +"Text Box (Help Window)\n" +"----------------------\n" +"Use movement keys as listed in table above.\n" +"\n" +"Press any of to exit.\n" +"\n" +"\n" +"Alternate configuration files\n" +"-----------------------------\n" +"nconfig supports switching between different configurations.\n" +"Press to save your current configuration. Press and enter\n" +"a file name to load a previously saved configuration.\n" +"\n" +"\n" +"Terminal configuration\n" +"----------------------\n" +"If you use nconfig in a xterm window, make sure your TERM environment\n" +"variable specifies a terminal configuration which supports at least\n" +"16 colors. Otherwise nconfig will look rather bad.\n" +"\n" +"If the \"stty size\" command reports the current terminalsize correctly,\n" +"nconfig will adapt to sizes larger than the traditional 80x25 \"standard\"\n" +"and display longer menus properly.\n" +"\n" +"\n" +"Single menu mode\n" +"----------------\n" +"If you prefer to have all of the menu entries listed in a single menu,\n" +"rather than the default multimenu hierarchy, run nconfig with\n" +"NCONFIG_MODE environment variable set to single_menu. Example:\n" +"\n" +"make NCONFIG_MODE=single_menu nconfig\n" +"\n" +" will then unfold the appropriate category, or fold it if it\n" +"is already unfolded. Folded menu entries will be designated by a\n" +"leading \"++>\" and unfolded entries by a leading \"-->\".\n" +"\n" +"Note that this mode can eventually be a little more CPU expensive than\n" +"the default mode, especially with a larger number of unfolded submenus.\n" +"\n"), +menu_no_f_instructions[] = N_( +"Legend: [*] built-in [ ] excluded module < > module capable.\n" +"Submenus are designated by a trailing \"--->\", empty ones by \"----\".\n" +"\n" +"Use the following keys to navigate the menus:\n" +"Move up or down with and .\n" +"Enter a submenu with or .\n" +"Exit a submenu to its parent menu with or .\n" +"Pressing includes, excludes, modularizes features.\n" +"Pressing cycles through the available options.\n" +"To search for menu entries press .\n" +" always leaves the current window.\n" +"\n" +"You do not have function keys support.\n" +"Press <1> instead of , <2> instead of , etc.\n" +"For verbose global help use key <1>.\n" +"For help related to the current menu entry press or .\n"), +menu_instructions[] = N_( +"Legend: [*] built-in [ ] excluded module < > module capable.\n" +"Submenus are designated by a trailing \"--->\", empty ones by \"----\".\n" +"\n" +"Use the following keys to navigate the menus:\n" +"Move up or down with or .\n" +"Enter a submenu with or .\n" +"Exit a submenu to its parent menu with or .\n" +"Pressing includes, excludes, modularizes features.\n" +"Pressing cycles through the available options.\n" +"To search for menu entries press .\n" +" always leaves the current window.\n" +"\n" +"Pressing <1> may be used instead of , <2> instead of , etc.\n" +"For verbose global help press .\n" +"For help related to the current menu entry press or .\n"), +radiolist_instructions[] = N_( +"Press , , or to navigate a radiolist, select\n" +"with .\n" +"For help related to the current entry press or .\n" +"For global help press .\n"), +inputbox_instructions_int[] = N_( +"Please enter a decimal value.\n" +"Fractions will not be accepted.\n" +"Press to apply, to cancel."), +inputbox_instructions_hex[] = N_( +"Please enter a hexadecimal value.\n" +"Press to apply, to cancel."), +inputbox_instructions_string[] = N_( +"Please enter a string value.\n" +"Press to apply, to cancel."), +setmod_text[] = N_( +"This feature depends on another feature which has been configured as a\n" +"module. As a result, the current feature will be built as a module too."), +load_config_text[] = N_( +"Enter the name of the configuration file you wish to load.\n" +"Accept the name shown to restore the configuration you last\n" +"retrieved. Leave empty to abort."), +load_config_help[] = N_( +"For various reasons, one may wish to keep several different\n" +"configurations available on a single machine.\n" +"\n" +"If you have saved a previous configuration in a file other than the\n" +"default one, entering its name here will allow you to load and modify\n" +"that configuration.\n" +"\n" +"Leave empty to abort.\n"), +save_config_text[] = N_( +"Enter a filename to which this configuration should be saved\n" +"as an alternate. Leave empty to abort."), +save_config_help[] = N_( +"For various reasons, one may wish to keep several different\n" +"configurations available on a single machine.\n" +"\n" +"Entering a file name here will allow you to later retrieve, modify\n" +"and use the current configuration as an alternate to whatever\n" +"configuration options you have selected at that time.\n" +"\n" +"Leave empty to abort.\n"), +search_help[] = N_( +"Search for symbols (configuration variable names CONFIG_*) and display\n" +"their relations. Regular expressions are supported.\n" +"Example: Search for \"^FOO\".\n" +"Result:\n" +"-----------------------------------------------------------------\n" +"Symbol: FOO [ = m]\n" +"Prompt: Foo bus is used to drive the bar HW\n" +"Defined at drivers/pci/Kconfig:47\n" +"Depends on: X86_LOCAL_APIC && X86_IO_APIC || IA64\n" +"Location:\n" +" -> Bus options (PCI, PCMCIA, EISA, ISA)\n" +" -> PCI support (PCI [ = y])\n" +" -> PCI access mode ( [ = y])\n" +"Selects: LIBCRC32\n" +"Selected by: BAR\n" +"-----------------------------------------------------------------\n" +"o The line 'Prompt:' shows the text displayed for this symbol in\n" +" the menu hierarchy.\n" +"o The 'Defined at' line tells at what file / line number the symbol is\n" +" defined.\n" +"o The 'Depends on:' line lists symbols that need to be defined for\n" +" this symbol to be visible and selectable in the menu.\n" +"o The 'Location:' lines tell, where in the menu structure this symbol\n" +" is located. A location followed by a [ = y] indicates that this is\n" +" a selectable menu item, and the current value is displayed inside\n" +" brackets.\n" +"o The 'Selects:' line tells, what symbol will be automatically selected\n" +" if this symbol is selected (y or m).\n" +"o The 'Selected by' line tells what symbol has selected this symbol.\n" +"\n" +"Only relevant lines are shown.\n" +"\n\n" +"Search examples:\n" +"USB => find all symbols containing USB\n" +"^USB => find all symbols starting with USB\n" +"USB$ => find all symbols ending with USB\n" +"\n"); + +struct mitem { + char str[256]; + char tag; + void *usrptr; + int is_visible; +}; + +#define MAX_MENU_ITEMS 4096 +static int show_all_items; +static int indent; +static struct menu *current_menu; +static int child_count; +static int single_menu_mode; +/* the window in which all information appears */ +static WINDOW *main_window; +/* the largest size of the menu window */ +static int mwin_max_lines; +static int mwin_max_cols; +/* the window in which we show option buttons */ +static MENU *curses_menu; +static ITEM *curses_menu_items[MAX_MENU_ITEMS]; +static struct mitem k_menu_items[MAX_MENU_ITEMS]; +static int items_num; +static int global_exit; +/* the currently selected button */ +const char *current_instructions = menu_instructions; + +static char *dialog_input_result; +static int dialog_input_result_len; + +static void conf(struct menu *menu); +static void conf_choice(struct menu *menu); +static void conf_string(struct menu *menu); +static void conf_load(void); +static void conf_save(void); +static void show_help(struct menu *menu); +static int do_exit(void); +static void setup_windows(void); +static void search_conf(void); + +typedef void (*function_key_handler_t)(int *key, struct menu *menu); +static void handle_f1(int *key, struct menu *current_item); +static void handle_f2(int *key, struct menu *current_item); +static void handle_f3(int *key, struct menu *current_item); +static void handle_f4(int *key, struct menu *current_item); +static void handle_f5(int *key, struct menu *current_item); +static void handle_f6(int *key, struct menu *current_item); +static void handle_f7(int *key, struct menu *current_item); +static void handle_f8(int *key, struct menu *current_item); +static void handle_f9(int *key, struct menu *current_item); + +struct function_keys { + const char *key_str; + const char *func; + function_key key; + function_key_handler_t handler; +}; + +static const int function_keys_num = 9; +struct function_keys function_keys[] = { + { + .key_str = "F1", + .func = "Help", + .key = F_HELP, + .handler = handle_f1, + }, + { + .key_str = "F2", + .func = "SymInfo", + .key = F_SYMBOL, + .handler = handle_f2, + }, + { + .key_str = "F3", + .func = "Help 2", + .key = F_INSTS, + .handler = handle_f3, + }, + { + .key_str = "F4", + .func = "ShowAll", + .key = F_CONF, + .handler = handle_f4, + }, + { + .key_str = "F5", + .func = "Back", + .key = F_BACK, + .handler = handle_f5, + }, + { + .key_str = "F6", + .func = "Save", + .key = F_SAVE, + .handler = handle_f6, + }, + { + .key_str = "F7", + .func = "Load", + .key = F_LOAD, + .handler = handle_f7, + }, + { + .key_str = "F8", + .func = "SymSearch", + .key = F_SEARCH, + .handler = handle_f8, + }, + { + .key_str = "F9", + .func = "Exit", + .key = F_EXIT, + .handler = handle_f9, + }, +}; + +static void print_function_line(void) +{ + int i; + int offset = 1; + const int skip = 1; + int lines = getmaxy(stdscr); + + for (i = 0; i < function_keys_num; i++) { + (void) wattrset(main_window, attributes[FUNCTION_HIGHLIGHT]); + mvwprintw(main_window, lines-3, offset, + "%s", + function_keys[i].key_str); + (void) wattrset(main_window, attributes[FUNCTION_TEXT]); + offset += strlen(function_keys[i].key_str); + mvwprintw(main_window, lines-3, + offset, "%s", + function_keys[i].func); + offset += strlen(function_keys[i].func) + skip; + } + (void) wattrset(main_window, attributes[NORMAL]); +} + +/* help */ +static void handle_f1(int *key, struct menu *current_item) +{ + show_scroll_win(main_window, + _("Global help"), _(nconf_global_help)); + return; +} + +/* symbole help */ +static void handle_f2(int *key, struct menu *current_item) +{ + show_help(current_item); + return; +} + +/* instructions */ +static void handle_f3(int *key, struct menu *current_item) +{ + show_scroll_win(main_window, + _("Short help"), + _(current_instructions)); + return; +} + +/* config */ +static void handle_f4(int *key, struct menu *current_item) +{ + int res = btn_dialog(main_window, + _("Show all symbols?"), + 2, + " ", + ""); + if (res == 0) + show_all_items = 1; + else if (res == 1) + show_all_items = 0; + + return; +} + +/* back */ +static void handle_f5(int *key, struct menu *current_item) +{ + *key = KEY_LEFT; + return; +} + +/* save */ +static void handle_f6(int *key, struct menu *current_item) +{ + conf_save(); + return; +} + +/* load */ +static void handle_f7(int *key, struct menu *current_item) +{ + conf_load(); + return; +} + +/* search */ +static void handle_f8(int *key, struct menu *current_item) +{ + search_conf(); + return; +} + +/* exit */ +static void handle_f9(int *key, struct menu *current_item) +{ + do_exit(); + return; +} + +/* return != 0 to indicate the key was handles */ +static int process_special_keys(int *key, struct menu *menu) +{ + int i; + + if (*key == KEY_RESIZE) { + setup_windows(); + return 1; + } + + for (i = 0; i < function_keys_num; i++) { + if (*key == KEY_F(function_keys[i].key) || + *key == '0' + function_keys[i].key){ + function_keys[i].handler(key, menu); + return 1; + } + } + + return 0; +} + +static void clean_items(void) +{ + int i; + for (i = 0; curses_menu_items[i]; i++) + free_item(curses_menu_items[i]); + bzero(curses_menu_items, sizeof(curses_menu_items)); + bzero(k_menu_items, sizeof(k_menu_items)); + items_num = 0; +} + +typedef enum {MATCH_TINKER_PATTERN_UP, MATCH_TINKER_PATTERN_DOWN, + FIND_NEXT_MATCH_DOWN, FIND_NEXT_MATCH_UP} match_f; + +/* return the index of the matched item, or -1 if no such item exists */ +static int get_mext_match(const char *match_str, match_f flag) +{ + int match_start = item_index(current_item(curses_menu)); + int index; + + if (flag == FIND_NEXT_MATCH_DOWN) + ++match_start; + else if (flag == FIND_NEXT_MATCH_UP) + --match_start; + + index = match_start; + index = (index + items_num) % items_num; + while (true) { + char *str = k_menu_items[index].str; + if (strcasestr(str, match_str) != 0) + return index; + if (flag == FIND_NEXT_MATCH_UP || + flag == MATCH_TINKER_PATTERN_UP) + --index; + else + ++index; + index = (index + items_num) % items_num; + if (index == match_start) + return -1; + } +} + +/* Make a new item. */ +static void item_make(struct menu *menu, char tag, const char *fmt, ...) +{ + va_list ap; + + if (items_num > MAX_MENU_ITEMS-1) + return; + + bzero(&k_menu_items[items_num], sizeof(k_menu_items[0])); + k_menu_items[items_num].tag = tag; + k_menu_items[items_num].usrptr = menu; + if (menu != NULL) + k_menu_items[items_num].is_visible = + menu_is_visible(menu); + else + k_menu_items[items_num].is_visible = 1; + + va_start(ap, fmt); + vsnprintf(k_menu_items[items_num].str, + sizeof(k_menu_items[items_num].str), + fmt, ap); + va_end(ap); + + if (!k_menu_items[items_num].is_visible) + memcpy(k_menu_items[items_num].str, "XXX", 3); + + curses_menu_items[items_num] = new_item( + k_menu_items[items_num].str, + k_menu_items[items_num].str); + set_item_userptr(curses_menu_items[items_num], + &k_menu_items[items_num]); + /* + if (!k_menu_items[items_num].is_visible) + item_opts_off(curses_menu_items[items_num], O_SELECTABLE); + */ + + items_num++; + curses_menu_items[items_num] = NULL; +} + +/* very hackish. adds a string to the last item added */ +static void item_add_str(const char *fmt, ...) +{ + va_list ap; + int index = items_num-1; + char new_str[256]; + char tmp_str[256]; + + if (index < 0) + return; + + va_start(ap, fmt); + vsnprintf(new_str, sizeof(new_str), fmt, ap); + va_end(ap); + snprintf(tmp_str, sizeof(tmp_str), "%s%s", + k_menu_items[index].str, new_str); + strncpy(k_menu_items[index].str, + tmp_str, + sizeof(k_menu_items[index].str)); + + free_item(curses_menu_items[index]); + curses_menu_items[index] = new_item( + k_menu_items[index].str, + k_menu_items[index].str); + set_item_userptr(curses_menu_items[index], + &k_menu_items[index]); +} + +/* get the tag of the currently selected item */ +static char item_tag(void) +{ + ITEM *cur; + struct mitem *mcur; + + cur = current_item(curses_menu); + if (cur == NULL) + return 0; + mcur = (struct mitem *) item_userptr(cur); + return mcur->tag; +} + +static int curses_item_index(void) +{ + return item_index(current_item(curses_menu)); +} + +static void *item_data(void) +{ + ITEM *cur; + struct mitem *mcur; + + cur = current_item(curses_menu); + if (!cur) + return NULL; + mcur = (struct mitem *) item_userptr(cur); + return mcur->usrptr; + +} + +static int item_is_tag(char tag) +{ + return item_tag() == tag; +} + +static char filename[PATH_MAX+1]; +static char menu_backtitle[PATH_MAX+128]; +static const char *set_config_filename(const char *config_filename) +{ + int size; + + size = snprintf(menu_backtitle, sizeof(menu_backtitle), + "%s - %s", config_filename, rootmenu.prompt->text); + if (size >= sizeof(menu_backtitle)) + menu_backtitle[sizeof(menu_backtitle)-1] = '\0'; + + size = snprintf(filename, sizeof(filename), "%s", config_filename); + if (size >= sizeof(filename)) + filename[sizeof(filename)-1] = '\0'; + return menu_backtitle; +} + +/* return = 0 means we are successful. + * -1 means go on doing what you were doing + */ +static int do_exit(void) +{ + int res; + if (!conf_get_changed()) { + global_exit = 1; + return 0; + } + res = btn_dialog(main_window, + _("Do you wish to save your new configuration?\n" + " to cancel and resume nconfig."), + 2, + " ", + ""); + if (res == KEY_EXIT) { + global_exit = 0; + return -1; + } + + /* if we got here, the user really wants to exit */ + switch (res) { + case 0: + res = conf_write(filename); + if (res) + btn_dialog( + main_window, + _("Error during writing of configuration.\n" + "Your configuration changes were NOT saved."), + 1, + ""); + break; + default: + btn_dialog( + main_window, + _("Your configuration changes were NOT saved."), + 1, + ""); + break; + } + global_exit = 1; + return 0; +} + + +static void search_conf(void) +{ + struct symbol **sym_arr; + struct gstr res; + struct gstr title; + char *dialog_input; + int dres; + + title = str_new(); + str_printf( &title, _("Enter (sub)string or regexp to search for " + "(with or without \"%s\")"), CONFIG_); + +again: + dres = dialog_inputbox(main_window, + _("Search Configuration Parameter"), + str_get(&title), + "", &dialog_input_result, &dialog_input_result_len); + switch (dres) { + case 0: + break; + case 1: + show_scroll_win(main_window, + _("Search Configuration"), search_help); + goto again; + default: + str_free(&title); + return; + } + + /* strip the prefix if necessary */ + dialog_input = dialog_input_result; + if (strncasecmp(dialog_input_result, CONFIG_, strlen(CONFIG_)) == 0) + dialog_input += strlen(CONFIG_); + + sym_arr = sym_re_search(dialog_input); + res = get_relations_str(sym_arr, NULL); + free(sym_arr); + show_scroll_win(main_window, + _("Search Results"), str_get(&res)); + str_free(&res); + str_free(&title); +} + + +static void build_conf(struct menu *menu) +{ + struct symbol *sym; + struct property *prop; + struct menu *child; + int type, tmp, doint = 2; + tristate val; + char ch; + + if (!menu || (!show_all_items && !menu_is_visible(menu))) + return; + + sym = menu->sym; + prop = menu->prompt; + if (!sym) { + if (prop && menu != current_menu) { + const char *prompt = menu_get_prompt(menu); + enum prop_type ptype; + ptype = menu->prompt ? menu->prompt->type : P_UNKNOWN; + switch (ptype) { + case P_MENU: + child_count++; + prompt = _(prompt); + if (single_menu_mode) { + item_make(menu, 'm', + "%s%*c%s", + menu->data ? "-->" : "++>", + indent + 1, ' ', prompt); + } else + item_make(menu, 'm', + " %*c%s %s", + indent + 1, ' ', prompt, + menu_is_empty(menu) ? "----" : "--->"); + + if (single_menu_mode && menu->data) + goto conf_childs; + return; + case P_COMMENT: + if (prompt) { + child_count++; + item_make(menu, ':', + " %*c*** %s ***", + indent + 1, ' ', + _(prompt)); + } + break; + default: + if (prompt) { + child_count++; + item_make(menu, ':', "---%*c%s", + indent + 1, ' ', + _(prompt)); + } + } + } else + doint = 0; + goto conf_childs; + } + + type = sym_get_type(sym); + if (sym_is_choice(sym)) { + struct symbol *def_sym = sym_get_choice_value(sym); + struct menu *def_menu = NULL; + + child_count++; + for (child = menu->list; child; child = child->next) { + if (menu_is_visible(child) && child->sym == def_sym) + def_menu = child; + } + + val = sym_get_tristate_value(sym); + if (sym_is_changable(sym)) { + switch (type) { + case S_BOOLEAN: + item_make(menu, 't', "[%c]", + val == no ? ' ' : '*'); + break; + case S_TRISTATE: + switch (val) { + case yes: + ch = '*'; + break; + case mod: + ch = 'M'; + break; + default: + ch = ' '; + break; + } + item_make(menu, 't', "<%c>", ch); + break; + } + } else { + item_make(menu, def_menu ? 't' : ':', " "); + } + + item_add_str("%*c%s", indent + 1, + ' ', _(menu_get_prompt(menu))); + if (val == yes) { + if (def_menu) { + item_add_str(" (%s)", + _(menu_get_prompt(def_menu))); + item_add_str(" --->"); + if (def_menu->list) { + indent += 2; + build_conf(def_menu); + indent -= 2; + } + } + return; + } + } else { + if (menu == current_menu) { + item_make(menu, ':', + "---%*c%s", indent + 1, + ' ', _(menu_get_prompt(menu))); + goto conf_childs; + } + child_count++; + val = sym_get_tristate_value(sym); + if (sym_is_choice_value(sym) && val == yes) { + item_make(menu, ':', " "); + } else { + switch (type) { + case S_BOOLEAN: + if (sym_is_changable(sym)) + item_make(menu, 't', "[%c]", + val == no ? ' ' : '*'); + else + item_make(menu, 't', "-%c-", + val == no ? ' ' : '*'); + break; + case S_TRISTATE: + switch (val) { + case yes: + ch = '*'; + break; + case mod: + ch = 'M'; + break; + default: + ch = ' '; + break; + } + if (sym_is_changable(sym)) { + if (sym->rev_dep.tri == mod) + item_make(menu, + 't', "{%c}", ch); + else + item_make(menu, + 't', "<%c>", ch); + } else + item_make(menu, 't', "-%c-", ch); + break; + default: + tmp = 2 + strlen(sym_get_string_value(sym)); + item_make(menu, 's', " (%s)", + sym_get_string_value(sym)); + tmp = indent - tmp + 4; + if (tmp < 0) + tmp = 0; + item_add_str("%*c%s%s", tmp, ' ', + _(menu_get_prompt(menu)), + (sym_has_value(sym) || + !sym_is_changable(sym)) ? "" : + _(" (NEW)")); + goto conf_childs; + } + } + item_add_str("%*c%s%s", indent + 1, ' ', + _(menu_get_prompt(menu)), + (sym_has_value(sym) || !sym_is_changable(sym)) ? + "" : _(" (NEW)")); + if (menu->prompt && menu->prompt->type == P_MENU) { + item_add_str(" %s", menu_is_empty(menu) ? "----" : "--->"); + return; + } + } + +conf_childs: + indent += doint; + for (child = menu->list; child; child = child->next) + build_conf(child); + indent -= doint; +} + +static void reset_menu(void) +{ + unpost_menu(curses_menu); + clean_items(); +} + +/* adjust the menu to show this item. + * prefer not to scroll the menu if possible*/ +static void center_item(int selected_index, int *last_top_row) +{ + int toprow; + + set_top_row(curses_menu, *last_top_row); + toprow = top_row(curses_menu); + if (selected_index < toprow || + selected_index >= toprow+mwin_max_lines) { + toprow = max(selected_index-mwin_max_lines/2, 0); + if (toprow >= item_count(curses_menu)-mwin_max_lines) + toprow = item_count(curses_menu)-mwin_max_lines; + set_top_row(curses_menu, toprow); + } + set_current_item(curses_menu, + curses_menu_items[selected_index]); + *last_top_row = toprow; + post_menu(curses_menu); + refresh_all_windows(main_window); +} + +/* this function assumes reset_menu has been called before */ +static void show_menu(const char *prompt, const char *instructions, + int selected_index, int *last_top_row) +{ + int maxx, maxy; + WINDOW *menu_window; + + current_instructions = instructions; + + clear(); + (void) wattrset(main_window, attributes[NORMAL]); + print_in_middle(stdscr, 1, 0, getmaxx(stdscr), + menu_backtitle, + attributes[MAIN_HEADING]); + + (void) wattrset(main_window, attributes[MAIN_MENU_BOX]); + box(main_window, 0, 0); + (void) wattrset(main_window, attributes[MAIN_MENU_HEADING]); + mvwprintw(main_window, 0, 3, " %s ", prompt); + (void) wattrset(main_window, attributes[NORMAL]); + + set_menu_items(curses_menu, curses_menu_items); + + /* position the menu at the middle of the screen */ + scale_menu(curses_menu, &maxy, &maxx); + maxx = min(maxx, mwin_max_cols-2); + maxy = mwin_max_lines; + menu_window = derwin(main_window, + maxy, + maxx, + 2, + (mwin_max_cols-maxx)/2); + keypad(menu_window, TRUE); + set_menu_win(curses_menu, menu_window); + set_menu_sub(curses_menu, menu_window); + + /* must reassert this after changing items, otherwise returns to a + * default of 16 + */ + set_menu_format(curses_menu, maxy, 1); + center_item(selected_index, last_top_row); + set_menu_format(curses_menu, maxy, 1); + + print_function_line(); + + /* Post the menu */ + post_menu(curses_menu); + refresh_all_windows(main_window); +} + +static void adj_match_dir(match_f *match_direction) +{ + if (*match_direction == FIND_NEXT_MATCH_DOWN) + *match_direction = + MATCH_TINKER_PATTERN_DOWN; + else if (*match_direction == FIND_NEXT_MATCH_UP) + *match_direction = + MATCH_TINKER_PATTERN_UP; + /* else, do no change.. */ +} + +struct match_state +{ + int in_search; + match_f match_direction; + char pattern[256]; +}; + +/* Return 0 means I have handled the key. In such a case, ans should hold the + * item to center, or -1 otherwise. + * Else return -1 . + */ +static int do_match(int key, struct match_state *state, int *ans) +{ + char c = (char) key; + int terminate_search = 0; + *ans = -1; + if (key == '/' || (state->in_search && key == 27)) { + move(0, 0); + refresh(); + clrtoeol(); + state->in_search = 1-state->in_search; + bzero(state->pattern, sizeof(state->pattern)); + state->match_direction = MATCH_TINKER_PATTERN_DOWN; + return 0; + } else if (!state->in_search) + return 1; + + if (isalnum(c) || isgraph(c) || c == ' ') { + state->pattern[strlen(state->pattern)] = c; + state->pattern[strlen(state->pattern)] = '\0'; + adj_match_dir(&state->match_direction); + *ans = get_mext_match(state->pattern, + state->match_direction); + } else if (key == KEY_DOWN) { + state->match_direction = FIND_NEXT_MATCH_DOWN; + *ans = get_mext_match(state->pattern, + state->match_direction); + } else if (key == KEY_UP) { + state->match_direction = FIND_NEXT_MATCH_UP; + *ans = get_mext_match(state->pattern, + state->match_direction); + } else if (key == KEY_BACKSPACE || key == 127) { + state->pattern[strlen(state->pattern)-1] = '\0'; + adj_match_dir(&state->match_direction); + } else + terminate_search = 1; + + if (terminate_search) { + state->in_search = 0; + bzero(state->pattern, sizeof(state->pattern)); + move(0, 0); + refresh(); + clrtoeol(); + return -1; + } + return 0; +} + +static void conf(struct menu *menu) +{ + struct menu *submenu = 0; + const char *prompt = menu_get_prompt(menu); + struct symbol *sym; + int res; + int current_index = 0; + int last_top_row = 0; + struct match_state match_state = { + .in_search = 0, + .match_direction = MATCH_TINKER_PATTERN_DOWN, + .pattern = "", + }; + + while (!global_exit) { + reset_menu(); + current_menu = menu; + build_conf(menu); + if (!child_count) + break; + + show_menu(prompt ? _(prompt) : _("Main Menu"), + _(menu_instructions), + current_index, &last_top_row); + keypad((menu_win(curses_menu)), TRUE); + while (!global_exit) { + if (match_state.in_search) { + mvprintw(0, 0, + "searching: %s", match_state.pattern); + clrtoeol(); + } + refresh_all_windows(main_window); + res = wgetch(menu_win(curses_menu)); + if (!res) + break; + if (do_match(res, &match_state, ¤t_index) == 0) { + if (current_index != -1) + center_item(current_index, + &last_top_row); + continue; + } + if (process_special_keys(&res, + (struct menu *) item_data())) + break; + switch (res) { + case KEY_DOWN: + menu_driver(curses_menu, REQ_DOWN_ITEM); + break; + case KEY_UP: + menu_driver(curses_menu, REQ_UP_ITEM); + break; + case KEY_NPAGE: + menu_driver(curses_menu, REQ_SCR_DPAGE); + break; + case KEY_PPAGE: + menu_driver(curses_menu, REQ_SCR_UPAGE); + break; + case KEY_HOME: + menu_driver(curses_menu, REQ_FIRST_ITEM); + break; + case KEY_END: + menu_driver(curses_menu, REQ_LAST_ITEM); + break; + case 'h': + case '?': + show_help((struct menu *) item_data()); + break; + } + if (res == 10 || res == 27 || + res == 32 || res == 'n' || res == 'y' || + res == KEY_LEFT || res == KEY_RIGHT || + res == 'm') + break; + refresh_all_windows(main_window); + } + + refresh_all_windows(main_window); + /* if ESC or left*/ + if (res == 27 || (menu != &rootmenu && res == KEY_LEFT)) + break; + + /* remember location in the menu */ + last_top_row = top_row(curses_menu); + current_index = curses_item_index(); + + if (!item_tag()) + continue; + + submenu = (struct menu *) item_data(); + if (!submenu || !menu_is_visible(submenu)) + continue; + sym = submenu->sym; + + switch (res) { + case ' ': + if (item_is_tag('t')) + sym_toggle_tristate_value(sym); + else if (item_is_tag('m')) + conf(submenu); + break; + case KEY_RIGHT: + case 10: /* ENTER WAS PRESSED */ + switch (item_tag()) { + case 'm': + if (single_menu_mode) + submenu->data = + (void *) (long) !submenu->data; + else + conf(submenu); + break; + case 't': + if (sym_is_choice(sym) && + sym_get_tristate_value(sym) == yes) + conf_choice(submenu); + else if (submenu->prompt && + submenu->prompt->type == P_MENU) + conf(submenu); + else if (res == 10) + sym_toggle_tristate_value(sym); + break; + case 's': + conf_string(submenu); + break; + } + break; + case 'y': + if (item_is_tag('t')) { + if (sym_set_tristate_value(sym, yes)) + break; + if (sym_set_tristate_value(sym, mod)) + btn_dialog(main_window, setmod_text, 0); + } + break; + case 'n': + if (item_is_tag('t')) + sym_set_tristate_value(sym, no); + break; + case 'm': + if (item_is_tag('t')) + sym_set_tristate_value(sym, mod); + break; + } + } +} + +static void conf_message_callback(const char *fmt, va_list ap) +{ + char buf[1024]; + + vsnprintf(buf, sizeof(buf), fmt, ap); + btn_dialog(main_window, buf, 1, ""); +} + +static void show_help(struct menu *menu) +{ + struct gstr help; + + if (!menu) + return; + + help = str_new(); + menu_get_ext_help(menu, &help); + show_scroll_win(main_window, _(menu_get_prompt(menu)), str_get(&help)); + str_free(&help); +} + +static void conf_choice(struct menu *menu) +{ + const char *prompt = _(menu_get_prompt(menu)); + struct menu *child = 0; + struct symbol *active; + int selected_index = 0; + int last_top_row = 0; + int res, i = 0; + struct match_state match_state = { + .in_search = 0, + .match_direction = MATCH_TINKER_PATTERN_DOWN, + .pattern = "", + }; + + active = sym_get_choice_value(menu->sym); + /* this is mostly duplicated from the conf() function. */ + while (!global_exit) { + reset_menu(); + + for (i = 0, child = menu->list; child; child = child->next) { + if (!show_all_items && !menu_is_visible(child)) + continue; + + if (child->sym == sym_get_choice_value(menu->sym)) + item_make(child, ':', " %s", + _(menu_get_prompt(child))); + else if (child->sym) + item_make(child, ':', " %s", + _(menu_get_prompt(child))); + else + item_make(child, ':', "*** %s ***", + _(menu_get_prompt(child))); + + if (child->sym == active){ + last_top_row = top_row(curses_menu); + selected_index = i; + } + i++; + } + show_menu(prompt ? _(prompt) : _("Choice Menu"), + _(radiolist_instructions), + selected_index, + &last_top_row); + while (!global_exit) { + if (match_state.in_search) { + mvprintw(0, 0, "searching: %s", + match_state.pattern); + clrtoeol(); + } + refresh_all_windows(main_window); + res = wgetch(menu_win(curses_menu)); + if (!res) + break; + if (do_match(res, &match_state, &selected_index) == 0) { + if (selected_index != -1) + center_item(selected_index, + &last_top_row); + continue; + } + if (process_special_keys( + &res, + (struct menu *) item_data())) + break; + switch (res) { + case KEY_DOWN: + menu_driver(curses_menu, REQ_DOWN_ITEM); + break; + case KEY_UP: + menu_driver(curses_menu, REQ_UP_ITEM); + break; + case KEY_NPAGE: + menu_driver(curses_menu, REQ_SCR_DPAGE); + break; + case KEY_PPAGE: + menu_driver(curses_menu, REQ_SCR_UPAGE); + break; + case KEY_HOME: + menu_driver(curses_menu, REQ_FIRST_ITEM); + break; + case KEY_END: + menu_driver(curses_menu, REQ_LAST_ITEM); + break; + case 'h': + case '?': + show_help((struct menu *) item_data()); + break; + } + if (res == 10 || res == 27 || res == ' ' || + res == KEY_LEFT){ + break; + } + refresh_all_windows(main_window); + } + /* if ESC or left */ + if (res == 27 || res == KEY_LEFT) + break; + + child = item_data(); + if (!child || !menu_is_visible(child) || !child->sym) + continue; + switch (res) { + case ' ': + case 10: + case KEY_RIGHT: + sym_set_tristate_value(child->sym, yes); + return; + case 'h': + case '?': + show_help(child); + active = child->sym; + break; + case KEY_EXIT: + return; + } + } +} + +static void conf_string(struct menu *menu) +{ + const char *prompt = menu_get_prompt(menu); + + while (1) { + int res; + const char *heading; + + switch (sym_get_type(menu->sym)) { + case S_INT: + heading = _(inputbox_instructions_int); + break; + case S_HEX: + heading = _(inputbox_instructions_hex); + break; + case S_STRING: + heading = _(inputbox_instructions_string); + break; + default: + heading = _("Internal nconf error!"); + } + res = dialog_inputbox(main_window, + prompt ? _(prompt) : _("Main Menu"), + heading, + sym_get_string_value(menu->sym), + &dialog_input_result, + &dialog_input_result_len); + switch (res) { + case 0: + if (sym_set_string_value(menu->sym, + dialog_input_result)) + return; + btn_dialog(main_window, + _("You have made an invalid entry."), 0); + break; + case 1: + show_help(menu); + break; + case KEY_EXIT: + return; + } + } +} + +static void conf_load(void) +{ + while (1) { + int res; + res = dialog_inputbox(main_window, + NULL, load_config_text, + filename, + &dialog_input_result, + &dialog_input_result_len); + switch (res) { + case 0: + if (!dialog_input_result[0]) + return; + if (!conf_read(dialog_input_result)) { + set_config_filename(dialog_input_result); + sym_set_change_count(1); + return; + } + btn_dialog(main_window, _("File does not exist!"), 0); + break; + case 1: + show_scroll_win(main_window, + _("Load Alternate Configuration"), + load_config_help); + break; + case KEY_EXIT: + return; + } + } +} + +static void conf_save(void) +{ + while (1) { + int res; + res = dialog_inputbox(main_window, + NULL, save_config_text, + filename, + &dialog_input_result, + &dialog_input_result_len); + switch (res) { + case 0: + if (!dialog_input_result[0]) + return; + res = conf_write(dialog_input_result); + if (!res) { + set_config_filename(dialog_input_result); + return; + } + btn_dialog(main_window, _("Can't create file! " + "Probably a nonexistent directory."), + 1, ""); + break; + case 1: + show_scroll_win(main_window, + _("Save Alternate Configuration"), + save_config_help); + break; + case KEY_EXIT: + return; + } + } +} + +void setup_windows(void) +{ + int lines, columns; + + getmaxyx(stdscr, lines, columns); + + if (main_window != NULL) + delwin(main_window); + + /* set up the menu and menu window */ + main_window = newwin(lines-2, columns-2, 2, 1); + keypad(main_window, TRUE); + mwin_max_lines = lines-7; + mwin_max_cols = columns-6; + + /* panels order is from bottom to top */ + new_panel(main_window); +} + +int main(int ac, char **av) +{ + int lines, columns; + char *mode; + + setlocale(LC_ALL, ""); + bindtextdomain(PACKAGE, LOCALEDIR); + textdomain(PACKAGE); + + if (ac > 1 && strcmp(av[1], "-s") == 0) { + /* Silence conf_read() until the real callback is set up */ + conf_set_message_callback(NULL); + av++; + } + conf_parse(av[1]); + conf_read(NULL); + + mode = getenv("NCONFIG_MODE"); + if (mode) { + if (!strcasecmp(mode, "single_menu")) + single_menu_mode = 1; + } + + /* Initialize curses */ + initscr(); + /* set color theme */ + set_colors(); + + cbreak(); + noecho(); + keypad(stdscr, TRUE); + curs_set(0); + + getmaxyx(stdscr, lines, columns); + if (columns < 75 || lines < 20) { + endwin(); + printf("Your terminal should have at " + "least 20 lines and 75 columns\n"); + return 1; + } + + notimeout(stdscr, FALSE); +#if NCURSES_REENTRANT + set_escdelay(1); +#else + ESCDELAY = 1; +#endif + + /* set btns menu */ + curses_menu = new_menu(curses_menu_items); + menu_opts_off(curses_menu, O_SHOWDESC); + menu_opts_on(curses_menu, O_SHOWMATCH); + menu_opts_on(curses_menu, O_ONEVALUE); + menu_opts_on(curses_menu, O_NONCYCLIC); + menu_opts_on(curses_menu, O_IGNORECASE); + set_menu_mark(curses_menu, " "); + set_menu_fore(curses_menu, attributes[MAIN_MENU_FORE]); + set_menu_back(curses_menu, attributes[MAIN_MENU_BACK]); + set_menu_grey(curses_menu, attributes[MAIN_MENU_GREY]); + + set_config_filename(conf_get_configname()); + setup_windows(); + + /* check for KEY_FUNC(1) */ + if (has_key(KEY_F(1)) == FALSE) { + show_scroll_win(main_window, + _("Instructions"), + _(menu_no_f_instructions)); + } + + conf_set_message_callback(conf_message_callback); + /* do the work */ + while (!global_exit) { + conf(&rootmenu); + if (!global_exit && do_exit() == 0) + break; + } + /* ok, we are done */ + unpost_menu(curses_menu); + free_menu(curses_menu); + delwin(main_window); + clear(); + refresh(); + endwin(); + return 0; +} diff --git a/scripts/kconfig/nconf.gui.c b/scripts/kconfig/nconf.gui.c new file mode 100644 index 0000000000..8275f0e551 --- /dev/null +++ b/scripts/kconfig/nconf.gui.c @@ -0,0 +1,656 @@ +/* + * Copyright (C) 2008 Nir Tzachar 0) + win_rows = msg_lines+4; + else + win_rows = msg_lines+2; + + win = newwin(win_rows, total_width+4, y, x); + keypad(win, TRUE); + menu_win = derwin(win, 1, btns_width, win_rows-2, + 1+(total_width+2-btns_width)/2); + menu = new_menu(btns); + msg_win = derwin(win, win_rows-2, msg_width, 1, + 1+(total_width+2-msg_width)/2); + + set_menu_fore(menu, attributes[DIALOG_MENU_FORE]); + set_menu_back(menu, attributes[DIALOG_MENU_BACK]); + + (void) wattrset(win, attributes[DIALOG_BOX]); + box(win, 0, 0); + + /* print message */ + (void) wattrset(msg_win, attributes[DIALOG_TEXT]); + fill_window(msg_win, msg); + + set_menu_win(menu, win); + set_menu_sub(menu, menu_win); + set_menu_format(menu, 1, btn_num); + menu_opts_off(menu, O_SHOWDESC); + menu_opts_off(menu, O_SHOWMATCH); + menu_opts_on(menu, O_ONEVALUE); + menu_opts_on(menu, O_NONCYCLIC); + set_menu_mark(menu, ""); + post_menu(menu); + + + touchwin(win); + refresh_all_windows(main_window); + while ((res = wgetch(win))) { + switch (res) { + case KEY_LEFT: + menu_driver(menu, REQ_LEFT_ITEM); + break; + case KEY_RIGHT: + menu_driver(menu, REQ_RIGHT_ITEM); + break; + case 10: /* ENTER */ + case 27: /* ESCAPE */ + case ' ': + case KEY_F(F_BACK): + case KEY_F(F_EXIT): + break; + } + touchwin(win); + refresh_all_windows(main_window); + + if (res == 10 || res == ' ') { + res = item_index(current_item(menu)); + break; + } else if (res == 27 || res == KEY_F(F_BACK) || + res == KEY_F(F_EXIT)) { + res = KEY_EXIT; + break; + } + } + + unpost_menu(menu); + free_menu(menu); + for (i = 0; i < btn_num; i++) + free_item(btns[i]); + + delwin(win); + return res; +} + +int dialog_inputbox(WINDOW *main_window, + const char *title, const char *prompt, + const char *init, char **resultp, int *result_len) +{ + int prompt_lines = 0; + int prompt_width = 0; + WINDOW *win; + WINDOW *prompt_win; + WINDOW *form_win; + PANEL *panel; + int i, x, y; + int res = -1; + int cursor_position = strlen(init); + int cursor_form_win; + char *result = *resultp; + + if (strlen(init)+1 > *result_len) { + *result_len = strlen(init)+1; + *resultp = result = realloc(result, *result_len); + } + + /* find the widest line of msg: */ + prompt_lines = get_line_no(prompt); + for (i = 0; i < prompt_lines; i++) { + const char *line = get_line(prompt, i); + int len = get_line_length(line); + prompt_width = max(prompt_width, len); + } + + if (title) + prompt_width = max(prompt_width, strlen(title)); + + /* place dialog in middle of screen */ + y = (getmaxy(stdscr)-(prompt_lines+4))/2; + x = (getmaxx(stdscr)-(prompt_width+4))/2; + + strncpy(result, init, *result_len); + + /* create the windows */ + win = newwin(prompt_lines+6, prompt_width+7, y, x); + prompt_win = derwin(win, prompt_lines+1, prompt_width, 2, 2); + form_win = derwin(win, 1, prompt_width, prompt_lines+3, 2); + keypad(form_win, TRUE); + + (void) wattrset(form_win, attributes[INPUT_FIELD]); + + (void) wattrset(win, attributes[INPUT_BOX]); + box(win, 0, 0); + (void) wattrset(win, attributes[INPUT_HEADING]); + if (title) + mvwprintw(win, 0, 3, "%s", title); + + /* print message */ + (void) wattrset(prompt_win, attributes[INPUT_TEXT]); + fill_window(prompt_win, prompt); + + mvwprintw(form_win, 0, 0, "%*s", prompt_width, " "); + cursor_form_win = min(cursor_position, prompt_width-1); + mvwprintw(form_win, 0, 0, "%s", + result + cursor_position-cursor_form_win); + + /* create panels */ + panel = new_panel(win); + + /* show the cursor */ + curs_set(1); + + touchwin(win); + refresh_all_windows(main_window); + while ((res = wgetch(form_win))) { + int len = strlen(result); + switch (res) { + case 10: /* ENTER */ + case 27: /* ESCAPE */ + case KEY_F(F_HELP): + case KEY_F(F_EXIT): + case KEY_F(F_BACK): + break; + case 127: + case KEY_BACKSPACE: + if (cursor_position > 0) { + memmove(&result[cursor_position-1], + &result[cursor_position], + len-cursor_position+1); + cursor_position--; + cursor_form_win--; + len--; + } + break; + case KEY_DC: + if (cursor_position >= 0 && cursor_position < len) { + memmove(&result[cursor_position], + &result[cursor_position+1], + len-cursor_position+1); + len--; + } + break; + case KEY_UP: + case KEY_RIGHT: + if (cursor_position < len) { + cursor_position++; + cursor_form_win++; + } + break; + case KEY_DOWN: + case KEY_LEFT: + if (cursor_position > 0) { + cursor_position--; + cursor_form_win--; + } + break; + case KEY_HOME: + cursor_position = 0; + cursor_form_win = 0; + break; + case KEY_END: + cursor_position = len; + cursor_form_win = min(cursor_position, prompt_width-1); + break; + default: + if ((isgraph(res) || isspace(res))) { + /* one for new char, one for '\0' */ + if (len+2 > *result_len) { + *result_len = len+2; + *resultp = result = realloc(result, + *result_len); + } + /* insert the char at the proper position */ + memmove(&result[cursor_position+1], + &result[cursor_position], + len-cursor_position+1); + result[cursor_position] = res; + cursor_position++; + cursor_form_win++; + len++; + } else { + mvprintw(0, 0, "unknown key: %d\n", res); + } + break; + } + if (cursor_form_win < 0) + cursor_form_win = 0; + else if (cursor_form_win > prompt_width-1) + cursor_form_win = prompt_width-1; + + wmove(form_win, 0, 0); + wclrtoeol(form_win); + mvwprintw(form_win, 0, 0, "%*s", prompt_width, " "); + mvwprintw(form_win, 0, 0, "%s", + result + cursor_position-cursor_form_win); + wmove(form_win, 0, cursor_form_win); + touchwin(win); + refresh_all_windows(main_window); + + if (res == 10) { + res = 0; + break; + } else if (res == 27 || res == KEY_F(F_BACK) || + res == KEY_F(F_EXIT)) { + res = KEY_EXIT; + break; + } else if (res == KEY_F(F_HELP)) { + res = 1; + break; + } + } + + /* hide the cursor */ + curs_set(0); + del_panel(panel); + delwin(prompt_win); + delwin(form_win); + delwin(win); + return res; +} + +/* refresh all windows in the correct order */ +void refresh_all_windows(WINDOW *main_window) +{ + update_panels(); + touchwin(main_window); + refresh(); +} + +/* layman's scrollable window... */ +void show_scroll_win(WINDOW *main_window, + const char *title, + const char *text) +{ + int res; + int total_lines = get_line_no(text); + int x, y, lines, columns; + int start_x = 0, start_y = 0; + int text_lines = 0, text_cols = 0; + int total_cols = 0; + int win_cols = 0; + int win_lines = 0; + int i = 0; + WINDOW *win; + WINDOW *pad; + PANEL *panel; + + getmaxyx(stdscr, lines, columns); + + /* find the widest line of msg: */ + total_lines = get_line_no(text); + for (i = 0; i < total_lines; i++) { + const char *line = get_line(text, i); + int len = get_line_length(line); + total_cols = max(total_cols, len+2); + } + + /* create the pad */ + pad = newpad(total_lines+10, total_cols+10); + (void) wattrset(pad, attributes[SCROLLWIN_TEXT]); + fill_window(pad, text); + + win_lines = min(total_lines+4, lines-2); + win_cols = min(total_cols+2, columns-2); + text_lines = max(win_lines-4, 0); + text_cols = max(win_cols-2, 0); + + /* place window in middle of screen */ + y = (lines-win_lines)/2; + x = (columns-win_cols)/2; + + win = newwin(win_lines, win_cols, y, x); + keypad(win, TRUE); + /* show the help in the help window, and show the help panel */ + (void) wattrset(win, attributes[SCROLLWIN_BOX]); + box(win, 0, 0); + (void) wattrset(win, attributes[SCROLLWIN_HEADING]); + mvwprintw(win, 0, 3, " %s ", title); + panel = new_panel(win); + + /* handle scrolling */ + do { + + copywin(pad, win, start_y, start_x, 2, 2, text_lines, + text_cols, 0); + print_in_middle(win, + text_lines+2, + 0, + text_cols, + "", + attributes[DIALOG_MENU_FORE]); + wrefresh(win); + + res = wgetch(win); + switch (res) { + case KEY_NPAGE: + case ' ': + case 'd': + start_y += text_lines-2; + break; + case KEY_PPAGE: + case 'u': + start_y -= text_lines+2; + break; + case KEY_HOME: + start_y = 0; + break; + case KEY_END: + start_y = total_lines-text_lines; + break; + case KEY_DOWN: + case 'j': + start_y++; + break; + case KEY_UP: + case 'k': + start_y--; + break; + case KEY_LEFT: + case 'h': + start_x--; + break; + case KEY_RIGHT: + case 'l': + start_x++; + break; + } + if (res == 10 || res == 27 || res == 'q' || + res == KEY_F(F_HELP) || res == KEY_F(F_BACK) || + res == KEY_F(F_EXIT)) + break; + if (start_y < 0) + start_y = 0; + if (start_y >= total_lines-text_lines) + start_y = total_lines-text_lines; + if (start_x < 0) + start_x = 0; + if (start_x >= total_cols-text_cols) + start_x = total_cols-text_cols; + } while (res); + + del_panel(panel); + delwin(win); + refresh_all_windows(main_window); +} diff --git a/scripts/kconfig/nconf.h b/scripts/kconfig/nconf.h new file mode 100644 index 0000000000..0d5261705e --- /dev/null +++ b/scripts/kconfig/nconf.h @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2008 Nir Tzachar +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "ncurses.h" + +#define max(a, b) ({\ + typeof(a) _a = a;\ + typeof(b) _b = b;\ + _a > _b ? _a : _b; }) + +#define min(a, b) ({\ + typeof(a) _a = a;\ + typeof(b) _b = b;\ + _a < _b ? _a : _b; }) + +typedef enum { + NORMAL = 1, + MAIN_HEADING, + MAIN_MENU_BOX, + MAIN_MENU_FORE, + MAIN_MENU_BACK, + MAIN_MENU_GREY, + MAIN_MENU_HEADING, + SCROLLWIN_TEXT, + SCROLLWIN_HEADING, + SCROLLWIN_BOX, + DIALOG_TEXT, + DIALOG_MENU_FORE, + DIALOG_MENU_BACK, + DIALOG_BOX, + INPUT_BOX, + INPUT_HEADING, + INPUT_TEXT, + INPUT_FIELD, + FUNCTION_TEXT, + FUNCTION_HIGHLIGHT, + ATTR_MAX +} attributes_t; +extern attributes_t attributes[]; + +typedef enum { + F_HELP = 1, + F_SYMBOL = 2, + F_INSTS = 3, + F_CONF = 4, + F_BACK = 5, + F_SAVE = 6, + F_LOAD = 7, + F_SEARCH = 8, + F_EXIT = 9, +} function_key; + +void set_colors(void); + +/* this changes the windows attributes !!! */ +void print_in_middle(WINDOW *win, + int starty, + int startx, + int width, + const char *string, + chtype color); +int get_line_length(const char *line); +int get_line_no(const char *text); +const char *get_line(const char *text, int line_no); +void fill_window(WINDOW *win, const char *text); +int btn_dialog(WINDOW *main_window, const char *msg, int btn_num, ...); +int dialog_inputbox(WINDOW *main_window, + const char *title, const char *prompt, + const char *init, char **resultp, int *result_len); +void refresh_all_windows(WINDOW *main_window); +void show_scroll_win(WINDOW *main_window, + const char *title, + const char *text); diff --git a/scripts/kconfig/qconf.cc b/scripts/kconfig/qconf.cc new file mode 100644 index 0000000000..fc55559922 --- /dev/null +++ b/scripts/kconfig/qconf.cc @@ -0,0 +1,1870 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Copyright (C) 2015 Boris Barbulovski + * Released under the terms of the GNU GPL v2.0. + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "lkc.h" +#include "qconf.h" + +#include "qconf.moc" +#include "images.c" + +#ifdef _ +# undef _ +# define _ qgettext +#endif + +static QApplication *configApp; +static ConfigSettings *configSettings; + +QAction *ConfigMainWindow::saveAction; + +static inline QString qgettext(const char* str) +{ + return QString::fromLocal8Bit(gettext(str)); +} + +static inline QString qgettext(const QString& str) +{ + return QString::fromLocal8Bit(gettext(str.toLatin1())); +} + +ConfigSettings::ConfigSettings() + : QSettings("kernel.org", "qconf") +{ +} + +/** + * Reads a list of integer values from the application settings. + */ +QList ConfigSettings::readSizes(const QString& key, bool *ok) +{ + QList result; + QStringList entryList = value(key).toStringList(); + QStringList::Iterator it; + + for (it = entryList.begin(); it != entryList.end(); ++it) + result.push_back((*it).toInt()); + + return result; +} + +/** + * Writes a list of integer values to the application settings. + */ +bool ConfigSettings::writeSizes(const QString& key, const QList& value) +{ + QStringList stringList; + QList::ConstIterator it; + + for (it = value.begin(); it != value.end(); ++it) + stringList.push_back(QString::number(*it)); + setValue(key, stringList); + + return true; +} + + +/* + * set the new data + * TODO check the value + */ +void ConfigItem::okRename(int col) +{ +} + +/* + * update the displayed of a menu entry + */ +void ConfigItem::updateMenu(void) +{ + ConfigList* list; + struct symbol* sym; + struct property *prop; + QString prompt; + int type; + tristate expr; + + list = listView(); + if (goParent) { + setPixmap(promptColIdx, list->menuBackPix); + prompt = ".."; + goto set_prompt; + } + + sym = menu->sym; + prop = menu->prompt; + prompt = _(menu_get_prompt(menu)); + + if (prop) switch (prop->type) { + case P_MENU: + if (list->mode == singleMode || list->mode == symbolMode) { + /* a menuconfig entry is displayed differently + * depending whether it's at the view root or a child. + */ + if (sym && list->rootEntry == menu) + break; + setPixmap(promptColIdx, list->menuPix); + } else { + if (sym) + break; + setPixmap(promptColIdx, QIcon()); + } + goto set_prompt; + case P_COMMENT: + setPixmap(promptColIdx, QIcon()); + goto set_prompt; + default: + ; + } + if (!sym) + goto set_prompt; + + setText(nameColIdx, QString::fromLocal8Bit(sym->name)); + + type = sym_get_type(sym); + switch (type) { + case S_BOOLEAN: + case S_TRISTATE: + char ch; + + if (!sym_is_changable(sym) && list->optMode == normalOpt) { + setPixmap(promptColIdx, QIcon()); + setText(noColIdx, QString::null); + setText(modColIdx, QString::null); + setText(yesColIdx, QString::null); + break; + } + expr = sym_get_tristate_value(sym); + switch (expr) { + case yes: + if (sym_is_choice_value(sym) && type == S_BOOLEAN) + setPixmap(promptColIdx, list->choiceYesPix); + else + setPixmap(promptColIdx, list->symbolYesPix); + setText(yesColIdx, "Y"); + ch = 'Y'; + break; + case mod: + setPixmap(promptColIdx, list->symbolModPix); + setText(modColIdx, "M"); + ch = 'M'; + break; + default: + if (sym_is_choice_value(sym) && type == S_BOOLEAN) + setPixmap(promptColIdx, list->choiceNoPix); + else + setPixmap(promptColIdx, list->symbolNoPix); + setText(noColIdx, "N"); + ch = 'N'; + break; + } + if (expr != no) + setText(noColIdx, sym_tristate_within_range(sym, no) ? "_" : 0); + if (expr != mod) + setText(modColIdx, sym_tristate_within_range(sym, mod) ? "_" : 0); + if (expr != yes) + setText(yesColIdx, sym_tristate_within_range(sym, yes) ? "_" : 0); + + setText(dataColIdx, QChar(ch)); + break; + case S_INT: + case S_HEX: + case S_STRING: + const char* data; + + data = sym_get_string_value(sym); + + setText(dataColIdx, data); + if (type == S_STRING) + prompt = QString("%1: %2").arg(prompt).arg(data); + else + prompt = QString("(%2) %1").arg(prompt).arg(data); + break; + } + if (!sym_has_value(sym) && visible) + prompt += _(" (NEW)"); +set_prompt: + setText(promptColIdx, prompt); +} + +void ConfigItem::testUpdateMenu(bool v) +{ + ConfigItem* i; + + visible = v; + if (!menu) + return; + + sym_calc_value(menu->sym); + if (menu->flags & MENU_CHANGED) { + /* the menu entry changed, so update all list items */ + menu->flags &= ~MENU_CHANGED; + for (i = (ConfigItem*)menu->data; i; i = i->nextItem) + i->updateMenu(); + } else if (listView()->updateAll) + updateMenu(); +} + + +/* + * construct a menu entry + */ +void ConfigItem::init(void) +{ + if (menu) { + ConfigList* list = listView(); + nextItem = (ConfigItem*)menu->data; + menu->data = this; + + if (list->mode != fullMode) + setExpanded(true); + sym_calc_value(menu->sym); + } + updateMenu(); +} + +/* + * destruct a menu entry + */ +ConfigItem::~ConfigItem(void) +{ + if (menu) { + ConfigItem** ip = (ConfigItem**)&menu->data; + for (; *ip; ip = &(*ip)->nextItem) { + if (*ip == this) { + *ip = nextItem; + break; + } + } + } +} + +ConfigLineEdit::ConfigLineEdit(ConfigView* parent) + : Parent(parent) +{ + connect(this, SIGNAL(editingFinished()), SLOT(hide())); +} + +void ConfigLineEdit::show(ConfigItem* i) +{ + item = i; + if (sym_get_string_value(item->menu->sym)) + setText(QString::fromLocal8Bit(sym_get_string_value(item->menu->sym))); + else + setText(QString::null); + Parent::show(); + setFocus(); +} + +void ConfigLineEdit::keyPressEvent(QKeyEvent* e) +{ + switch (e->key()) { + case Qt::Key_Escape: + break; + case Qt::Key_Return: + case Qt::Key_Enter: + sym_set_string_value(item->menu->sym, text().toLatin1()); + parent()->updateList(item); + break; + default: + Parent::keyPressEvent(e); + return; + } + e->accept(); + parent()->list->setFocus(); + hide(); +} + +ConfigList::ConfigList(ConfigView* p, const char *name) + : Parent(p), + updateAll(false), + symbolYesPix(xpm_symbol_yes), symbolModPix(xpm_symbol_mod), symbolNoPix(xpm_symbol_no), + choiceYesPix(xpm_choice_yes), choiceNoPix(xpm_choice_no), + menuPix(xpm_menu), menuInvPix(xpm_menu_inv), menuBackPix(xpm_menuback), voidPix(xpm_void), + showName(false), showRange(false), showData(false), mode(singleMode), optMode(normalOpt), + rootEntry(0), headerPopup(0) +{ + int i; + + setObjectName(name); + setSortingEnabled(false); + setRootIsDecorated(true); + + setVerticalScrollMode(ScrollPerPixel); + setHorizontalScrollMode(ScrollPerPixel); + + setHeaderLabels(QStringList() << _("Option") << _("Name") << "N" << "M" << "Y" << _("Value")); + + connect(this, SIGNAL(itemSelectionChanged(void)), + SLOT(updateSelection(void))); + + if (name) { + configSettings->beginGroup(name); + showName = configSettings->value("/showName", false).toBool(); + showRange = configSettings->value("/showRange", false).toBool(); + showData = configSettings->value("/showData", false).toBool(); + optMode = (enum optionMode)configSettings->value("/optionMode", 0).toInt(); + configSettings->endGroup(); + connect(configApp, SIGNAL(aboutToQuit()), SLOT(saveSettings())); + } + + addColumn(promptColIdx); + + reinit(); +} + +bool ConfigList::menuSkip(struct menu *menu) +{ + if (optMode == normalOpt && menu_is_visible(menu)) + return false; + if (optMode == promptOpt && menu_has_prompt(menu)) + return false; + if (optMode == allOpt) + return false; + return true; +} + +void ConfigList::reinit(void) +{ + removeColumn(dataColIdx); + removeColumn(yesColIdx); + removeColumn(modColIdx); + removeColumn(noColIdx); + removeColumn(nameColIdx); + + if (showName) + addColumn(nameColIdx); + if (showRange) { + addColumn(noColIdx); + addColumn(modColIdx); + addColumn(yesColIdx); + } + if (showData) + addColumn(dataColIdx); + + updateListAll(); +} + +void ConfigList::saveSettings(void) +{ + if (!objectName().isEmpty()) { + configSettings->beginGroup(objectName()); + configSettings->setValue("/showName", showName); + configSettings->setValue("/showRange", showRange); + configSettings->setValue("/showData", showData); + configSettings->setValue("/optionMode", (int)optMode); + configSettings->endGroup(); + } +} + +ConfigItem* ConfigList::findConfigItem(struct menu *menu) +{ + ConfigItem* item = (ConfigItem*)menu->data; + + for (; item; item = item->nextItem) { + if (this == item->listView()) + break; + } + + return item; +} + +void ConfigList::updateSelection(void) +{ + struct menu *menu; + enum prop_type type; + + if (selectedItems().count() == 0) + return; + + ConfigItem* item = (ConfigItem*)selectedItems().first(); + if (!item) + return; + + menu = item->menu; + emit menuChanged(menu); + if (!menu) + return; + type = menu->prompt ? menu->prompt->type : P_UNKNOWN; + if (mode == menuMode && type == P_MENU) + emit menuSelected(menu); +} + +void ConfigList::updateList(ConfigItem* item) +{ + ConfigItem* last = 0; + + if (!rootEntry) { + if (mode != listMode) + goto update; + QTreeWidgetItemIterator it(this); + ConfigItem* item; + + while (*it) { + item = (ConfigItem*)(*it); + if (!item->menu) + continue; + item->testUpdateMenu(menu_is_visible(item->menu)); + + ++it; + } + return; + } + + if (rootEntry != &rootmenu && (mode == singleMode || + (mode == symbolMode && rootEntry->parent != &rootmenu))) { + item = (ConfigItem *)topLevelItem(0); + if (!item) + item = new ConfigItem(this, 0, true); + last = item; + } + if ((mode == singleMode || (mode == symbolMode && !(rootEntry->flags & MENU_ROOT))) && + rootEntry->sym && rootEntry->prompt) { + item = last ? last->nextSibling() : firstChild(); + if (!item) + item = new ConfigItem(this, last, rootEntry, true); + else + item->testUpdateMenu(true); + + updateMenuList(item, rootEntry); + update(); + resizeColumnToContents(0); + return; + } +update: + updateMenuList(this, rootEntry); + update(); + resizeColumnToContents(0); +} + +void ConfigList::setValue(ConfigItem* item, tristate val) +{ + struct symbol* sym; + int type; + tristate oldval; + + sym = item->menu ? item->menu->sym : 0; + if (!sym) + return; + + type = sym_get_type(sym); + switch (type) { + case S_BOOLEAN: + case S_TRISTATE: + oldval = sym_get_tristate_value(sym); + + if (!sym_set_tristate_value(sym, val)) + return; + if (oldval == no && item->menu->list) + item->setExpanded(true); + parent()->updateList(item); + break; + } +} + +void ConfigList::changeValue(ConfigItem* item) +{ + struct symbol* sym; + struct menu* menu; + int type, oldexpr, newexpr; + + menu = item->menu; + if (!menu) + return; + sym = menu->sym; + if (!sym) { + if (item->menu->list) + item->setExpanded(!item->isExpanded()); + return; + } + + type = sym_get_type(sym); + switch (type) { + case S_BOOLEAN: + case S_TRISTATE: + oldexpr = sym_get_tristate_value(sym); + newexpr = sym_toggle_tristate_value(sym); + if (item->menu->list) { + if (oldexpr == newexpr) + item->setExpanded(!item->isExpanded()); + else if (oldexpr == no) + item->setExpanded(true); + } + if (oldexpr != newexpr) + parent()->updateList(item); + break; + case S_INT: + case S_HEX: + case S_STRING: + parent()->lineEdit->show(item); + break; + } +} + +void ConfigList::setRootMenu(struct menu *menu) +{ + enum prop_type type; + + if (rootEntry == menu) + return; + type = menu && menu->prompt ? menu->prompt->type : P_UNKNOWN; + if (type != P_MENU) + return; + updateMenuList(this, 0); + rootEntry = menu; + updateListAll(); + if (currentItem()) { + currentItem()->setSelected(hasFocus()); + scrollToItem(currentItem()); + } +} + +void ConfigList::setParentMenu(void) +{ + ConfigItem* item; + struct menu *oldroot; + + oldroot = rootEntry; + if (rootEntry == &rootmenu) + return; + setRootMenu(menu_get_parent_menu(rootEntry->parent)); + + QTreeWidgetItemIterator it(this); + while (*it) { + item = (ConfigItem *)(*it); + if (item->menu == oldroot) { + setCurrentItem(item); + scrollToItem(item); + break; + } + + ++it; + } +} + +/* + * update all the children of a menu entry + * removes/adds the entries from the parent widget as necessary + * + * parent: either the menu list widget or a menu entry widget + * menu: entry to be updated + */ +void ConfigList::updateMenuList(ConfigItem *parent, struct menu* menu) +{ + struct menu* child; + ConfigItem* item; + ConfigItem* last; + bool visible; + enum prop_type type; + + if (!menu) { + while (parent->childCount() > 0) + { + delete parent->takeChild(0); + } + + return; + } + + last = parent->firstChild(); + if (last && !last->goParent) + last = 0; + for (child = menu->list; child; child = child->next) { + item = last ? last->nextSibling() : parent->firstChild(); + type = child->prompt ? child->prompt->type : P_UNKNOWN; + + switch (mode) { + case menuMode: + if (!(child->flags & MENU_ROOT)) + goto hide; + break; + case symbolMode: + if (child->flags & MENU_ROOT) + goto hide; + break; + default: + break; + } + + visible = menu_is_visible(child); + if (!menuSkip(child)) { + if (!child->sym && !child->list && !child->prompt) + continue; + if (!item || item->menu != child) + item = new ConfigItem(parent, last, child, visible); + else + item->testUpdateMenu(visible); + + if (mode == fullMode || mode == menuMode || type != P_MENU) + updateMenuList(item, child); + else + updateMenuList(item, 0); + last = item; + continue; + } + hide: + if (item && item->menu == child) { + last = parent->firstChild(); + if (last == item) + last = 0; + else while (last->nextSibling() != item) + last = last->nextSibling(); + delete item; + } + } +} + +void ConfigList::updateMenuList(ConfigList *parent, struct menu* menu) +{ + struct menu* child; + ConfigItem* item; + ConfigItem* last; + bool visible; + enum prop_type type; + + if (!menu) { + while (parent->topLevelItemCount() > 0) + { + delete parent->takeTopLevelItem(0); + } + + return; + } + + last = (ConfigItem*)parent->topLevelItem(0); + if (last && !last->goParent) + last = 0; + for (child = menu->list; child; child = child->next) { + item = last ? last->nextSibling() : (ConfigItem*)parent->topLevelItem(0); + type = child->prompt ? child->prompt->type : P_UNKNOWN; + + switch (mode) { + case menuMode: + if (!(child->flags & MENU_ROOT)) + goto hide; + break; + case symbolMode: + if (child->flags & MENU_ROOT) + goto hide; + break; + default: + break; + } + + visible = menu_is_visible(child); + if (!menuSkip(child)) { + if (!child->sym && !child->list && !child->prompt) + continue; + if (!item || item->menu != child) + item = new ConfigItem(parent, last, child, visible); + else + item->testUpdateMenu(visible); + + if (mode == fullMode || mode == menuMode || type != P_MENU) + updateMenuList(item, child); + else + updateMenuList(item, 0); + last = item; + continue; + } + hide: + if (item && item->menu == child) { + last = (ConfigItem*)parent->topLevelItem(0); + if (last == item) + last = 0; + else while (last->nextSibling() != item) + last = last->nextSibling(); + delete item; + } + } +} + +void ConfigList::keyPressEvent(QKeyEvent* ev) +{ + QTreeWidgetItem* i = currentItem(); + ConfigItem* item; + struct menu *menu; + enum prop_type type; + + if (ev->key() == Qt::Key_Escape && mode != fullMode && mode != listMode) { + emit parentSelected(); + ev->accept(); + return; + } + + if (!i) { + Parent::keyPressEvent(ev); + return; + } + item = (ConfigItem*)i; + + switch (ev->key()) { + case Qt::Key_Return: + case Qt::Key_Enter: + if (item->goParent) { + emit parentSelected(); + break; + } + menu = item->menu; + if (!menu) + break; + type = menu->prompt ? menu->prompt->type : P_UNKNOWN; + if (type == P_MENU && rootEntry != menu && + mode != fullMode && mode != menuMode) { + emit menuSelected(menu); + break; + } + case Qt::Key_Space: + changeValue(item); + break; + case Qt::Key_N: + setValue(item, no); + break; + case Qt::Key_M: + setValue(item, mod); + break; + case Qt::Key_Y: + setValue(item, yes); + break; + default: + Parent::keyPressEvent(ev); + return; + } + ev->accept(); +} + +void ConfigList::mousePressEvent(QMouseEvent* e) +{ + //QPoint p(contentsToViewport(e->pos())); + //printf("contentsMousePressEvent: %d,%d\n", p.x(), p.y()); + Parent::mousePressEvent(e); +} + +void ConfigList::mouseReleaseEvent(QMouseEvent* e) +{ + QPoint p = e->pos(); + ConfigItem* item = (ConfigItem*)itemAt(p); + struct menu *menu; + enum prop_type ptype; + QIcon icon; + int idx, x; + + if (!item) + goto skip; + + menu = item->menu; + x = header()->offset() + p.x(); + idx = header()->logicalIndexAt(x); + switch (idx) { + case promptColIdx: + icon = item->pixmap(promptColIdx); + if (!icon.isNull()) { + int off = header()->sectionPosition(0) + visualRect(indexAt(p)).x() + 4; // 4 is Hardcoded image offset. There might be a way to do it properly. + if (x >= off && x < off + icon.availableSizes().first().width()) { + if (item->goParent) { + emit parentSelected(); + break; + } else if (!menu) + break; + ptype = menu->prompt ? menu->prompt->type : P_UNKNOWN; + if (ptype == P_MENU && rootEntry != menu && + mode != fullMode && mode != menuMode) + emit menuSelected(menu); + else + changeValue(item); + } + } + break; + case noColIdx: + setValue(item, no); + break; + case modColIdx: + setValue(item, mod); + break; + case yesColIdx: + setValue(item, yes); + break; + case dataColIdx: + changeValue(item); + break; + } + +skip: + //printf("contentsMouseReleaseEvent: %d,%d\n", p.x(), p.y()); + Parent::mouseReleaseEvent(e); +} + +void ConfigList::mouseMoveEvent(QMouseEvent* e) +{ + //QPoint p(contentsToViewport(e->pos())); + //printf("contentsMouseMoveEvent: %d,%d\n", p.x(), p.y()); + Parent::mouseMoveEvent(e); +} + +void ConfigList::mouseDoubleClickEvent(QMouseEvent* e) +{ + QPoint p = e->pos(); // TODO: Check if this works(was contentsToViewport). + ConfigItem* item = (ConfigItem*)itemAt(p); + struct menu *menu; + enum prop_type ptype; + + if (!item) + goto skip; + if (item->goParent) { + emit parentSelected(); + goto skip; + } + menu = item->menu; + if (!menu) + goto skip; + ptype = menu->prompt ? menu->prompt->type : P_UNKNOWN; + if (ptype == P_MENU && (mode == singleMode || mode == symbolMode)) + emit menuSelected(menu); + else if (menu->sym) + changeValue(item); + +skip: + //printf("contentsMouseDoubleClickEvent: %d,%d\n", p.x(), p.y()); + Parent::mouseDoubleClickEvent(e); +} + +void ConfigList::focusInEvent(QFocusEvent *e) +{ + struct menu *menu = NULL; + + Parent::focusInEvent(e); + + ConfigItem* item = (ConfigItem *)currentItem(); + if (item) { + item->setSelected(true); + menu = item->menu; + } + emit gotFocus(menu); +} + +void ConfigList::contextMenuEvent(QContextMenuEvent *e) +{ + if (e->y() <= header()->geometry().bottom()) { + if (!headerPopup) { + QAction *action; + + headerPopup = new QMenu(this); + action = new QAction(_("Show Name"), this); + action->setCheckable(true); + connect(action, SIGNAL(toggled(bool)), + parent(), SLOT(setShowName(bool))); + connect(parent(), SIGNAL(showNameChanged(bool)), + action, SLOT(setOn(bool))); + action->setChecked(showName); + headerPopup->addAction(action); + action = new QAction(_("Show Range"), this); + action->setCheckable(true); + connect(action, SIGNAL(toggled(bool)), + parent(), SLOT(setShowRange(bool))); + connect(parent(), SIGNAL(showRangeChanged(bool)), + action, SLOT(setOn(bool))); + action->setChecked(showRange); + headerPopup->addAction(action); + action = new QAction(_("Show Data"), this); + action->setCheckable(true); + connect(action, SIGNAL(toggled(bool)), + parent(), SLOT(setShowData(bool))); + connect(parent(), SIGNAL(showDataChanged(bool)), + action, SLOT(setOn(bool))); + action->setChecked(showData); + headerPopup->addAction(action); + } + headerPopup->exec(e->globalPos()); + e->accept(); + } else + e->ignore(); +} + +ConfigView*ConfigView::viewList; +QAction *ConfigView::showNormalAction; +QAction *ConfigView::showAllAction; +QAction *ConfigView::showPromptAction; + +ConfigView::ConfigView(QWidget* parent, const char *name) + : Parent(parent) +{ + setObjectName(name); + QVBoxLayout *verticalLayout = new QVBoxLayout(this); + verticalLayout->setContentsMargins(0, 0, 0, 0); + + list = new ConfigList(this); + verticalLayout->addWidget(list); + lineEdit = new ConfigLineEdit(this); + lineEdit->hide(); + verticalLayout->addWidget(lineEdit); + + this->nextView = viewList; + viewList = this; +} + +ConfigView::~ConfigView(void) +{ + ConfigView** vp; + + for (vp = &viewList; *vp; vp = &(*vp)->nextView) { + if (*vp == this) { + *vp = nextView; + break; + } + } +} + +void ConfigView::setOptionMode(QAction *act) +{ + if (act == showNormalAction) + list->optMode = normalOpt; + else if (act == showAllAction) + list->optMode = allOpt; + else + list->optMode = promptOpt; + + list->updateListAll(); +} + +void ConfigView::setShowName(bool b) +{ + if (list->showName != b) { + list->showName = b; + list->reinit(); + emit showNameChanged(b); + } +} + +void ConfigView::setShowRange(bool b) +{ + if (list->showRange != b) { + list->showRange = b; + list->reinit(); + emit showRangeChanged(b); + } +} + +void ConfigView::setShowData(bool b) +{ + if (list->showData != b) { + list->showData = b; + list->reinit(); + emit showDataChanged(b); + } +} + +void ConfigList::setAllOpen(bool open) +{ + QTreeWidgetItemIterator it(this); + + while (*it) { + (*it)->setExpanded(open); + + ++it; + } +} + +void ConfigView::updateList(ConfigItem* item) +{ + ConfigView* v; + + for (v = viewList; v; v = v->nextView) + v->list->updateList(item); +} + +void ConfigView::updateListAll(void) +{ + ConfigView* v; + + for (v = viewList; v; v = v->nextView) + v->list->updateListAll(); +} + +ConfigInfoView::ConfigInfoView(QWidget* parent, const char *name) + : Parent(parent), sym(0), _menu(0) +{ + setObjectName(name); + + + if (!objectName().isEmpty()) { + configSettings->beginGroup(objectName()); + _showDebug = configSettings->value("/showDebug", false).toBool(); + configSettings->endGroup(); + connect(configApp, SIGNAL(aboutToQuit()), SLOT(saveSettings())); + } +} + +void ConfigInfoView::saveSettings(void) +{ + if (!objectName().isEmpty()) { + configSettings->beginGroup(objectName()); + configSettings->setValue("/showDebug", showDebug()); + configSettings->endGroup(); + } +} + +void ConfigInfoView::setShowDebug(bool b) +{ + if (_showDebug != b) { + _showDebug = b; + if (_menu) + menuInfo(); + else if (sym) + symbolInfo(); + emit showDebugChanged(b); + } +} + +void ConfigInfoView::setInfo(struct menu *m) +{ + if (_menu == m) + return; + _menu = m; + sym = NULL; + if (!_menu) + clear(); + else + menuInfo(); +} + +void ConfigInfoView::symbolInfo(void) +{ + QString str; + + str += "Symbol: "; + str += print_filter(sym->name); + str += "

value: "; + str += print_filter(sym_get_string_value(sym)); + str += "
visibility: "; + str += sym->visible == yes ? "y" : sym->visible == mod ? "m" : "n"; + str += "
"; + str += debug_info(sym); + + setText(str); +} + +void ConfigInfoView::menuInfo(void) +{ + struct symbol* sym; + QString head, debug, help; + + sym = _menu->sym; + if (sym) { + if (_menu->prompt) { + head += ""; + head += print_filter(_(_menu->prompt->text)); + head += ""; + if (sym->name) { + head += " ("; + if (showDebug()) + head += QString().sprintf("", sym); + head += print_filter(sym->name); + if (showDebug()) + head += ""; + head += ")"; + } + } else if (sym->name) { + head += ""; + if (showDebug()) + head += QString().sprintf("", sym); + head += print_filter(sym->name); + if (showDebug()) + head += ""; + head += ""; + } + head += "

"; + + if (showDebug()) + debug = debug_info(sym); + + struct gstr help_gstr = str_new(); + menu_get_ext_help(_menu, &help_gstr); + help = print_filter(str_get(&help_gstr)); + str_free(&help_gstr); + } else if (_menu->prompt) { + head += ""; + head += print_filter(_(_menu->prompt->text)); + head += "

"; + if (showDebug()) { + if (_menu->prompt->visible.expr) { + debug += "  dep: "; + expr_print(_menu->prompt->visible.expr, expr_print_help, &debug, E_NONE); + debug += "

"; + } + } + } + if (showDebug()) + debug += QString().sprintf("defined at %s:%d

", _menu->file->name, _menu->lineno); + + setText(head + debug + help); +} + +QString ConfigInfoView::debug_info(struct symbol *sym) +{ + QString debug; + + debug += "type: "; + debug += print_filter(sym_type_name(sym->type)); + if (sym_is_choice(sym)) + debug += " (choice)"; + debug += "
"; + if (sym->rev_dep.expr) { + debug += "reverse dep: "; + expr_print(sym->rev_dep.expr, expr_print_help, &debug, E_NONE); + debug += "
"; + } + for (struct property *prop = sym->prop; prop; prop = prop->next) { + switch (prop->type) { + case P_PROMPT: + case P_MENU: + debug += QString().sprintf("prompt: ", prop->menu); + debug += print_filter(_(prop->text)); + debug += "
"; + break; + case P_DEFAULT: + case P_SELECT: + case P_RANGE: + case P_ENV: + debug += prop_get_type_name(prop->type); + debug += ": "; + expr_print(prop->expr, expr_print_help, &debug, E_NONE); + debug += "
"; + break; + case P_CHOICE: + if (sym_is_choice(sym)) { + debug += "choice: "; + expr_print(prop->expr, expr_print_help, &debug, E_NONE); + debug += "
"; + } + break; + default: + debug += "unknown property: "; + debug += prop_get_type_name(prop->type); + debug += "
"; + } + if (prop->visible.expr) { + debug += "    dep: "; + expr_print(prop->visible.expr, expr_print_help, &debug, E_NONE); + debug += "
"; + } + } + debug += "
"; + + return debug; +} + +QString ConfigInfoView::print_filter(const QString &str) +{ + QRegExp re("[<>&\"\\n]"); + QString res = str; + for (int i = 0; (i = res.indexOf(re, i)) >= 0;) { + switch (res[i].toLatin1()) { + case '<': + res.replace(i, 1, "<"); + i += 4; + break; + case '>': + res.replace(i, 1, ">"); + i += 4; + break; + case '&': + res.replace(i, 1, "&"); + i += 5; + break; + case '"': + res.replace(i, 1, """); + i += 6; + break; + case '\n': + res.replace(i, 1, "
"); + i += 4; + break; + } + } + return res; +} + +void ConfigInfoView::expr_print_help(void *data, struct symbol *sym, const char *str) +{ + QString* text = reinterpret_cast(data); + QString str2 = print_filter(str); + + if (sym && sym->name && !(sym->flags & SYMBOL_CONST)) { + *text += QString().sprintf("", sym); + *text += str2; + *text += ""; + } else + *text += str2; +} + +QMenu* ConfigInfoView::createStandardContextMenu(const QPoint & pos) +{ + QMenu* popup = Parent::createStandardContextMenu(pos); + QAction* action = new QAction(_("Show Debug Info"), popup); + action->setCheckable(true); + connect(action, SIGNAL(toggled(bool)), SLOT(setShowDebug(bool))); + connect(this, SIGNAL(showDebugChanged(bool)), action, SLOT(setOn(bool))); + action->setChecked(showDebug()); + popup->addSeparator(); + popup->addAction(action); + return popup; +} + +void ConfigInfoView::contextMenuEvent(QContextMenuEvent *e) +{ + Parent::contextMenuEvent(e); +} + +ConfigSearchWindow::ConfigSearchWindow(ConfigMainWindow* parent, const char *name) + : Parent(parent), result(NULL) +{ + setObjectName(name); + setWindowTitle("Search Config"); + + QVBoxLayout* layout1 = new QVBoxLayout(this); + layout1->setContentsMargins(11, 11, 11, 11); + layout1->setSpacing(6); + QHBoxLayout* layout2 = new QHBoxLayout(0); + layout2->setContentsMargins(0, 0, 0, 0); + layout2->setSpacing(6); + layout2->addWidget(new QLabel(_("Find:"), this)); + editField = new QLineEdit(this); + connect(editField, SIGNAL(returnPressed()), SLOT(search())); + layout2->addWidget(editField); + searchButton = new QPushButton(_("Search"), this); + searchButton->setAutoDefault(false); + connect(searchButton, SIGNAL(clicked()), SLOT(search())); + layout2->addWidget(searchButton); + layout1->addLayout(layout2); + + split = new QSplitter(this); + split->setOrientation(Qt::Vertical); + list = new ConfigView(split, name); + list->list->mode = listMode; + info = new ConfigInfoView(split, name); + connect(list->list, SIGNAL(menuChanged(struct menu *)), + info, SLOT(setInfo(struct menu *))); + connect(list->list, SIGNAL(menuChanged(struct menu *)), + parent, SLOT(setMenuLink(struct menu *))); + + layout1->addWidget(split); + + if (name) { + QVariant x, y; + int width, height; + bool ok; + + configSettings->beginGroup(name); + width = configSettings->value("/window width", parent->width() / 2).toInt(); + height = configSettings->value("/window height", parent->height() / 2).toInt(); + resize(width, height); + x = configSettings->value("/window x"); + y = configSettings->value("/window y"); + if ((x.isValid())&&(y.isValid())) + move(x.toInt(), y.toInt()); + QList sizes = configSettings->readSizes("/split", &ok); + if (ok) + split->setSizes(sizes); + configSettings->endGroup(); + connect(configApp, SIGNAL(aboutToQuit()), SLOT(saveSettings())); + } +} + +void ConfigSearchWindow::saveSettings(void) +{ + if (!objectName().isEmpty()) { + configSettings->beginGroup(objectName()); + configSettings->setValue("/window x", pos().x()); + configSettings->setValue("/window y", pos().y()); + configSettings->setValue("/window width", size().width()); + configSettings->setValue("/window height", size().height()); + configSettings->writeSizes("/split", split->sizes()); + configSettings->endGroup(); + } +} + +void ConfigSearchWindow::search(void) +{ + struct symbol **p; + struct property *prop; + ConfigItem *lastItem = NULL; + + free(result); + list->list->clear(); + info->clear(); + + result = sym_re_search(editField->text().toLatin1()); + if (!result) + return; + for (p = result; *p; p++) { + for_all_prompts((*p), prop) + lastItem = new ConfigItem(list->list, lastItem, prop->menu, + menu_is_visible(prop->menu)); + } +} + +/* + * Construct the complete config widget + */ +ConfigMainWindow::ConfigMainWindow(void) + : searchWindow(0) +{ + QMenuBar* menu; + bool ok = true; + QVariant x, y; + int width, height; + char title[256]; + + QDesktopWidget *d = configApp->desktop(); + snprintf(title, sizeof(title), "%s%s", + rootmenu.prompt->text, + "" + ); + setWindowTitle(title); + + width = configSettings->value("/window width", d->width() - 64).toInt(); + height = configSettings->value("/window height", d->height() - 64).toInt(); + resize(width, height); + x = configSettings->value("/window x"); + y = configSettings->value("/window y"); + if ((x.isValid())&&(y.isValid())) + move(x.toInt(), y.toInt()); + + split1 = new QSplitter(this); + split1->setOrientation(Qt::Horizontal); + setCentralWidget(split1); + + menuView = new ConfigView(split1, "menu"); + menuList = menuView->list; + + split2 = new QSplitter(split1); + split2->setOrientation(Qt::Vertical); + + // create config tree + configView = new ConfigView(split2, "config"); + configList = configView->list; + + helpText = new ConfigInfoView(split2, "help"); + + setTabOrder(configList, helpText); + configList->setFocus(); + + menu = menuBar(); + toolBar = new QToolBar("Tools", this); + addToolBar(toolBar); + + backAction = new QAction(QPixmap(xpm_back), _("Back"), this); + connect(backAction, SIGNAL(triggered(bool)), SLOT(goBack())); + backAction->setEnabled(false); + QAction *quitAction = new QAction(_("&Quit"), this); + quitAction->setShortcut(Qt::CTRL + Qt::Key_Q); + connect(quitAction, SIGNAL(triggered(bool)), SLOT(close())); + QAction *loadAction = new QAction(QPixmap(xpm_load), _("&Load"), this); + loadAction->setShortcut(Qt::CTRL + Qt::Key_L); + connect(loadAction, SIGNAL(triggered(bool)), SLOT(loadConfig())); + saveAction = new QAction(QPixmap(xpm_save), _("&Save"), this); + saveAction->setShortcut(Qt::CTRL + Qt::Key_S); + connect(saveAction, SIGNAL(triggered(bool)), SLOT(saveConfig())); + conf_set_changed_callback(conf_changed); + // Set saveAction's initial state + conf_changed(); + QAction *saveAsAction = new QAction(_("Save &As..."), this); + connect(saveAsAction, SIGNAL(triggered(bool)), SLOT(saveConfigAs())); + QAction *searchAction = new QAction(_("&Find"), this); + searchAction->setShortcut(Qt::CTRL + Qt::Key_F); + connect(searchAction, SIGNAL(triggered(bool)), SLOT(searchConfig())); + singleViewAction = new QAction(QPixmap(xpm_single_view), _("Single View"), this); + singleViewAction->setCheckable(true); + connect(singleViewAction, SIGNAL(triggered(bool)), SLOT(showSingleView())); + splitViewAction = new QAction(QPixmap(xpm_split_view), _("Split View"), this); + splitViewAction->setCheckable(true); + connect(splitViewAction, SIGNAL(triggered(bool)), SLOT(showSplitView())); + fullViewAction = new QAction(QPixmap(xpm_tree_view), _("Full View"), this); + fullViewAction->setCheckable(true); + connect(fullViewAction, SIGNAL(triggered(bool)), SLOT(showFullView())); + + QAction *showNameAction = new QAction(_("Show Name"), this); + showNameAction->setCheckable(true); + connect(showNameAction, SIGNAL(toggled(bool)), configView, SLOT(setShowName(bool))); + showNameAction->setChecked(configView->showName()); + QAction *showRangeAction = new QAction(_("Show Range"), this); + showRangeAction->setCheckable(true); + connect(showRangeAction, SIGNAL(toggled(bool)), configView, SLOT(setShowRange(bool))); + QAction *showDataAction = new QAction(_("Show Data"), this); + showDataAction->setCheckable(true); + connect(showDataAction, SIGNAL(toggled(bool)), configView, SLOT(setShowData(bool))); + + QActionGroup *optGroup = new QActionGroup(this); + optGroup->setExclusive(true); + connect(optGroup, SIGNAL(triggered(QAction*)), configView, + SLOT(setOptionMode(QAction *))); + connect(optGroup, SIGNAL(triggered(QAction *)), menuView, + SLOT(setOptionMode(QAction *))); + + configView->showNormalAction = new QAction(_("Show Normal Options"), optGroup); + configView->showAllAction = new QAction(_("Show All Options"), optGroup); + configView->showPromptAction = new QAction(_("Show Prompt Options"), optGroup); + configView->showNormalAction->setCheckable(true); + configView->showAllAction->setCheckable(true); + configView->showPromptAction->setCheckable(true); + + QAction *showDebugAction = new QAction( _("Show Debug Info"), this); + showDebugAction->setCheckable(true); + connect(showDebugAction, SIGNAL(toggled(bool)), helpText, SLOT(setShowDebug(bool))); + showDebugAction->setChecked(helpText->showDebug()); + + QAction *showIntroAction = new QAction( _("Introduction"), this); + connect(showIntroAction, SIGNAL(triggered(bool)), SLOT(showIntro())); + QAction *showAboutAction = new QAction( _("About"), this); + connect(showAboutAction, SIGNAL(triggered(bool)), SLOT(showAbout())); + + // init tool bar + toolBar->addAction(backAction); + toolBar->addSeparator(); + toolBar->addAction(loadAction); + toolBar->addAction(saveAction); + toolBar->addSeparator(); + toolBar->addAction(singleViewAction); + toolBar->addAction(splitViewAction); + toolBar->addAction(fullViewAction); + + // create config menu + QMenu* config = menu->addMenu(_("&File")); + config->addAction(loadAction); + config->addAction(saveAction); + config->addAction(saveAsAction); + config->addSeparator(); + config->addAction(quitAction); + + // create edit menu + QMenu* editMenu = menu->addMenu(_("&Edit")); + editMenu->addAction(searchAction); + + // create options menu + QMenu* optionMenu = menu->addMenu(_("&Option")); + optionMenu->addAction(showNameAction); + optionMenu->addAction(showRangeAction); + optionMenu->addAction(showDataAction); + optionMenu->addSeparator(); + optionMenu->addActions(optGroup->actions()); + optionMenu->addSeparator(); + + // create help menu + menu->addSeparator(); + QMenu* helpMenu = menu->addMenu(_("&Help")); + helpMenu->addAction(showIntroAction); + helpMenu->addAction(showAboutAction); + + connect(configList, SIGNAL(menuChanged(struct menu *)), + helpText, SLOT(setInfo(struct menu *))); + connect(configList, SIGNAL(menuSelected(struct menu *)), + SLOT(changeMenu(struct menu *))); + connect(configList, SIGNAL(parentSelected()), + SLOT(goBack())); + connect(menuList, SIGNAL(menuChanged(struct menu *)), + helpText, SLOT(setInfo(struct menu *))); + connect(menuList, SIGNAL(menuSelected(struct menu *)), + SLOT(changeMenu(struct menu *))); + + connect(configList, SIGNAL(gotFocus(struct menu *)), + helpText, SLOT(setInfo(struct menu *))); + connect(menuList, SIGNAL(gotFocus(struct menu *)), + helpText, SLOT(setInfo(struct menu *))); + connect(menuList, SIGNAL(gotFocus(struct menu *)), + SLOT(listFocusChanged(void))); + connect(helpText, SIGNAL(menuSelected(struct menu *)), + SLOT(setMenuLink(struct menu *))); + + QString listMode = configSettings->value("/listMode", "symbol").toString(); + if (listMode == "single") + showSingleView(); + else if (listMode == "full") + showFullView(); + else /*if (listMode == "split")*/ + showSplitView(); + + // UI setup done, restore splitter positions + QList sizes = configSettings->readSizes("/split1", &ok); + if (ok) + split1->setSizes(sizes); + + sizes = configSettings->readSizes("/split2", &ok); + if (ok) + split2->setSizes(sizes); +} + +void ConfigMainWindow::loadConfig(void) +{ + QString s = QFileDialog::getOpenFileName(this, "", conf_get_configname()); + if (s.isNull()) + return; + if (conf_read(QFile::encodeName(s))) + QMessageBox::information(this, "qconf", _("Unable to load configuration!")); + ConfigView::updateListAll(); +} + +bool ConfigMainWindow::saveConfig(void) +{ + if (conf_write(NULL)) { + QMessageBox::information(this, "qconf", _("Unable to save configuration!")); + return false; + } + return true; +} + +void ConfigMainWindow::saveConfigAs(void) +{ + QString s = QFileDialog::getSaveFileName(this, "", conf_get_configname()); + if (s.isNull()) + return; + saveConfig(); +} + +void ConfigMainWindow::searchConfig(void) +{ + if (!searchWindow) + searchWindow = new ConfigSearchWindow(this, "search"); + searchWindow->show(); +} + +void ConfigMainWindow::changeMenu(struct menu *menu) +{ + configList->setRootMenu(menu); + if (configList->rootEntry->parent == &rootmenu) + backAction->setEnabled(false); + else + backAction->setEnabled(true); +} + +void ConfigMainWindow::setMenuLink(struct menu *menu) +{ + struct menu *parent; + ConfigList* list = NULL; + ConfigItem* item; + + if (configList->menuSkip(menu)) + return; + + switch (configList->mode) { + case singleMode: + list = configList; + parent = menu_get_parent_menu(menu); + if (!parent) + return; + list->setRootMenu(parent); + break; + case symbolMode: + if (menu->flags & MENU_ROOT) { + configList->setRootMenu(menu); + configList->clearSelection(); + list = menuList; + } else { + list = configList; + parent = menu_get_parent_menu(menu->parent); + if (!parent) + return; + item = menuList->findConfigItem(parent); + if (item) { + item->setSelected(true); + menuList->scrollToItem(item); + } + list->setRootMenu(parent); + } + break; + case fullMode: + list = configList; + break; + default: + break; + } + + if (list) { + item = list->findConfigItem(menu); + if (item) { + item->setSelected(true); + list->scrollToItem(item); + list->setFocus(); + } + } +} + +void ConfigMainWindow::listFocusChanged(void) +{ + if (menuList->mode == menuMode) + configList->clearSelection(); +} + +void ConfigMainWindow::goBack(void) +{ + ConfigItem* item, *oldSelection; + + configList->setParentMenu(); + if (configList->rootEntry == &rootmenu) + backAction->setEnabled(false); + + if (menuList->selectedItems().count() == 0) + return; + + item = (ConfigItem*)menuList->selectedItems().first(); + oldSelection = item; + while (item) { + if (item->menu == configList->rootEntry) { + oldSelection->setSelected(false); + item->setSelected(true); + break; + } + item = (ConfigItem*)item->parent(); + } +} + +void ConfigMainWindow::showSingleView(void) +{ + singleViewAction->setEnabled(false); + singleViewAction->setChecked(true); + splitViewAction->setEnabled(true); + splitViewAction->setChecked(false); + fullViewAction->setEnabled(true); + fullViewAction->setChecked(false); + + menuView->hide(); + menuList->setRootMenu(0); + configList->mode = singleMode; + if (configList->rootEntry == &rootmenu) + configList->updateListAll(); + else + configList->setRootMenu(&rootmenu); + configList->setFocus(); +} + +void ConfigMainWindow::showSplitView(void) +{ + singleViewAction->setEnabled(true); + singleViewAction->setChecked(false); + splitViewAction->setEnabled(false); + splitViewAction->setChecked(true); + fullViewAction->setEnabled(true); + fullViewAction->setChecked(false); + + configList->mode = symbolMode; + if (configList->rootEntry == &rootmenu) + configList->updateListAll(); + else + configList->setRootMenu(&rootmenu); + configList->setAllOpen(true); + configApp->processEvents(); + menuList->mode = menuMode; + menuList->setRootMenu(&rootmenu); + menuList->setAllOpen(true); + menuView->show(); + menuList->setFocus(); +} + +void ConfigMainWindow::showFullView(void) +{ + singleViewAction->setEnabled(true); + singleViewAction->setChecked(false); + splitViewAction->setEnabled(true); + splitViewAction->setChecked(false); + fullViewAction->setEnabled(false); + fullViewAction->setChecked(true); + + menuView->hide(); + menuList->setRootMenu(0); + configList->mode = fullMode; + if (configList->rootEntry == &rootmenu) + configList->updateListAll(); + else + configList->setRootMenu(&rootmenu); + configList->setFocus(); +} + +/* + * ask for saving configuration before quitting + * TODO ask only when something changed + */ +void ConfigMainWindow::closeEvent(QCloseEvent* e) +{ + if (!conf_get_changed()) { + e->accept(); + return; + } + QMessageBox mb("qconf", _("Save configuration?"), QMessageBox::Warning, + QMessageBox::Yes | QMessageBox::Default, QMessageBox::No, QMessageBox::Cancel | QMessageBox::Escape); + mb.setButtonText(QMessageBox::Yes, _("&Save Changes")); + mb.setButtonText(QMessageBox::No, _("&Discard Changes")); + mb.setButtonText(QMessageBox::Cancel, _("Cancel Exit")); + switch (mb.exec()) { + case QMessageBox::Yes: + if (saveConfig()) + e->accept(); + else + e->ignore(); + break; + case QMessageBox::No: + e->accept(); + break; + case QMessageBox::Cancel: + e->ignore(); + break; + } +} + +void ConfigMainWindow::showIntro(void) +{ + static const QString str = _("Welcome to the qconf graphical configuration tool.\n\n" + "For each option, a blank box indicates the feature is disabled, a check\n" + "indicates it is enabled, and a dot indicates that it is to be compiled\n" + "as a module. Clicking on the box will cycle through the three states.\n\n" + "If you do not see an option (e.g., a device driver) that you believe\n" + "should be present, try turning on Show All Options under the Options menu.\n" + "Although there is no cross reference yet to help you figure out what other\n" + "options must be enabled to support the option you are interested in, you can\n" + "still view the help of a grayed-out option.\n\n" + "Toggling Show Debug Info under the Options menu will show the dependencies,\n" + "which you can then match by examining other options.\n\n"); + + QMessageBox::information(this, "qconf", str); +} + +void ConfigMainWindow::showAbout(void) +{ + static const QString str = _("qconf is Copyright (C) 2002 Roman Zippel .\n" + "Copyright (C) 2015 Boris Barbulovski .\n\n" + "Bug reports and feature request can also be entered at http://bugzilla.kernel.org/\n"); + + QMessageBox::information(this, "qconf", str); +} + +void ConfigMainWindow::saveSettings(void) +{ + configSettings->setValue("/window x", pos().x()); + configSettings->setValue("/window y", pos().y()); + configSettings->setValue("/window width", size().width()); + configSettings->setValue("/window height", size().height()); + + QString entry; + switch(configList->mode) { + case singleMode : + entry = "single"; + break; + + case symbolMode : + entry = "split"; + break; + + case fullMode : + entry = "full"; + break; + + default: + break; + } + configSettings->setValue("/listMode", entry); + + configSettings->writeSizes("/split1", split1->sizes()); + configSettings->writeSizes("/split2", split2->sizes()); +} + +void ConfigMainWindow::conf_changed(void) +{ + if (saveAction) + saveAction->setEnabled(conf_get_changed()); +} + +void fixup_rootmenu(struct menu *menu) +{ + struct menu *child; + static int menu_cnt = 0; + + menu->flags |= MENU_ROOT; + for (child = menu->list; child; child = child->next) { + if (child->prompt && child->prompt->type == P_MENU) { + menu_cnt++; + fixup_rootmenu(child); + menu_cnt--; + } else if (!menu_cnt) + fixup_rootmenu(child); + } +} + +static const char *progname; + +static void usage(void) +{ + printf(_("%s [-s] \n").toLatin1().constData(), progname); + exit(0); +} + +int main(int ac, char** av) +{ + ConfigMainWindow* v; + const char *name; + + bindtextdomain(PACKAGE, LOCALEDIR); + textdomain(PACKAGE); + + progname = av[0]; + configApp = new QApplication(ac, av); + if (ac > 1 && av[1][0] == '-') { + switch (av[1][1]) { + case 's': + conf_set_message_callback(NULL); + break; + case 'h': + case '?': + usage(); + } + name = av[2]; + } else + name = av[1]; + if (!name) + usage(); + + conf_parse(name); + fixup_rootmenu(&rootmenu); + conf_read(NULL); + //zconfdump(stdout); + + configSettings = new ConfigSettings(); + configSettings->beginGroup("/kconfig/qconf"); + v = new ConfigMainWindow(); + + //zconfdump(stdout); + configApp->connect(configApp, SIGNAL(lastWindowClosed()), SLOT(quit())); + configApp->connect(configApp, SIGNAL(aboutToQuit()), v, SLOT(saveSettings())); + v->show(); + configApp->exec(); + + configSettings->endGroup(); + delete configSettings; + delete v; + delete configApp; + + return 0; +} diff --git a/scripts/kconfig/qconf.h b/scripts/kconfig/qconf.h new file mode 100644 index 0000000000..a40036d1b0 --- /dev/null +++ b/scripts/kconfig/qconf.h @@ -0,0 +1,330 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "expr.h" + +class ConfigView; +class ConfigList; +class ConfigItem; +class ConfigLineEdit; +class ConfigMainWindow; + +class ConfigSettings : public QSettings { +public: + ConfigSettings(); + QList readSizes(const QString& key, bool *ok); + bool writeSizes(const QString& key, const QList& value); +}; + +enum colIdx { + promptColIdx, nameColIdx, noColIdx, modColIdx, yesColIdx, dataColIdx, colNr +}; +enum listMode { + singleMode, menuMode, symbolMode, fullMode, listMode +}; +enum optionMode { + normalOpt = 0, allOpt, promptOpt +}; + +class ConfigList : public QTreeWidget { + Q_OBJECT + typedef class QTreeWidget Parent; +public: + ConfigList(ConfigView* p, const char *name = 0); + void reinit(void); + ConfigView* parent(void) const + { + return (ConfigView*)Parent::parent(); + } + ConfigItem* findConfigItem(struct menu *); + +protected: + void keyPressEvent(QKeyEvent *e); + void mousePressEvent(QMouseEvent *e); + void mouseReleaseEvent(QMouseEvent *e); + void mouseMoveEvent(QMouseEvent *e); + void mouseDoubleClickEvent(QMouseEvent *e); + void focusInEvent(QFocusEvent *e); + void contextMenuEvent(QContextMenuEvent *e); + +public slots: + void setRootMenu(struct menu *menu); + + void updateList(ConfigItem *item); + void setValue(ConfigItem* item, tristate val); + void changeValue(ConfigItem* item); + void updateSelection(void); + void saveSettings(void); +signals: + void menuChanged(struct menu *menu); + void menuSelected(struct menu *menu); + void parentSelected(void); + void gotFocus(struct menu *); + +public: + void updateListAll(void) + { + updateAll = true; + updateList(NULL); + updateAll = false; + } + ConfigList* listView() + { + return this; + } + ConfigItem* firstChild() const + { + return (ConfigItem *)children().first(); + } + void addColumn(colIdx idx) + { + showColumn(idx); + } + void removeColumn(colIdx idx) + { + hideColumn(idx); + } + void setAllOpen(bool open); + void setParentMenu(void); + + bool menuSkip(struct menu *); + + void updateMenuList(ConfigItem *parent, struct menu*); + void updateMenuList(ConfigList *parent, struct menu*); + + bool updateAll; + + QPixmap symbolYesPix, symbolModPix, symbolNoPix; + QPixmap choiceYesPix, choiceNoPix; + QPixmap menuPix, menuInvPix, menuBackPix, voidPix; + + bool showName, showRange, showData; + enum listMode mode; + enum optionMode optMode; + struct menu *rootEntry; + QPalette disabledColorGroup; + QPalette inactivedColorGroup; + QMenu* headerPopup; +}; + +class ConfigItem : public QTreeWidgetItem { + typedef class QTreeWidgetItem Parent; +public: + ConfigItem(ConfigList *parent, ConfigItem *after, struct menu *m, bool v) + : Parent(parent, after), nextItem(0), menu(m), visible(v), goParent(false) + { + init(); + } + ConfigItem(ConfigItem *parent, ConfigItem *after, struct menu *m, bool v) + : Parent(parent, after), nextItem(0), menu(m), visible(v), goParent(false) + { + init(); + } + ConfigItem(ConfigList *parent, ConfigItem *after, bool v) + : Parent(parent, after), nextItem(0), menu(0), visible(v), goParent(true) + { + init(); + } + ~ConfigItem(void); + void init(void); + void okRename(int col); + void updateMenu(void); + void testUpdateMenu(bool v); + ConfigList* listView() const + { + return (ConfigList*)Parent::treeWidget(); + } + ConfigItem* firstChild() const + { + return (ConfigItem *)Parent::child(0); + } + ConfigItem* nextSibling() + { + ConfigItem *ret = NULL; + ConfigItem *_parent = (ConfigItem *)parent(); + + if(_parent) { + ret = (ConfigItem *)_parent->child(_parent->indexOfChild(this)+1); + } else { + QTreeWidget *_treeWidget = treeWidget(); + ret = (ConfigItem *)_treeWidget->topLevelItem(_treeWidget->indexOfTopLevelItem(this)+1); + } + + return ret; + } + void setText(colIdx idx, const QString& text) + { + Parent::setText(idx, text); + } + QString text(colIdx idx) const + { + return Parent::text(idx); + } + void setPixmap(colIdx idx, const QIcon &icon) + { + Parent::setIcon(idx, icon); + } + const QIcon pixmap(colIdx idx) const + { + return icon(idx); + } + // TODO: Implement paintCell + + ConfigItem* nextItem; + struct menu *menu; + bool visible; + bool goParent; +}; + +class ConfigLineEdit : public QLineEdit { + Q_OBJECT + typedef class QLineEdit Parent; +public: + ConfigLineEdit(ConfigView* parent); + ConfigView* parent(void) const + { + return (ConfigView*)Parent::parent(); + } + void show(ConfigItem *i); + void keyPressEvent(QKeyEvent *e); + +public: + ConfigItem *item; +}; + +class ConfigView : public QWidget { + Q_OBJECT + typedef class QWidget Parent; +public: + ConfigView(QWidget* parent, const char *name = 0); + ~ConfigView(void); + static void updateList(ConfigItem* item); + static void updateListAll(void); + + bool showName(void) const { return list->showName; } + bool showRange(void) const { return list->showRange; } + bool showData(void) const { return list->showData; } +public slots: + void setShowName(bool); + void setShowRange(bool); + void setShowData(bool); + void setOptionMode(QAction *); +signals: + void showNameChanged(bool); + void showRangeChanged(bool); + void showDataChanged(bool); +public: + ConfigList* list; + ConfigLineEdit* lineEdit; + + static ConfigView* viewList; + ConfigView* nextView; + + static QAction *showNormalAction; + static QAction *showAllAction; + static QAction *showPromptAction; +}; + +class ConfigInfoView : public QTextBrowser { + Q_OBJECT + typedef class QTextBrowser Parent; +public: + ConfigInfoView(QWidget* parent, const char *name = 0); + bool showDebug(void) const { return _showDebug; } + +public slots: + void setInfo(struct menu *menu); + void saveSettings(void); + void setShowDebug(bool); + +signals: + void showDebugChanged(bool); + void menuSelected(struct menu *); + +protected: + void symbolInfo(void); + void menuInfo(void); + QString debug_info(struct symbol *sym); + static QString print_filter(const QString &str); + static void expr_print_help(void *data, struct symbol *sym, const char *str); + QMenu *createStandardContextMenu(const QPoint & pos); + void contextMenuEvent(QContextMenuEvent *e); + + struct symbol *sym; + struct menu *_menu; + bool _showDebug; +}; + +class ConfigSearchWindow : public QDialog { + Q_OBJECT + typedef class QDialog Parent; +public: + ConfigSearchWindow(ConfigMainWindow* parent, const char *name = 0); + +public slots: + void saveSettings(void); + void search(void); + +protected: + QLineEdit* editField; + QPushButton* searchButton; + QSplitter* split; + ConfigView* list; + ConfigInfoView* info; + + struct symbol **result; +}; + +class ConfigMainWindow : public QMainWindow { + Q_OBJECT + + static QAction *saveAction; + static void conf_changed(void); +public: + ConfigMainWindow(void); +public slots: + void changeMenu(struct menu *); + void setMenuLink(struct menu *); + void listFocusChanged(void); + void goBack(void); + void loadConfig(void); + bool saveConfig(void); + void saveConfigAs(void); + void searchConfig(void); + void showSingleView(void); + void showSplitView(void); + void showFullView(void); + void showIntro(void); + void showAbout(void); + void saveSettings(void); + +protected: + void closeEvent(QCloseEvent *e); + + ConfigSearchWindow *searchWindow; + ConfigView *menuView; + ConfigList *menuList; + ConfigView *configView; + ConfigList *configList; + ConfigInfoView *helpText; + QToolBar *toolBar; + QAction *backAction; + QAction *singleViewAction; + QAction *splitViewAction; + QAction *fullViewAction; + QSplitter *split1; + QSplitter *split2; +}; diff --git a/scripts/kconfig/streamline_config.pl b/scripts/kconfig/streamline_config.pl new file mode 100755 index 0000000000..b8c7b29aff --- /dev/null +++ b/scripts/kconfig/streamline_config.pl @@ -0,0 +1,681 @@ +#!/usr/bin/perl -w +# +# Copyright 2005-2009 - Steven Rostedt +# Licensed under the terms of the GNU GPL License version 2 +# +# It's simple enough to figure out how this works. +# If not, then you can ask me at stripconfig@goodmis.org +# +# What it does? +# +# If you have installed a Linux kernel from a distribution +# that turns on way too many modules than you need, and +# you only want the modules you use, then this program +# is perfect for you. +# +# It gives you the ability to turn off all the modules that are +# not loaded on your system. +# +# Howto: +# +# 1. Boot up the kernel that you want to stream line the config on. +# 2. Change directory to the directory holding the source of the +# kernel that you just booted. +# 3. Copy the configuraton file to this directory as .config +# 4. Have all your devices that you need modules for connected and +# operational (make sure that their corresponding modules are loaded) +# 5. Run this script redirecting the output to some other file +# like config_strip. +# 6. Back up your old config (if you want too). +# 7. copy the config_strip file to .config +# 8. Run "make oldconfig" +# +# Now your kernel is ready to be built with only the modules that +# are loaded. +# +# Here's what I did with my Debian distribution. +# +# cd /usr/src/linux-2.6.10 +# cp /boot/config-2.6.10-1-686-smp .config +# ~/bin/streamline_config > config_strip +# mv .config config_sav +# mv config_strip .config +# make oldconfig +# +use strict; +use Getopt::Long; + +# set the environment variable LOCALMODCONFIG_DEBUG to get +# debug output. +my $debugprint = 0; +$debugprint = 1 if (defined($ENV{LOCALMODCONFIG_DEBUG})); + +sub dprint { + return if (!$debugprint); + print STDERR @_; +} + +my $config = ".config"; + +my $uname = `uname -r`; +chomp $uname; + +my @searchconfigs = ( + { + "file" => ".config", + "exec" => "cat", + }, + { + "file" => "/proc/config.gz", + "exec" => "zcat", + }, + { + "file" => "/boot/config-$uname", + "exec" => "cat", + }, + { + "file" => "/boot/vmlinuz-$uname", + "exec" => "scripts/extract-ikconfig", + "test" => "scripts/extract-ikconfig", + }, + { + "file" => "vmlinux", + "exec" => "scripts/extract-ikconfig", + "test" => "scripts/extract-ikconfig", + }, + { + "file" => "/lib/modules/$uname/kernel/kernel/configs.ko", + "exec" => "scripts/extract-ikconfig", + "test" => "scripts/extract-ikconfig", + }, + { + "file" => "kernel/configs.ko", + "exec" => "scripts/extract-ikconfig", + "test" => "scripts/extract-ikconfig", + }, + { + "file" => "kernel/configs.o", + "exec" => "scripts/extract-ikconfig", + "test" => "scripts/extract-ikconfig", + }, +); + +sub read_config { + foreach my $conf (@searchconfigs) { + my $file = $conf->{"file"}; + + next if ( ! -f "$file"); + + if (defined($conf->{"test"})) { + `$conf->{"test"} $conf->{"file"} 2>/dev/null`; + next if ($?); + } + + my $exec = $conf->{"exec"}; + + print STDERR "using config: '$file'\n"; + + open(my $infile, '-|', "$exec $file") || die "Failed to run $exec $file"; + my @x = <$infile>; + close $infile; + return @x; + } + die "No config file found"; +} + +my @config_file = read_config; + +# Parse options +my $localmodconfig = 0; +my $localyesconfig = 0; + +GetOptions("localmodconfig" => \$localmodconfig, + "localyesconfig" => \$localyesconfig); + +# Get the build source and top level Kconfig file (passed in) +my $ksource = ($ARGV[0] ? $ARGV[0] : '.'); +my $kconfig = $ARGV[1]; +my $lsmod_file = $ENV{'LSMOD'}; + +my @makefiles = `find $ksource -name Makefile -or -name Kbuild 2>/dev/null`; +chomp @makefiles; + +my %depends; +my %selects; +my %prompts; +my %objects; +my $var; +my $iflevel = 0; +my @ifdeps; + +# prevent recursion +my %read_kconfigs; + +sub read_kconfig { + my ($kconfig) = @_; + + my $state = "NONE"; + my $config; + + my $cont = 0; + my $line; + + my $source = "$ksource/$kconfig"; + my $last_source = ""; + + # Check for any environment variables used + while ($source =~ /\$(\w+)/ && $last_source ne $source) { + my $env = $1; + $last_source = $source; + $source =~ s/\$$env/$ENV{$env}/; + } + + open(my $kinfile, '<', $source) || die "Can't open $kconfig"; + while (<$kinfile>) { + chomp; + + # Make sure that lines ending with \ continue + if ($cont) { + $_ = $line . " " . $_; + } + + if (s/\\$//) { + $cont = 1; + $line = $_; + next; + } + + $cont = 0; + + # collect any Kconfig sources + if (/^source\s+"?([^"]+)/) { + my $kconfig = $1; + # prevent reading twice. + if (!defined($read_kconfigs{$kconfig})) { + $read_kconfigs{$kconfig} = 1; + read_kconfig($kconfig); + } + next; + } + + # configs found + if (/^\s*(menu)?config\s+(\S+)\s*$/) { + $state = "NEW"; + $config = $2; + + # Add depends for 'if' nesting + for (my $i = 0; $i < $iflevel; $i++) { + if ($i) { + $depends{$config} .= " " . $ifdeps[$i]; + } else { + $depends{$config} = $ifdeps[$i]; + } + $state = "DEP"; + } + + # collect the depends for the config + } elsif ($state eq "NEW" && /^\s*depends\s+on\s+(.*)$/) { + $state = "DEP"; + $depends{$config} = $1; + } elsif ($state eq "DEP" && /^\s*depends\s+on\s+(.*)$/) { + $depends{$config} .= " " . $1; + } elsif ($state eq "DEP" && /^\s*def(_(bool|tristate)|ault)\s+(\S.*)$/) { + my $dep = $3; + if ($dep !~ /^\s*(y|m|n)\s*$/) { + $dep =~ s/.*\sif\s+//; + $depends{$config} .= " " . $dep; + dprint "Added default depends $dep to $config\n"; + } + + # Get the configs that select this config + } elsif ($state ne "NONE" && /^\s*select\s+(\S+)/) { + my $conf = $1; + if (defined($selects{$conf})) { + $selects{$conf} .= " " . $config; + } else { + $selects{$conf} = $config; + } + + # configs without prompts must be selected + } elsif ($state ne "NONE" && /^\s*(tristate\s+\S|prompt\b)/) { + # note if the config has a prompt + $prompts{$config} = 1; + + # Check for if statements + } elsif (/^if\s+(.*\S)\s*$/) { + my $deps = $1; + # remove beginning and ending non text + $deps =~ s/^[^a-zA-Z0-9_]*//; + $deps =~ s/[^a-zA-Z0-9_]*$//; + + my @deps = split /[^a-zA-Z0-9_]+/, $deps; + + $ifdeps[$iflevel++] = join ':', @deps; + + } elsif (/^endif/) { + + $iflevel-- if ($iflevel); + + # stop on "help" and keywords that end a menu entry + } elsif (/^\s*(---)?help(---)?\s*$/ || /^(comment|choice|menu)\b/) { + $state = "NONE"; + } + } + close($kinfile); +} + +if ($kconfig) { + read_kconfig($kconfig); +} + +# Makefiles can use variables to define their dependencies +sub convert_vars { + my ($line, %vars) = @_; + + my $process = ""; + + while ($line =~ s/^(.*?)(\$\((.*?)\))//) { + my $start = $1; + my $variable = $2; + my $var = $3; + + if (defined($vars{$var})) { + $process .= $start . $vars{$var}; + } else { + $process .= $start . $variable; + } + } + + $process .= $line; + + return $process; +} + +# Read all Makefiles to map the configs to the objects +foreach my $makefile (@makefiles) { + + my $line = ""; + my %make_vars; + + open(my $infile, '<', $makefile) || die "Can't open $makefile"; + while (<$infile>) { + # if this line ends with a backslash, continue + chomp; + if (/^(.*)\\$/) { + $line .= $1; + next; + } + + $line .= $_; + $_ = $line; + $line = ""; + + my $objs; + + # Convert variables in a line (could define configs) + $_ = convert_vars($_, %make_vars); + + # collect objects after obj-$(CONFIG_FOO_BAR) + if (/obj-\$\((CONFIG_[^\)]*)\)\s*[+:]?=\s*(.*)/) { + $var = $1; + $objs = $2; + + # check if variables are set + } elsif (/^\s*(\S+)\s*[:]?=\s*(.*\S)/) { + $make_vars{$1} = $2; + } + if (defined($objs)) { + foreach my $obj (split /\s+/,$objs) { + $obj =~ s/-/_/g; + if ($obj =~ /(.*)\.o$/) { + # Objects may be enabled by more than one config. + # Store configs in an array. + my @arr; + + if (defined($objects{$1})) { + @arr = @{$objects{$1}}; + } + + $arr[$#arr+1] = $var; + + # The objects have a hash mapping to a reference + # of an array of configs. + $objects{$1} = \@arr; + } + } + } + } + close($infile); +} + +my %modules; +my $linfile; + +if (defined($lsmod_file)) { + if ( ! -f $lsmod_file) { + if ( -f $ENV{'objtree'}."/".$lsmod_file) { + $lsmod_file = $ENV{'objtree'}."/".$lsmod_file; + } else { + die "$lsmod_file not found"; + } + } + + my $otype = ( -x $lsmod_file) ? '-|' : '<'; + open($linfile, $otype, $lsmod_file); + +} else { + + # see what modules are loaded on this system + my $lsmod; + + foreach my $dir ( ("/sbin", "/bin", "/usr/sbin", "/usr/bin") ) { + if ( -x "$dir/lsmod" ) { + $lsmod = "$dir/lsmod"; + last; + } +} + if (!defined($lsmod)) { + # try just the path + $lsmod = "lsmod"; + } + + open($linfile, '-|', $lsmod) || die "Can not call lsmod with $lsmod"; +} + +while (<$linfile>) { + next if (/^Module/); # Skip the first line. + if (/^(\S+)/) { + $modules{$1} = 1; + } +} +close ($linfile); + +# add to the configs hash all configs that are needed to enable +# a loaded module. This is a direct obj-${CONFIG_FOO} += bar.o +# where we know we need bar.o so we add FOO to the list. +my %configs; +foreach my $module (keys(%modules)) { + if (defined($objects{$module})) { + my @arr = @{$objects{$module}}; + foreach my $conf (@arr) { + $configs{$conf} = $module; + dprint "$conf added by direct ($module)\n"; + if ($debugprint) { + my $c=$conf; + $c =~ s/^CONFIG_//; + if (defined($depends{$c})) { + dprint " deps = $depends{$c}\n"; + } else { + dprint " no deps\n"; + } + } + } + } else { + # Most likely, someone has a custom (binary?) module loaded. + print STDERR "$module config not found!!\n"; + } +} + +# Read the current config, and see what is enabled. We want to +# ignore configs that we would not enable anyway. + +my %orig_configs; +my $valid = "A-Za-z_0-9"; + +foreach my $line (@config_file) { + $_ = $line; + + if (/(CONFIG_[$valid]*)=(m|y)/) { + $orig_configs{$1} = $2; + } +} + +my $repeat = 1; + +my $depconfig; + +# +# Note, we do not care about operands (like: &&, ||, !) we want to add any +# config that is in the depend list of another config. This script does +# not enable configs that are not already enabled. If we come across a +# config A that depends on !B, we can still add B to the list of depends +# to keep on. If A was on in the original config, B would not have been +# and B would not be turned on by this script. +# +sub parse_config_depends +{ + my ($p) = @_; + + while ($p =~ /[$valid]/) { + + if ($p =~ /^[^$valid]*([$valid]+)/) { + my $conf = "CONFIG_" . $1; + + $p =~ s/^[^$valid]*[$valid]+//; + + # We only need to process if the depend config is a module + if (!defined($orig_configs{$conf}) || $orig_configs{$conf} eq "y") { + next; + } + + if (!defined($configs{$conf})) { + # We must make sure that this config has its + # dependencies met. + $repeat = 1; # do again + dprint "$conf selected by depend $depconfig\n"; + $configs{$conf} = 1; + } + } else { + die "this should never happen"; + } + } +} + +# Select is treated a bit differently than depends. We call this +# when a config has no prompt and requires another config to be +# selected. We use to just select all configs that selected this +# config, but found that that can balloon into enabling hundreds +# of configs that we do not care about. +# +# The idea is we look at all the configs that select it. If one +# is already in our list of configs to enable, then there's nothing +# else to do. If there isn't, we pick the first config that was +# enabled in the orignal config and use that. +sub parse_config_selects +{ + my ($config, $p) = @_; + + my $next_config; + + while ($p =~ /[$valid]/) { + + if ($p =~ /^[^$valid]*([$valid]+)/) { + my $conf = "CONFIG_" . $1; + + $p =~ s/^[^$valid]*[$valid]+//; + + # Make sure that this config exists in the current .config file + if (!defined($orig_configs{$conf})) { + dprint "$conf not set for $config select\n"; + next; + } + + # Check if something other than a module selects this config + if (defined($orig_configs{$conf}) && $orig_configs{$conf} ne "m") { + dprint "$conf (non module) selects config, we are good\n"; + # we are good with this + return; + } + if (defined($configs{$conf})) { + dprint "$conf selects $config so we are good\n"; + # A set config selects this config, we are good + return; + } + # Set this config to be selected + if (!defined($next_config)) { + $next_config = $conf; + } + } else { + die "this should never happen"; + } + } + + # If no possible config selected this, then something happened. + if (!defined($next_config)) { + print STDERR "WARNING: $config is required, but nothing in the\n"; + print STDERR " current config selects it.\n"; + return; + } + + # If we are here, then we found no config that is set and + # selects this config. Repeat. + $repeat = 1; + # Make this config need to be selected + $configs{$next_config} = 1; + dprint "$next_config selected by select $config\n"; +} + +my %process_selects; + +# loop through all configs, select their dependencies. +sub loop_depend { + $repeat = 1; + + while ($repeat) { + $repeat = 0; + + forloop: + foreach my $config (keys %configs) { + + # If this config is not a module, we do not need to process it + if (defined($orig_configs{$config}) && $orig_configs{$config} ne "m") { + next forloop; + } + + $config =~ s/^CONFIG_//; + $depconfig = $config; + + if (defined($depends{$config})) { + # This config has dependencies. Make sure they are also included + parse_config_depends $depends{$config}; + } + + # If the config has no prompt, then we need to check if a config + # that is enabled selected it. Or if we need to enable one. + if (!defined($prompts{$config}) && defined($selects{$config})) { + $process_selects{$config} = 1; + } + } + } +} + +sub loop_select { + + foreach my $config (keys %process_selects) { + $config =~ s/^CONFIG_//; + + dprint "Process select $config\n"; + + # config has no prompt and must be selected. + parse_config_selects $config, $selects{$config}; + } +} + +while ($repeat) { + # Get the first set of configs and their dependencies. + loop_depend; + + $repeat = 0; + + # Now we need to see if we have to check selects; + loop_select; +} + +my %setconfigs; + +# Finally, read the .config file and turn off any module enabled that +# we could not find a reason to keep enabled. +foreach my $line (@config_file) { + $_ = $line; + + if (/CONFIG_IKCONFIG/) { + if (/# CONFIG_IKCONFIG is not set/) { + # enable IKCONFIG at least as a module + print "CONFIG_IKCONFIG=m\n"; + # don't ask about PROC + print "# CONFIG_IKCONFIG_PROC is not set\n"; + } else { + print; + } + next; + } + + if (/CONFIG_MODULE_SIG_KEY="(.+)"/) { + my $orig_cert = $1; + my $default_cert = "certs/signing_key.pem"; + + # Check that the logic in this script still matches the one in Kconfig + if (!defined($depends{"MODULE_SIG_KEY"}) || + $depends{"MODULE_SIG_KEY"} !~ /"\Q$default_cert\E"/) { + print STDERR "WARNING: MODULE_SIG_KEY assertion failure, ", + "update needed to ", __FILE__, " line ", __LINE__, "\n"; + print; + } elsif ($orig_cert ne $default_cert && ! -f $orig_cert) { + print STDERR "Module signature verification enabled but ", + "module signing key \"$orig_cert\" not found. Resetting ", + "signing key to default value.\n"; + print "CONFIG_MODULE_SIG_KEY=\"$default_cert\"\n"; + } else { + print; + } + next; + } + + if (/CONFIG_SYSTEM_TRUSTED_KEYS="(.+)"/) { + my $orig_keys = $1; + + if (! -f $orig_keys) { + print STDERR "System keyring enabled but keys \"$orig_keys\" ", + "not found. Resetting keys to default value.\n"; + print "CONFIG_SYSTEM_TRUSTED_KEYS=\"\"\n"; + } else { + print; + } + next; + } + + if (/^(CONFIG.*)=(m|y)/) { + if (defined($configs{$1})) { + if ($localyesconfig) { + $setconfigs{$1} = 'y'; + print "$1=y\n"; + next; + } else { + $setconfigs{$1} = $2; + } + } elsif ($2 eq "m") { + print "# $1 is not set\n"; + next; + } + } + print; +} + +# Integrity check, make sure all modules that we want enabled do +# indeed have their configs set. +loop: +foreach my $module (keys(%modules)) { + if (defined($objects{$module})) { + my @arr = @{$objects{$module}}; + foreach my $conf (@arr) { + if (defined($setconfigs{$conf})) { + next loop; + } + } + print STDERR "module $module did not have configs"; + foreach my $conf (@arr) { + print STDERR " " , $conf; + } + print STDERR "\n"; + } +} diff --git a/scripts/kconfig/symbol.c b/scripts/kconfig/symbol.c new file mode 100644 index 0000000000..2432298487 --- /dev/null +++ b/scripts/kconfig/symbol.c @@ -0,0 +1,1392 @@ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include + +#include "lkc.h" + +struct symbol symbol_yes = { + .name = "y", + .curr = { "y", yes }, + .flags = SYMBOL_CONST|SYMBOL_VALID, +}, symbol_mod = { + .name = "m", + .curr = { "m", mod }, + .flags = SYMBOL_CONST|SYMBOL_VALID, +}, symbol_no = { + .name = "n", + .curr = { "n", no }, + .flags = SYMBOL_CONST|SYMBOL_VALID, +}, symbol_empty = { + .name = "", + .curr = { "", no }, + .flags = SYMBOL_VALID, +}; + +struct symbol *sym_defconfig_list; +struct symbol *modules_sym; +tristate modules_val; + +struct expr *sym_env_list; + +static void sym_add_default(struct symbol *sym, const char *def) +{ + struct property *prop = prop_alloc(P_DEFAULT, sym); + + prop->expr = expr_alloc_symbol(sym_lookup(def, SYMBOL_CONST)); +} + +void sym_init(void) +{ + struct symbol *sym; + struct utsname uts; + static bool inited = false; + + if (inited) + return; + inited = true; + + uname(&uts); + + sym = sym_lookup("UNAME_RELEASE", 0); + sym->type = S_STRING; + sym->flags |= SYMBOL_AUTO; + sym_add_default(sym, uts.release); +} + +enum symbol_type sym_get_type(struct symbol *sym) +{ + enum symbol_type type = sym->type; + + if (type == S_TRISTATE) { + if (sym_is_choice_value(sym) && sym->visible == yes) + type = S_BOOLEAN; + else if (modules_val == no) + type = S_BOOLEAN; + } + return type; +} + +const char *sym_type_name(enum symbol_type type) +{ + switch (type) { + case S_BOOLEAN: + return "boolean"; + case S_TRISTATE: + return "tristate"; + case S_INT: + return "integer"; + case S_HEX: + return "hex"; + case S_STRING: + return "string"; + case S_UNKNOWN: + return "unknown"; + case S_OTHER: + break; + } + return "???"; +} + +struct property *sym_get_choice_prop(struct symbol *sym) +{ + struct property *prop; + + for_all_choices(sym, prop) + return prop; + return NULL; +} + +struct property *sym_get_env_prop(struct symbol *sym) +{ + struct property *prop; + + for_all_properties(sym, prop, P_ENV) + return prop; + return NULL; +} + +static struct property *sym_get_default_prop(struct symbol *sym) +{ + struct property *prop; + + for_all_defaults(sym, prop) { + prop->visible.tri = expr_calc_value(prop->visible.expr); + if (prop->visible.tri != no) + return prop; + } + return NULL; +} + +static struct property *sym_get_range_prop(struct symbol *sym) +{ + struct property *prop; + + for_all_properties(sym, prop, P_RANGE) { + prop->visible.tri = expr_calc_value(prop->visible.expr); + if (prop->visible.tri != no) + return prop; + } + return NULL; +} + +static long long sym_get_range_val(struct symbol *sym, int base) +{ + sym_calc_value(sym); + switch (sym->type) { + case S_INT: + base = 10; + break; + case S_HEX: + base = 16; + break; + default: + break; + } + return strtoll(sym->curr.val, NULL, base); +} + +static void sym_validate_range(struct symbol *sym) +{ + struct property *prop; + int base; + long long val, val2; + char str[64]; + + switch (sym->type) { + case S_INT: + base = 10; + break; + case S_HEX: + base = 16; + break; + default: + return; + } + prop = sym_get_range_prop(sym); + if (!prop) + return; + val = strtoll(sym->curr.val, NULL, base); + val2 = sym_get_range_val(prop->expr->left.sym, base); + if (val >= val2) { + val2 = sym_get_range_val(prop->expr->right.sym, base); + if (val <= val2) + return; + } + if (sym->type == S_INT) + sprintf(str, "%lld", val2); + else + sprintf(str, "0x%llx", val2); + sym->curr.val = strdup(str); +} + +static void sym_set_changed(struct symbol *sym) +{ + struct property *prop; + + sym->flags |= SYMBOL_CHANGED; + for (prop = sym->prop; prop; prop = prop->next) { + if (prop->menu) + prop->menu->flags |= MENU_CHANGED; + } +} + +static void sym_set_all_changed(void) +{ + struct symbol *sym; + int i; + + for_all_symbols(i, sym) + sym_set_changed(sym); +} + +static void sym_calc_visibility(struct symbol *sym) +{ + struct property *prop; + struct symbol *choice_sym = NULL; + tristate tri; + + /* any prompt visible? */ + tri = no; + + if (sym_is_choice_value(sym)) + choice_sym = prop_get_symbol(sym_get_choice_prop(sym)); + + for_all_prompts(sym, prop) { + prop->visible.tri = expr_calc_value(prop->visible.expr); + /* + * Tristate choice_values with visibility 'mod' are + * not visible if the corresponding choice's value is + * 'yes'. + */ + if (choice_sym && sym->type == S_TRISTATE && + prop->visible.tri == mod && choice_sym->curr.tri == yes) + prop->visible.tri = no; + + tri = EXPR_OR(tri, prop->visible.tri); + } + if (tri == mod && (sym->type != S_TRISTATE || modules_val == no)) + tri = yes; + if (sym->visible != tri) { + sym->visible = tri; + sym_set_changed(sym); + } + if (sym_is_choice_value(sym)) + return; + /* defaulting to "yes" if no explicit "depends on" are given */ + tri = yes; + if (sym->dir_dep.expr) + tri = expr_calc_value(sym->dir_dep.expr); + if (tri == mod) + tri = yes; + if (sym->dir_dep.tri != tri) { + sym->dir_dep.tri = tri; + sym_set_changed(sym); + } + tri = no; + if (sym->rev_dep.expr) + tri = expr_calc_value(sym->rev_dep.expr); + if (tri == mod && sym_get_type(sym) == S_BOOLEAN) + tri = yes; + if (sym->rev_dep.tri != tri) { + sym->rev_dep.tri = tri; + sym_set_changed(sym); + } +} + +/* + * Find the default symbol for a choice. + * First try the default values for the choice symbol + * Next locate the first visible choice value + * Return NULL if none was found + */ +struct symbol *sym_choice_default(struct symbol *sym) +{ + struct symbol *def_sym; + struct property *prop; + struct expr *e; + + /* any of the defaults visible? */ + for_all_defaults(sym, prop) { + prop->visible.tri = expr_calc_value(prop->visible.expr); + if (prop->visible.tri == no) + continue; + def_sym = prop_get_symbol(prop); + if (def_sym->visible != no) + return def_sym; + } + + /* just get the first visible value */ + prop = sym_get_choice_prop(sym); + expr_list_for_each_sym(prop->expr, e, def_sym) + if (def_sym->visible != no) + return def_sym; + + /* failed to locate any defaults */ + return NULL; +} + +static struct symbol *sym_calc_choice(struct symbol *sym) +{ + struct symbol *def_sym; + struct property *prop; + struct expr *e; + int flags; + + /* first calculate all choice values' visibilities */ + flags = sym->flags; + prop = sym_get_choice_prop(sym); + expr_list_for_each_sym(prop->expr, e, def_sym) { + sym_calc_visibility(def_sym); + if (def_sym->visible != no) + flags &= def_sym->flags; + } + + sym->flags &= flags | ~SYMBOL_DEF_USER; + + /* is the user choice visible? */ + def_sym = sym->def[S_DEF_USER].val; + if (def_sym && def_sym->visible != no) + return def_sym; + + def_sym = sym_choice_default(sym); + + if (def_sym == NULL) + /* no choice? reset tristate value */ + sym->curr.tri = no; + + return def_sym; +} + +void sym_calc_value(struct symbol *sym) +{ + struct symbol_value newval, oldval; + struct property *prop; + struct expr *e; + + if (!sym) + return; + + if (sym->flags & SYMBOL_VALID) + return; + + if (sym_is_choice_value(sym) && + sym->flags & SYMBOL_NEED_SET_CHOICE_VALUES) { + sym->flags &= ~SYMBOL_NEED_SET_CHOICE_VALUES; + prop = sym_get_choice_prop(sym); + sym_calc_value(prop_get_symbol(prop)); + } + + sym->flags |= SYMBOL_VALID; + + oldval = sym->curr; + + switch (sym->type) { + case S_INT: + case S_HEX: + case S_STRING: + newval = symbol_empty.curr; + break; + case S_BOOLEAN: + case S_TRISTATE: + newval = symbol_no.curr; + break; + default: + sym->curr.val = sym->name; + sym->curr.tri = no; + return; + } + if (!sym_is_choice_value(sym)) + sym->flags &= ~SYMBOL_WRITE; + + sym_calc_visibility(sym); + + /* set default if recursively called */ + sym->curr = newval; + + switch (sym_get_type(sym)) { + case S_BOOLEAN: + case S_TRISTATE: + if (sym_is_choice_value(sym) && sym->visible == yes) { + prop = sym_get_choice_prop(sym); + newval.tri = (prop_get_symbol(prop)->curr.val == sym) ? yes : no; + } else { + if (sym->visible != no) { + /* if the symbol is visible use the user value + * if available, otherwise try the default value + */ + sym->flags |= SYMBOL_WRITE; + if (sym_has_value(sym)) { + newval.tri = EXPR_AND(sym->def[S_DEF_USER].tri, + sym->visible); + goto calc_newval; + } + } + if (sym->rev_dep.tri != no) + sym->flags |= SYMBOL_WRITE; + if (!sym_is_choice(sym)) { + prop = sym_get_default_prop(sym); + if (prop) { + sym->flags |= SYMBOL_WRITE; + newval.tri = EXPR_AND(expr_calc_value(prop->expr), + prop->visible.tri); + } + } + calc_newval: + if (sym->dir_dep.tri == no && sym->rev_dep.tri != no) { + struct expr *e; + e = expr_simplify_unmet_dep(sym->rev_dep.expr, + sym->dir_dep.expr); + fprintf(stderr, "warning: ("); + expr_fprint(e, stderr); + fprintf(stderr, ") selects %s which has unmet direct dependencies (", + sym->name); + expr_fprint(sym->dir_dep.expr, stderr); + fprintf(stderr, ")\n"); + expr_free(e); + } + newval.tri = EXPR_OR(newval.tri, sym->rev_dep.tri); + } + if (newval.tri == mod && sym_get_type(sym) == S_BOOLEAN) + newval.tri = yes; + break; + case S_STRING: + case S_HEX: + case S_INT: + if (sym->visible != no) { + sym->flags |= SYMBOL_WRITE; + if (sym_has_value(sym)) { + newval.val = sym->def[S_DEF_USER].val; + break; + } + } + prop = sym_get_default_prop(sym); + if (prop) { + struct symbol *ds = prop_get_symbol(prop); + if (ds) { + sym->flags |= SYMBOL_WRITE; + sym_calc_value(ds); + newval.val = ds->curr.val; + } + } + break; + default: + ; + } + + sym->curr = newval; + if (sym_is_choice(sym) && newval.tri == yes) + sym->curr.val = sym_calc_choice(sym); + sym_validate_range(sym); + + if (memcmp(&oldval, &sym->curr, sizeof(oldval))) { + sym_set_changed(sym); + if (modules_sym == sym) { + sym_set_all_changed(); + modules_val = modules_sym->curr.tri; + } + } + + if (sym_is_choice(sym)) { + struct symbol *choice_sym; + + prop = sym_get_choice_prop(sym); + expr_list_for_each_sym(prop->expr, e, choice_sym) { + if ((sym->flags & SYMBOL_WRITE) && + choice_sym->visible != no) + choice_sym->flags |= SYMBOL_WRITE; + if (sym->flags & SYMBOL_CHANGED) + sym_set_changed(choice_sym); + } + } + + if (sym->flags & SYMBOL_AUTO) + sym->flags &= ~SYMBOL_WRITE; + + if (sym->flags & SYMBOL_NEED_SET_CHOICE_VALUES) + set_all_choice_values(sym); +} + +void sym_clear_all_valid(void) +{ + struct symbol *sym; + int i; + + for_all_symbols(i, sym) + sym->flags &= ~SYMBOL_VALID; + sym_add_change_count(1); + sym_calc_value(modules_sym); +} + +bool sym_tristate_within_range(struct symbol *sym, tristate val) +{ + int type = sym_get_type(sym); + + if (sym->visible == no) + return false; + + if (type != S_BOOLEAN && type != S_TRISTATE) + return false; + + if (type == S_BOOLEAN && val == mod) + return false; + if (sym->visible <= sym->rev_dep.tri) + return false; + if (sym_is_choice_value(sym) && sym->visible == yes) + return val == yes; + return val >= sym->rev_dep.tri && val <= sym->visible; +} + +bool sym_set_tristate_value(struct symbol *sym, tristate val) +{ + tristate oldval = sym_get_tristate_value(sym); + + if (oldval != val && !sym_tristate_within_range(sym, val)) + return false; + + if (!(sym->flags & SYMBOL_DEF_USER)) { + sym->flags |= SYMBOL_DEF_USER; + sym_set_changed(sym); + } + /* + * setting a choice value also resets the new flag of the choice + * symbol and all other choice values. + */ + if (sym_is_choice_value(sym) && val == yes) { + struct symbol *cs = prop_get_symbol(sym_get_choice_prop(sym)); + struct property *prop; + struct expr *e; + + cs->def[S_DEF_USER].val = sym; + cs->flags |= SYMBOL_DEF_USER; + prop = sym_get_choice_prop(cs); + for (e = prop->expr; e; e = e->left.expr) { + if (e->right.sym->visible != no) + e->right.sym->flags |= SYMBOL_DEF_USER; + } + } + + sym->def[S_DEF_USER].tri = val; + if (oldval != val) + sym_clear_all_valid(); + + return true; +} + +tristate sym_toggle_tristate_value(struct symbol *sym) +{ + tristate oldval, newval; + + oldval = newval = sym_get_tristate_value(sym); + do { + switch (newval) { + case no: + newval = mod; + break; + case mod: + newval = yes; + break; + case yes: + newval = no; + break; + } + if (sym_set_tristate_value(sym, newval)) + break; + } while (oldval != newval); + return newval; +} + +bool sym_string_valid(struct symbol *sym, const char *str) +{ + signed char ch; + + switch (sym->type) { + case S_STRING: + return true; + case S_INT: + ch = *str++; + if (ch == '-') + ch = *str++; + if (!isdigit(ch)) + return false; + if (ch == '0' && *str != 0) + return false; + while ((ch = *str++)) { + if (!isdigit(ch)) + return false; + } + return true; + case S_HEX: + if (str[0] == '0' && (str[1] == 'x' || str[1] == 'X')) + str += 2; + ch = *str++; + do { + if (!isxdigit(ch)) + return false; + } while ((ch = *str++)); + return true; + case S_BOOLEAN: + case S_TRISTATE: + switch (str[0]) { + case 'y': case 'Y': + case 'm': case 'M': + case 'n': case 'N': + return true; + } + return false; + default: + return false; + } +} + +bool sym_string_within_range(struct symbol *sym, const char *str) +{ + struct property *prop; + long long val; + + switch (sym->type) { + case S_STRING: + return sym_string_valid(sym, str); + case S_INT: + if (!sym_string_valid(sym, str)) + return false; + prop = sym_get_range_prop(sym); + if (!prop) + return true; + val = strtoll(str, NULL, 10); + return val >= sym_get_range_val(prop->expr->left.sym, 10) && + val <= sym_get_range_val(prop->expr->right.sym, 10); + case S_HEX: + if (!sym_string_valid(sym, str)) + return false; + prop = sym_get_range_prop(sym); + if (!prop) + return true; + val = strtoll(str, NULL, 16); + return val >= sym_get_range_val(prop->expr->left.sym, 16) && + val <= sym_get_range_val(prop->expr->right.sym, 16); + case S_BOOLEAN: + case S_TRISTATE: + switch (str[0]) { + case 'y': case 'Y': + return sym_tristate_within_range(sym, yes); + case 'm': case 'M': + return sym_tristate_within_range(sym, mod); + case 'n': case 'N': + return sym_tristate_within_range(sym, no); + } + return false; + default: + return false; + } +} + +bool sym_set_string_value(struct symbol *sym, const char *newval) +{ + const char *oldval; + char *val; + int size; + + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + switch (newval[0]) { + case 'y': case 'Y': + return sym_set_tristate_value(sym, yes); + case 'm': case 'M': + return sym_set_tristate_value(sym, mod); + case 'n': case 'N': + return sym_set_tristate_value(sym, no); + } + return false; + default: + ; + } + + if (!sym_string_within_range(sym, newval)) + return false; + + if (!(sym->flags & SYMBOL_DEF_USER)) { + sym->flags |= SYMBOL_DEF_USER; + sym_set_changed(sym); + } + + oldval = sym->def[S_DEF_USER].val; + size = strlen(newval) + 1; + if (sym->type == S_HEX && (newval[0] != '0' || (newval[1] != 'x' && newval[1] != 'X'))) { + size += 2; + sym->def[S_DEF_USER].val = val = xmalloc(size); + *val++ = '0'; + *val++ = 'x'; + } else if (!oldval || strcmp(oldval, newval)) + sym->def[S_DEF_USER].val = val = xmalloc(size); + else + return true; + + strcpy(val, newval); + free((void *)oldval); + sym_clear_all_valid(); + + return true; +} + +/* + * Find the default value associated to a symbol. + * For tristate symbol handle the modules=n case + * in which case "m" becomes "y". + * If the symbol does not have any default then fallback + * to the fixed default values. + */ +const char *sym_get_string_default(struct symbol *sym) +{ + struct property *prop; + struct symbol *ds; + const char *str; + tristate val; + + sym_calc_visibility(sym); + sym_calc_value(modules_sym); + val = symbol_no.curr.tri; + str = symbol_empty.curr.val; + + /* If symbol has a default value look it up */ + prop = sym_get_default_prop(sym); + if (prop != NULL) { + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + /* The visibility may limit the value from yes => mod */ + val = EXPR_AND(expr_calc_value(prop->expr), prop->visible.tri); + break; + default: + /* + * The following fails to handle the situation + * where a default value is further limited by + * the valid range. + */ + ds = prop_get_symbol(prop); + if (ds != NULL) { + sym_calc_value(ds); + str = (const char *)ds->curr.val; + } + } + } + + /* Handle select statements */ + val = EXPR_OR(val, sym->rev_dep.tri); + + /* transpose mod to yes if modules are not enabled */ + if (val == mod) + if (!sym_is_choice_value(sym) && modules_sym->curr.tri == no) + val = yes; + + /* transpose mod to yes if type is bool */ + if (sym->type == S_BOOLEAN && val == mod) + val = yes; + + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + switch (val) { + case no: return "n"; + case mod: return "m"; + case yes: return "y"; + } + case S_INT: + case S_HEX: + return str; + case S_STRING: + return str; + case S_OTHER: + case S_UNKNOWN: + break; + } + return ""; +} + +const char *sym_get_string_value(struct symbol *sym) +{ + tristate val; + + switch (sym->type) { + case S_BOOLEAN: + case S_TRISTATE: + val = sym_get_tristate_value(sym); + switch (val) { + case no: + return "n"; + case mod: + sym_calc_value(modules_sym); + return (modules_sym->curr.tri == no) ? "n" : "m"; + case yes: + return "y"; + } + break; + default: + ; + } + return (const char *)sym->curr.val; +} + +bool sym_is_changable(struct symbol *sym) +{ + return sym->visible > sym->rev_dep.tri; +} + +static unsigned strhash(const char *s) +{ + /* fnv32 hash */ + unsigned hash = 2166136261U; + for (; *s; s++) + hash = (hash ^ *s) * 0x01000193; + return hash; +} + +struct symbol *sym_lookup(const char *name, int flags) +{ + struct symbol *symbol; + char *new_name; + int hash; + + if (name) { + if (name[0] && !name[1]) { + switch (name[0]) { + case 'y': return &symbol_yes; + case 'm': return &symbol_mod; + case 'n': return &symbol_no; + } + } + hash = strhash(name) % SYMBOL_HASHSIZE; + + for (symbol = symbol_hash[hash]; symbol; symbol = symbol->next) { + if (symbol->name && + !strcmp(symbol->name, name) && + (flags ? symbol->flags & flags + : !(symbol->flags & (SYMBOL_CONST|SYMBOL_CHOICE)))) + return symbol; + } + new_name = strdup(name); + } else { + new_name = NULL; + hash = 0; + } + + symbol = xmalloc(sizeof(*symbol)); + memset(symbol, 0, sizeof(*symbol)); + symbol->name = new_name; + symbol->type = S_UNKNOWN; + symbol->flags |= flags; + + symbol->next = symbol_hash[hash]; + symbol_hash[hash] = symbol; + + return symbol; +} + +struct symbol *sym_find(const char *name) +{ + struct symbol *symbol = NULL; + int hash = 0; + + if (!name) + return NULL; + + if (name[0] && !name[1]) { + switch (name[0]) { + case 'y': return &symbol_yes; + case 'm': return &symbol_mod; + case 'n': return &symbol_no; + } + } + hash = strhash(name) % SYMBOL_HASHSIZE; + + for (symbol = symbol_hash[hash]; symbol; symbol = symbol->next) { + if (symbol->name && + !strcmp(symbol->name, name) && + !(symbol->flags & SYMBOL_CONST)) + break; + } + + return symbol; +} + +/* + * Expand symbol's names embedded in the string given in argument. Symbols' + * name to be expanded shall be prefixed by a '$'. Unknown symbol expands to + * the empty string. + */ +const char *sym_expand_string_value(const char *in) +{ + const char *src; + char *res; + size_t reslen; + + reslen = strlen(in) + 1; + res = xmalloc(reslen); + res[0] = '\0'; + + while ((src = strchr(in, '$'))) { + char *p, name[SYMBOL_MAXLENGTH]; + const char *symval = ""; + struct symbol *sym; + size_t newlen; + + strncat(res, in, src - in); + src++; + + p = name; + while (isalnum(*src) || *src == '_') + *p++ = *src++; + *p = '\0'; + + sym = sym_find(name); + if (sym != NULL) { + sym_calc_value(sym); + symval = sym_get_string_value(sym); + } + + newlen = strlen(res) + strlen(symval) + strlen(src) + 1; + if (newlen > reslen) { + reslen = newlen; + res = realloc(res, reslen); + } + + strcat(res, symval); + in = src; + } + strcat(res, in); + + return res; +} + +const char *sym_escape_string_value(const char *in) +{ + const char *p; + size_t reslen; + char *res; + size_t l; + + reslen = strlen(in) + strlen("\"\"") + 1; + + p = in; + for (;;) { + l = strcspn(p, "\"\\"); + p += l; + + if (p[0] == '\0') + break; + + reslen++; + p++; + } + + res = xmalloc(reslen); + res[0] = '\0'; + + strcat(res, "\""); + + p = in; + for (;;) { + l = strcspn(p, "\"\\"); + strncat(res, p, l); + p += l; + + if (p[0] == '\0') + break; + + strcat(res, "\\"); + strncat(res, p++, 1); + } + + strcat(res, "\""); + return res; +} + +struct sym_match { + struct symbol *sym; + off_t so, eo; +}; + +/* Compare matched symbols as thus: + * - first, symbols that match exactly + * - then, alphabetical sort + */ +static int sym_rel_comp(const void *sym1, const void *sym2) +{ + const struct sym_match *s1 = sym1; + const struct sym_match *s2 = sym2; + int exact1, exact2; + + /* Exact match: + * - if matched length on symbol s1 is the length of that symbol, + * then this symbol should come first; + * - if matched length on symbol s2 is the length of that symbol, + * then this symbol should come first. + * Note: since the search can be a regexp, both symbols may match + * exactly; if this is the case, we can't decide which comes first, + * and we fallback to sorting alphabetically. + */ + exact1 = (s1->eo - s1->so) == strlen(s1->sym->name); + exact2 = (s2->eo - s2->so) == strlen(s2->sym->name); + if (exact1 && !exact2) + return -1; + if (!exact1 && exact2) + return 1; + + /* As a fallback, sort symbols alphabetically */ + return strcmp(s1->sym->name, s2->sym->name); +} + +struct symbol **sym_re_search(const char *pattern) +{ + struct symbol *sym, **sym_arr = NULL; + struct sym_match *sym_match_arr = NULL; + int i, cnt, size; + regex_t re; + regmatch_t match[1]; + + cnt = size = 0; + /* Skip if empty */ + if (strlen(pattern) == 0) + return NULL; + if (regcomp(&re, pattern, REG_EXTENDED|REG_ICASE)) + return NULL; + + for_all_symbols(i, sym) { + if (sym->flags & SYMBOL_CONST || !sym->name) + continue; + if (regexec(&re, sym->name, 1, match, 0)) + continue; + if (cnt >= size) { + void *tmp; + size += 16; + tmp = realloc(sym_match_arr, size * sizeof(struct sym_match)); + if (!tmp) + goto sym_re_search_free; + sym_match_arr = tmp; + } + sym_calc_value(sym); + /* As regexec returned 0, we know we have a match, so + * we can use match[0].rm_[se]o without further checks + */ + sym_match_arr[cnt].so = match[0].rm_so; + sym_match_arr[cnt].eo = match[0].rm_eo; + sym_match_arr[cnt++].sym = sym; + } + if (sym_match_arr) { + qsort(sym_match_arr, cnt, sizeof(struct sym_match), sym_rel_comp); + sym_arr = malloc((cnt+1) * sizeof(struct symbol)); + if (!sym_arr) + goto sym_re_search_free; + for (i = 0; i < cnt; i++) + sym_arr[i] = sym_match_arr[i].sym; + sym_arr[cnt] = NULL; + } +sym_re_search_free: + /* sym_match_arr can be NULL if no match, but free(NULL) is OK */ + free(sym_match_arr); + regfree(&re); + + return sym_arr; +} + +/* + * When we check for recursive dependencies we use a stack to save + * current state so we can print out relevant info to user. + * The entries are located on the call stack so no need to free memory. + * Note insert() remove() must always match to properly clear the stack. + */ +static struct dep_stack { + struct dep_stack *prev, *next; + struct symbol *sym; + struct property *prop; + struct expr *expr; +} *check_top; + +static void dep_stack_insert(struct dep_stack *stack, struct symbol *sym) +{ + memset(stack, 0, sizeof(*stack)); + if (check_top) + check_top->next = stack; + stack->prev = check_top; + stack->sym = sym; + check_top = stack; +} + +static void dep_stack_remove(void) +{ + check_top = check_top->prev; + if (check_top) + check_top->next = NULL; +} + +/* + * Called when we have detected a recursive dependency. + * check_top point to the top of the stact so we use + * the ->prev pointer to locate the bottom of the stack. + */ +static void sym_check_print_recursive(struct symbol *last_sym) +{ + struct dep_stack *stack; + struct symbol *sym, *next_sym; + struct menu *menu = NULL; + struct property *prop; + struct dep_stack cv_stack; + + if (sym_is_choice_value(last_sym)) { + dep_stack_insert(&cv_stack, last_sym); + last_sym = prop_get_symbol(sym_get_choice_prop(last_sym)); + } + + for (stack = check_top; stack != NULL; stack = stack->prev) + if (stack->sym == last_sym) + break; + if (!stack) { + fprintf(stderr, "unexpected recursive dependency error\n"); + return; + } + + for (; stack; stack = stack->next) { + sym = stack->sym; + next_sym = stack->next ? stack->next->sym : last_sym; + prop = stack->prop; + if (prop == NULL) + prop = stack->sym->prop; + + /* for choice values find the menu entry (used below) */ + if (sym_is_choice(sym) || sym_is_choice_value(sym)) { + for (prop = sym->prop; prop; prop = prop->next) { + menu = prop->menu; + if (prop->menu) + break; + } + } + if (stack->sym == last_sym) + fprintf(stderr, "%s:%d:error: recursive dependency detected!\n", + prop->file->name, prop->lineno); + fprintf(stderr, "For a resolution refer to Documentation/kbuild/kconfig-language.txt\n"); + fprintf(stderr, "subsection \"Kconfig recursive dependency limitations\"\n"); + if (stack->expr) { + fprintf(stderr, "%s:%d:\tsymbol %s %s value contains %s\n", + prop->file->name, prop->lineno, + sym->name ? sym->name : "", + prop_get_type_name(prop->type), + next_sym->name ? next_sym->name : ""); + } else if (stack->prop) { + fprintf(stderr, "%s:%d:\tsymbol %s depends on %s\n", + prop->file->name, prop->lineno, + sym->name ? sym->name : "", + next_sym->name ? next_sym->name : ""); + } else if (sym_is_choice(sym)) { + fprintf(stderr, "%s:%d:\tchoice %s contains symbol %s\n", + menu->file->name, menu->lineno, + sym->name ? sym->name : "", + next_sym->name ? next_sym->name : ""); + } else if (sym_is_choice_value(sym)) { + fprintf(stderr, "%s:%d:\tsymbol %s is part of choice %s\n", + menu->file->name, menu->lineno, + sym->name ? sym->name : "", + next_sym->name ? next_sym->name : ""); + } else { + fprintf(stderr, "%s:%d:\tsymbol %s is selected by %s\n", + prop->file->name, prop->lineno, + sym->name ? sym->name : "", + next_sym->name ? next_sym->name : ""); + } + } + + if (check_top == &cv_stack) + dep_stack_remove(); +} + +static struct symbol *sym_check_expr_deps(struct expr *e) +{ + struct symbol *sym; + + if (!e) + return NULL; + switch (e->type) { + case E_OR: + case E_AND: + sym = sym_check_expr_deps(e->left.expr); + if (sym) + return sym; + return sym_check_expr_deps(e->right.expr); + case E_NOT: + return sym_check_expr_deps(e->left.expr); + case E_EQUAL: + case E_GEQ: + case E_GTH: + case E_LEQ: + case E_LTH: + case E_UNEQUAL: + sym = sym_check_deps(e->left.sym); + if (sym) + return sym; + return sym_check_deps(e->right.sym); + case E_SYMBOL: + return sym_check_deps(e->left.sym); + default: + break; + } + printf("Oops! How to check %d?\n", e->type); + return NULL; +} + +/* return NULL when dependencies are OK */ +static struct symbol *sym_check_sym_deps(struct symbol *sym) +{ + struct symbol *sym2; + struct property *prop; + struct dep_stack stack; + + dep_stack_insert(&stack, sym); + + sym2 = sym_check_expr_deps(sym->rev_dep.expr); + if (sym2) + goto out; + + for (prop = sym->prop; prop; prop = prop->next) { + if (prop->type == P_CHOICE || prop->type == P_SELECT) + continue; + stack.prop = prop; + sym2 = sym_check_expr_deps(prop->visible.expr); + if (sym2) + break; + if (prop->type != P_DEFAULT || sym_is_choice(sym)) + continue; + stack.expr = prop->expr; + sym2 = sym_check_expr_deps(prop->expr); + if (sym2) + break; + stack.expr = NULL; + } + +out: + dep_stack_remove(); + + return sym2; +} + +static struct symbol *sym_check_choice_deps(struct symbol *choice) +{ + struct symbol *sym, *sym2; + struct property *prop; + struct expr *e; + struct dep_stack stack; + + dep_stack_insert(&stack, choice); + + prop = sym_get_choice_prop(choice); + expr_list_for_each_sym(prop->expr, e, sym) + sym->flags |= (SYMBOL_CHECK | SYMBOL_CHECKED); + + choice->flags |= (SYMBOL_CHECK | SYMBOL_CHECKED); + sym2 = sym_check_sym_deps(choice); + choice->flags &= ~SYMBOL_CHECK; + if (sym2) + goto out; + + expr_list_for_each_sym(prop->expr, e, sym) { + sym2 = sym_check_sym_deps(sym); + if (sym2) + break; + } +out: + expr_list_for_each_sym(prop->expr, e, sym) + sym->flags &= ~SYMBOL_CHECK; + + if (sym2 && sym_is_choice_value(sym2) && + prop_get_symbol(sym_get_choice_prop(sym2)) == choice) + sym2 = choice; + + dep_stack_remove(); + + return sym2; +} + +struct symbol *sym_check_deps(struct symbol *sym) +{ + struct symbol *sym2; + struct property *prop; + + if (sym->flags & SYMBOL_CHECK) { + sym_check_print_recursive(sym); + return sym; + } + if (sym->flags & SYMBOL_CHECKED) + return NULL; + + if (sym_is_choice_value(sym)) { + struct dep_stack stack; + + /* for choice groups start the check with main choice symbol */ + dep_stack_insert(&stack, sym); + prop = sym_get_choice_prop(sym); + sym2 = sym_check_deps(prop_get_symbol(prop)); + dep_stack_remove(); + } else if (sym_is_choice(sym)) { + sym2 = sym_check_choice_deps(sym); + } else { + sym->flags |= (SYMBOL_CHECK | SYMBOL_CHECKED); + sym2 = sym_check_sym_deps(sym); + sym->flags &= ~SYMBOL_CHECK; + } + + if (sym2 && sym2 == sym) + sym2 = NULL; + + return sym2; +} + +struct property *prop_alloc(enum prop_type type, struct symbol *sym) +{ + struct property *prop; + struct property **propp; + + prop = xmalloc(sizeof(*prop)); + memset(prop, 0, sizeof(*prop)); + prop->type = type; + prop->sym = sym; + prop->file = current_file; + prop->lineno = zconf_lineno(); + + /* append property to the prop list of symbol */ + if (sym) { + for (propp = &sym->prop; *propp; propp = &(*propp)->next) + ; + *propp = prop; + } + + return prop; +} + +struct symbol *prop_get_symbol(struct property *prop) +{ + if (prop->expr && (prop->expr->type == E_SYMBOL || + prop->expr->type == E_LIST)) + return prop->expr->left.sym; + return NULL; +} + +const char *prop_get_type_name(enum prop_type type) +{ + switch (type) { + case P_PROMPT: + return "prompt"; + case P_ENV: + return "env"; + case P_COMMENT: + return "comment"; + case P_MENU: + return "menu"; + case P_DEFAULT: + return "default"; + case P_CHOICE: + return "choice"; + case P_SELECT: + return "select"; + case P_RANGE: + return "range"; + case P_SYMBOL: + return "symbol"; + case P_UNKNOWN: + break; + } + return "unknown"; +} + +static void prop_add_env(const char *env) +{ + struct symbol *sym, *sym2; + struct property *prop; + char *p; + + sym = current_entry->sym; + sym->flags |= SYMBOL_AUTO; + for_all_properties(sym, prop, P_ENV) { + sym2 = prop_get_symbol(prop); + if (strcmp(sym2->name, env)) + menu_warn(current_entry, "redefining environment symbol from %s", + sym2->name); + return; + } + + prop = prop_alloc(P_ENV, sym); + prop->expr = expr_alloc_symbol(sym_lookup(env, SYMBOL_CONST)); + + sym_env_list = expr_alloc_one(E_LIST, sym_env_list); + sym_env_list->right.sym = sym; + + p = getenv(env); + if (p) + sym_add_default(sym, p); + else + menu_warn(current_entry, "environment variable %s undefined", env); +} diff --git a/scripts/kconfig/util.c b/scripts/kconfig/util.c new file mode 100644 index 0000000000..0e76042473 --- /dev/null +++ b/scripts/kconfig/util.c @@ -0,0 +1,147 @@ +/* + * Copyright (C) 2002-2005 Roman Zippel + * Copyright (C) 2002-2005 Sam Ravnborg + * + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include "lkc.h" + +/* file already present in list? If not add it */ +struct file *file_lookup(const char *name) +{ + struct file *file; + const char *file_name = sym_expand_string_value(name); + + for (file = file_list; file; file = file->next) { + if (!strcmp(name, file->name)) { + free((void *)file_name); + return file; + } + } + + file = xmalloc(sizeof(*file)); + memset(file, 0, sizeof(*file)); + file->name = file_name; + file->next = file_list; + file_list = file; + return file; +} + +/* write a dependency file as used by kbuild to track dependencies */ +int file_write_dep(const char *name) +{ + struct symbol *sym, *env_sym; + struct expr *e; + struct file *file; + FILE *out; + + if (!name) + name = ".kconfig.d"; + out = fopen("..config.tmp", "w"); + if (!out) + return 1; + fprintf(out, "deps_config := \\\n"); + for (file = file_list; file; file = file->next) { + if (file->next) + fprintf(out, "\t%s \\\n", file->name); + else + fprintf(out, "\t%s\n", file->name); + } + fprintf(out, "\n%s: \\\n" + "\t$(deps_config)\n\n", conf_get_autoconfig_name()); + + expr_list_for_each_sym(sym_env_list, e, sym) { + struct property *prop; + const char *value; + + prop = sym_get_env_prop(sym); + env_sym = prop_get_symbol(prop); + if (!env_sym) + continue; + value = getenv(env_sym->name); + if (!value) + value = ""; + fprintf(out, "ifneq \"$(%s)\" \"%s\"\n", env_sym->name, value); + fprintf(out, "%s: FORCE\n", conf_get_autoconfig_name()); + fprintf(out, "endif\n"); + } + + fprintf(out, "\n$(deps_config): ;\n"); + fclose(out); + rename("..config.tmp", name); + return 0; +} + + +/* Allocate initial growable string */ +struct gstr str_new(void) +{ + struct gstr gs; + gs.s = xmalloc(sizeof(char) * 64); + gs.len = 64; + gs.max_width = 0; + strcpy(gs.s, "\0"); + return gs; +} + +/* Free storage for growable string */ +void str_free(struct gstr *gs) +{ + if (gs->s) + free(gs->s); + gs->s = NULL; + gs->len = 0; +} + +/* Append to growable string */ +void str_append(struct gstr *gs, const char *s) +{ + size_t l; + if (s) { + l = strlen(gs->s) + strlen(s) + 1; + if (l > gs->len) { + gs->s = realloc(gs->s, l); + gs->len = l; + } + strcat(gs->s, s); + } +} + +/* Append printf formatted string to growable string */ +void str_printf(struct gstr *gs, const char *fmt, ...) +{ + va_list ap; + char s[10000]; /* big enough... */ + va_start(ap, fmt); + vsnprintf(s, sizeof(s), fmt, ap); + str_append(gs, s); + va_end(ap); +} + +/* Retrieve value of growable string */ +const char *str_get(struct gstr *gs) +{ + return gs->s; +} + +void *xmalloc(size_t size) +{ + void *p = malloc(size); + if (p) + return p; + fprintf(stderr, "Out of memory.\n"); + exit(1); +} + +void *xcalloc(size_t nmemb, size_t size) +{ + void *p = calloc(nmemb, size); + if (p) + return p; + fprintf(stderr, "Out of memory.\n"); + exit(1); +} diff --git a/scripts/kconfig/zconf.gperf b/scripts/kconfig/zconf.gperf new file mode 100644 index 0000000000..ac498f01b4 --- /dev/null +++ b/scripts/kconfig/zconf.gperf @@ -0,0 +1,49 @@ +%language=ANSI-C +%define hash-function-name kconf_id_hash +%define lookup-function-name kconf_id_lookup +%define string-pool-name kconf_id_strings +%compare-strncmp +%enum +%pic +%struct-type + +struct kconf_id; + +static const struct kconf_id *kconf_id_lookup(register const char *str, register unsigned int len); + +%% +mainmenu, T_MAINMENU, TF_COMMAND +menu, T_MENU, TF_COMMAND +endmenu, T_ENDMENU, TF_COMMAND +source, T_SOURCE, TF_COMMAND +choice, T_CHOICE, TF_COMMAND +endchoice, T_ENDCHOICE, TF_COMMAND +comment, T_COMMENT, TF_COMMAND +config, T_CONFIG, TF_COMMAND +menuconfig, T_MENUCONFIG, TF_COMMAND +help, T_HELP, TF_COMMAND +---help---, T_HELP, TF_COMMAND +if, T_IF, TF_COMMAND|TF_PARAM +endif, T_ENDIF, TF_COMMAND +depends, T_DEPENDS, TF_COMMAND +optional, T_OPTIONAL, TF_COMMAND +default, T_DEFAULT, TF_COMMAND, S_UNKNOWN +prompt, T_PROMPT, TF_COMMAND +tristate, T_TYPE, TF_COMMAND, S_TRISTATE +def_tristate, T_DEFAULT, TF_COMMAND, S_TRISTATE +bool, T_TYPE, TF_COMMAND, S_BOOLEAN +boolean, T_TYPE, TF_COMMAND, S_BOOLEAN +def_bool, T_DEFAULT, TF_COMMAND, S_BOOLEAN +int, T_TYPE, TF_COMMAND, S_INT +hex, T_TYPE, TF_COMMAND, S_HEX +string, T_TYPE, TF_COMMAND, S_STRING +select, T_SELECT, TF_COMMAND +range, T_RANGE, TF_COMMAND +visible, T_VISIBLE, TF_COMMAND +option, T_OPTION, TF_COMMAND +on, T_ON, TF_PARAM +modules, T_OPT_MODULES, TF_OPTION +defconfig_list, T_OPT_DEFCONFIG_LIST,TF_OPTION +env, T_OPT_ENV, TF_OPTION +allnoconfig_y, T_OPT_ALLNOCONFIG_Y,TF_OPTION +%% diff --git a/scripts/kconfig/zconf.hash.c_shipped b/scripts/kconfig/zconf.hash.c_shipped new file mode 100644 index 0000000000..360a62df2b --- /dev/null +++ b/scripts/kconfig/zconf.hash.c_shipped @@ -0,0 +1,293 @@ +/* ANSI-C code produced by gperf version 3.0.4 */ +/* Command-line: gperf -t --output-file scripts/kconfig/zconf.hash.c_shipped -a -C -E -g -k '1,3,$' -p -t scripts/kconfig/zconf.gperf */ + +#if !((' ' == 32) && ('!' == 33) && ('"' == 34) && ('#' == 35) \ + && ('%' == 37) && ('&' == 38) && ('\'' == 39) && ('(' == 40) \ + && (')' == 41) && ('*' == 42) && ('+' == 43) && (',' == 44) \ + && ('-' == 45) && ('.' == 46) && ('/' == 47) && ('0' == 48) \ + && ('1' == 49) && ('2' == 50) && ('3' == 51) && ('4' == 52) \ + && ('5' == 53) && ('6' == 54) && ('7' == 55) && ('8' == 56) \ + && ('9' == 57) && (':' == 58) && (';' == 59) && ('<' == 60) \ + && ('=' == 61) && ('>' == 62) && ('?' == 63) && ('A' == 65) \ + && ('B' == 66) && ('C' == 67) && ('D' == 68) && ('E' == 69) \ + && ('F' == 70) && ('G' == 71) && ('H' == 72) && ('I' == 73) \ + && ('J' == 74) && ('K' == 75) && ('L' == 76) && ('M' == 77) \ + && ('N' == 78) && ('O' == 79) && ('P' == 80) && ('Q' == 81) \ + && ('R' == 82) && ('S' == 83) && ('T' == 84) && ('U' == 85) \ + && ('V' == 86) && ('W' == 87) && ('X' == 88) && ('Y' == 89) \ + && ('Z' == 90) && ('[' == 91) && ('\\' == 92) && (']' == 93) \ + && ('^' == 94) && ('_' == 95) && ('a' == 97) && ('b' == 98) \ + && ('c' == 99) && ('d' == 100) && ('e' == 101) && ('f' == 102) \ + && ('g' == 103) && ('h' == 104) && ('i' == 105) && ('j' == 106) \ + && ('k' == 107) && ('l' == 108) && ('m' == 109) && ('n' == 110) \ + && ('o' == 111) && ('p' == 112) && ('q' == 113) && ('r' == 114) \ + && ('s' == 115) && ('t' == 116) && ('u' == 117) && ('v' == 118) \ + && ('w' == 119) && ('x' == 120) && ('y' == 121) && ('z' == 122) \ + && ('{' == 123) && ('|' == 124) && ('}' == 125) && ('~' == 126)) +/* The character set is not based on ISO-646. */ +#error "gperf generated tables don't work with this execution character set. Please report a bug to ." +#endif + +#line 10 "scripts/kconfig/zconf.gperf" +struct kconf_id; + +static const struct kconf_id *kconf_id_lookup(register const char *str, register unsigned int len); +/* maximum key range = 71, duplicates = 0 */ + +#ifdef __GNUC__ +__inline +#else +#ifdef __cplusplus +inline +#endif +#endif +static unsigned int +kconf_id_hash (register const char *str, register unsigned int len) +{ + static const unsigned char asso_values[] = + { + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 0, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 5, 25, 25, + 0, 0, 0, 5, 0, 0, 73, 73, 5, 0, + 10, 5, 45, 73, 20, 20, 0, 15, 15, 73, + 20, 5, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73, 73, 73, 73, 73, + 73, 73, 73, 73, 73, 73 + }; + register int hval = len; + + switch (hval) + { + default: + hval += asso_values[(unsigned char)str[2]]; + /*FALLTHROUGH*/ + case 2: + case 1: + hval += asso_values[(unsigned char)str[0]]; + break; + } + return hval + asso_values[(unsigned char)str[len - 1]]; +} + +struct kconf_id_strings_t + { + char kconf_id_strings_str2[sizeof("if")]; + char kconf_id_strings_str3[sizeof("int")]; + char kconf_id_strings_str5[sizeof("endif")]; + char kconf_id_strings_str7[sizeof("default")]; + char kconf_id_strings_str8[sizeof("tristate")]; + char kconf_id_strings_str9[sizeof("endchoice")]; + char kconf_id_strings_str10[sizeof("---help---")]; + char kconf_id_strings_str12[sizeof("def_tristate")]; + char kconf_id_strings_str13[sizeof("def_bool")]; + char kconf_id_strings_str14[sizeof("defconfig_list")]; + char kconf_id_strings_str17[sizeof("on")]; + char kconf_id_strings_str18[sizeof("optional")]; + char kconf_id_strings_str21[sizeof("option")]; + char kconf_id_strings_str22[sizeof("endmenu")]; + char kconf_id_strings_str23[sizeof("mainmenu")]; + char kconf_id_strings_str25[sizeof("menuconfig")]; + char kconf_id_strings_str27[sizeof("modules")]; + char kconf_id_strings_str28[sizeof("allnoconfig_y")]; + char kconf_id_strings_str29[sizeof("menu")]; + char kconf_id_strings_str31[sizeof("select")]; + char kconf_id_strings_str32[sizeof("comment")]; + char kconf_id_strings_str33[sizeof("env")]; + char kconf_id_strings_str35[sizeof("range")]; + char kconf_id_strings_str36[sizeof("choice")]; + char kconf_id_strings_str39[sizeof("bool")]; + char kconf_id_strings_str41[sizeof("source")]; + char kconf_id_strings_str42[sizeof("visible")]; + char kconf_id_strings_str43[sizeof("hex")]; + char kconf_id_strings_str46[sizeof("config")]; + char kconf_id_strings_str47[sizeof("boolean")]; + char kconf_id_strings_str51[sizeof("string")]; + char kconf_id_strings_str54[sizeof("help")]; + char kconf_id_strings_str56[sizeof("prompt")]; + char kconf_id_strings_str72[sizeof("depends")]; + }; +static const struct kconf_id_strings_t kconf_id_strings_contents = + { + "if", + "int", + "endif", + "default", + "tristate", + "endchoice", + "---help---", + "def_tristate", + "def_bool", + "defconfig_list", + "on", + "optional", + "option", + "endmenu", + "mainmenu", + "menuconfig", + "modules", + "allnoconfig_y", + "menu", + "select", + "comment", + "env", + "range", + "choice", + "bool", + "source", + "visible", + "hex", + "config", + "boolean", + "string", + "help", + "prompt", + "depends" + }; +#define kconf_id_strings ((const char *) &kconf_id_strings_contents) +#ifdef __GNUC__ +__inline +#if defined __GNUC_STDC_INLINE__ || defined __GNUC_GNU_INLINE__ +__attribute__ ((__gnu_inline__)) +#endif +#endif +const struct kconf_id * +kconf_id_lookup (register const char *str, register unsigned int len) +{ + enum + { + TOTAL_KEYWORDS = 34, + MIN_WORD_LENGTH = 2, + MAX_WORD_LENGTH = 14, + MIN_HASH_VALUE = 2, + MAX_HASH_VALUE = 72 + }; + + static const struct kconf_id wordlist[] = + { + {-1}, {-1}, +#line 26 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str2, T_IF, TF_COMMAND|TF_PARAM}, +#line 37 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str3, T_TYPE, TF_COMMAND, S_INT}, + {-1}, +#line 27 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str5, T_ENDIF, TF_COMMAND}, + {-1}, +#line 30 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str7, T_DEFAULT, TF_COMMAND, S_UNKNOWN}, +#line 32 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str8, T_TYPE, TF_COMMAND, S_TRISTATE}, +#line 20 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str9, T_ENDCHOICE, TF_COMMAND}, +#line 25 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str10, T_HELP, TF_COMMAND}, + {-1}, +#line 33 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str12, T_DEFAULT, TF_COMMAND, S_TRISTATE}, +#line 36 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str13, T_DEFAULT, TF_COMMAND, S_BOOLEAN}, +#line 46 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str14, T_OPT_DEFCONFIG_LIST,TF_OPTION}, + {-1}, {-1}, +#line 44 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str17, T_ON, TF_PARAM}, +#line 29 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str18, T_OPTIONAL, TF_COMMAND}, + {-1}, {-1}, +#line 43 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str21, T_OPTION, TF_COMMAND}, +#line 17 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str22, T_ENDMENU, TF_COMMAND}, +#line 15 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str23, T_MAINMENU, TF_COMMAND}, + {-1}, +#line 23 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str25, T_MENUCONFIG, TF_COMMAND}, + {-1}, +#line 45 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str27, T_OPT_MODULES, TF_OPTION}, +#line 48 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str28, T_OPT_ALLNOCONFIG_Y,TF_OPTION}, +#line 16 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str29, T_MENU, TF_COMMAND}, + {-1}, +#line 40 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str31, T_SELECT, TF_COMMAND}, +#line 21 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str32, T_COMMENT, TF_COMMAND}, +#line 47 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str33, T_OPT_ENV, TF_OPTION}, + {-1}, +#line 41 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str35, T_RANGE, TF_COMMAND}, +#line 19 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str36, T_CHOICE, TF_COMMAND}, + {-1}, {-1}, +#line 34 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str39, T_TYPE, TF_COMMAND, S_BOOLEAN}, + {-1}, +#line 18 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str41, T_SOURCE, TF_COMMAND}, +#line 42 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str42, T_VISIBLE, TF_COMMAND}, +#line 38 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str43, T_TYPE, TF_COMMAND, S_HEX}, + {-1}, {-1}, +#line 22 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str46, T_CONFIG, TF_COMMAND}, +#line 35 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str47, T_TYPE, TF_COMMAND, S_BOOLEAN}, + {-1}, {-1}, {-1}, +#line 39 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str51, T_TYPE, TF_COMMAND, S_STRING}, + {-1}, {-1}, +#line 24 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str54, T_HELP, TF_COMMAND}, + {-1}, +#line 31 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str56, T_PROMPT, TF_COMMAND}, + {-1}, {-1}, {-1}, {-1}, {-1}, {-1}, {-1}, {-1}, {-1}, + {-1}, {-1}, {-1}, {-1}, {-1}, {-1}, +#line 28 "scripts/kconfig/zconf.gperf" + {(int)(long)&((struct kconf_id_strings_t *)0)->kconf_id_strings_str72, T_DEPENDS, TF_COMMAND} + }; + + if (len <= MAX_WORD_LENGTH && len >= MIN_WORD_LENGTH) + { + register int key = kconf_id_hash (str, len); + + if (key <= MAX_HASH_VALUE && key >= 0) + { + register int o = wordlist[key].name; + if (o >= 0) + { + register const char *s = o + kconf_id_strings; + + if (*str == *s && !strncmp (str + 1, s + 1, len - 1) && s[len] == '\0') + return &wordlist[key]; + } + } + } + return 0; +} +#line 49 "scripts/kconfig/zconf.gperf" + diff --git a/scripts/kconfig/zconf.l b/scripts/kconfig/zconf.l new file mode 100644 index 0000000000..c410d257da --- /dev/null +++ b/scripts/kconfig/zconf.l @@ -0,0 +1,374 @@ +%option nostdinit noyywrap never-interactive full ecs +%option 8bit nodefault perf-report perf-report +%option noinput +%x COMMAND HELP STRING PARAM +%{ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include + +#include "lkc.h" + +#define START_STRSIZE 16 + +static struct { + struct file *file; + int lineno; +} current_pos; + +static char *text; +static int text_size, text_asize; + +struct buffer { + struct buffer *parent; + YY_BUFFER_STATE state; +}; + +struct buffer *current_buf; + +static int last_ts, first_ts; + +static void zconf_endhelp(void); +static void zconf_endfile(void); + +static void new_string(void) +{ + text = xmalloc(START_STRSIZE); + text_asize = START_STRSIZE; + text_size = 0; + *text = 0; +} + +static void append_string(const char *str, int size) +{ + int new_size = text_size + size + 1; + if (new_size > text_asize) { + new_size += START_STRSIZE - 1; + new_size &= -START_STRSIZE; + text = realloc(text, new_size); + text_asize = new_size; + } + memcpy(text + text_size, str, size); + text_size += size; + text[text_size] = 0; +} + +static void alloc_string(const char *str, int size) +{ + text = xmalloc(size + 1); + memcpy(text, str, size); + text[size] = 0; +} + +static void warn_ignored_character(char chr) +{ + fprintf(stderr, + "%s:%d:warning: ignoring unsupported character '%c'\n", + zconf_curname(), zconf_lineno(), chr); +} +%} + +n [A-Za-z0-9_-] + +%% + int str = 0; + int ts, i; + +[ \t]*#.*\n | +[ \t]*\n { + current_file->lineno++; + return T_EOL; +} +[ \t]*#.* + + +[ \t]+ { + BEGIN(COMMAND); +} + +. { + unput(yytext[0]); + BEGIN(COMMAND); +} + + +{ + {n}+ { + const struct kconf_id *id = kconf_id_lookup(yytext, yyleng); + BEGIN(PARAM); + current_pos.file = current_file; + current_pos.lineno = current_file->lineno; + if (id && id->flags & TF_COMMAND) { + zconflval.id = id; + return id->token; + } + alloc_string(yytext, yyleng); + zconflval.string = text; + return T_WORD; + } + . warn_ignored_character(*yytext); + \n { + BEGIN(INITIAL); + current_file->lineno++; + return T_EOL; + } +} + +{ + "&&" return T_AND; + "||" return T_OR; + "(" return T_OPEN_PAREN; + ")" return T_CLOSE_PAREN; + "!" return T_NOT; + "=" return T_EQUAL; + "!=" return T_UNEQUAL; + "<=" return T_LESS_EQUAL; + ">=" return T_GREATER_EQUAL; + "<" return T_LESS; + ">" return T_GREATER; + \"|\' { + str = yytext[0]; + new_string(); + BEGIN(STRING); + } + \n BEGIN(INITIAL); current_file->lineno++; return T_EOL; + ({n}|[/.])+ { + const struct kconf_id *id = kconf_id_lookup(yytext, yyleng); + if (id && id->flags & TF_PARAM) { + zconflval.id = id; + return id->token; + } + alloc_string(yytext, yyleng); + zconflval.string = text; + return T_WORD; + } + #.* /* comment */ + \\\n current_file->lineno++; + [[:blank:]]+ + . warn_ignored_character(*yytext); + <> { + BEGIN(INITIAL); + } +} + +{ + [^'"\\\n]+/\n { + append_string(yytext, yyleng); + zconflval.string = text; + return T_WORD_QUOTE; + } + [^'"\\\n]+ { + append_string(yytext, yyleng); + } + \\.?/\n { + append_string(yytext + 1, yyleng - 1); + zconflval.string = text; + return T_WORD_QUOTE; + } + \\.? { + append_string(yytext + 1, yyleng - 1); + } + \'|\" { + if (str == yytext[0]) { + BEGIN(PARAM); + zconflval.string = text; + return T_WORD_QUOTE; + } else + append_string(yytext, 1); + } + \n { + printf("%s:%d:warning: multi-line strings not supported\n", zconf_curname(), zconf_lineno()); + current_file->lineno++; + BEGIN(INITIAL); + return T_EOL; + } + <> { + BEGIN(INITIAL); + } +} + +{ + [ \t]+ { + ts = 0; + for (i = 0; i < yyleng; i++) { + if (yytext[i] == '\t') + ts = (ts & ~7) + 8; + else + ts++; + } + last_ts = ts; + if (first_ts) { + if (ts < first_ts) { + zconf_endhelp(); + return T_HELPTEXT; + } + ts -= first_ts; + while (ts > 8) { + append_string(" ", 8); + ts -= 8; + } + append_string(" ", ts); + } + } + [ \t]*\n/[^ \t\n] { + current_file->lineno++; + zconf_endhelp(); + return T_HELPTEXT; + } + [ \t]*\n { + current_file->lineno++; + append_string("\n", 1); + } + [^ \t\n].* { + while (yyleng) { + if ((yytext[yyleng-1] != ' ') && (yytext[yyleng-1] != '\t')) + break; + yyleng--; + } + append_string(yytext, yyleng); + if (!first_ts) + first_ts = last_ts; + } + <> { + zconf_endhelp(); + return T_HELPTEXT; + } +} + +<> { + if (current_file) { + zconf_endfile(); + return T_EOL; + } + fclose(yyin); + yyterminate(); +} + +%% +void zconf_starthelp(void) +{ + new_string(); + last_ts = first_ts = 0; + BEGIN(HELP); +} + +static void zconf_endhelp(void) +{ + zconflval.string = text; + BEGIN(INITIAL); +} + + +/* + * Try to open specified file with following names: + * ./name + * $(srctree)/name + * The latter is used when srctree is separate from objtree + * when compiling the kernel. + * Return NULL if file is not found. + */ +FILE *zconf_fopen(const char *name) +{ + char *env, fullname[PATH_MAX+1]; + FILE *f; + + f = fopen(name, "r"); + if (!f && name != NULL && name[0] != '/') { + env = getenv(SRCTREE); + if (env) { + sprintf(fullname, "%s/%s", env, name); + f = fopen(fullname, "r"); + } + } + return f; +} + +void zconf_initscan(const char *name) +{ + yyin = zconf_fopen(name); + if (!yyin) { + printf("can't find file %s\n", name); + exit(1); + } + + current_buf = xmalloc(sizeof(*current_buf)); + memset(current_buf, 0, sizeof(*current_buf)); + + current_file = file_lookup(name); + current_file->lineno = 1; +} + +void zconf_nextfile(const char *name) +{ + struct file *iter; + struct file *file = file_lookup(name); + struct buffer *buf = xmalloc(sizeof(*buf)); + memset(buf, 0, sizeof(*buf)); + + current_buf->state = YY_CURRENT_BUFFER; + yyin = zconf_fopen(file->name); + if (!yyin) { + printf("%s:%d: can't open file \"%s\"\n", + zconf_curname(), zconf_lineno(), file->name); + exit(1); + } + yy_switch_to_buffer(yy_create_buffer(yyin, YY_BUF_SIZE)); + buf->parent = current_buf; + current_buf = buf; + + for (iter = current_file->parent; iter; iter = iter->parent ) { + if (!strcmp(current_file->name,iter->name) ) { + printf("%s:%d: recursive inclusion detected. " + "Inclusion path:\n current file : '%s'\n", + zconf_curname(), zconf_lineno(), + zconf_curname()); + iter = current_file->parent; + while (iter && \ + strcmp(iter->name,current_file->name)) { + printf(" included from: '%s:%d'\n", + iter->name, iter->lineno-1); + iter = iter->parent; + } + if (iter) + printf(" included from: '%s:%d'\n", + iter->name, iter->lineno+1); + exit(1); + } + } + file->lineno = 1; + file->parent = current_file; + current_file = file; +} + +static void zconf_endfile(void) +{ + struct buffer *parent; + + current_file = current_file->parent; + + parent = current_buf->parent; + if (parent) { + fclose(yyin); + yy_delete_buffer(YY_CURRENT_BUFFER); + yy_switch_to_buffer(parent->state); + } + free(current_buf); + current_buf = parent; +} + +int zconf_lineno(void) +{ + return current_pos.lineno; +} + +const char *zconf_curname(void) +{ + return current_pos.file ? current_pos.file->name : ""; +} diff --git a/scripts/kconfig/zconf.lex.c_shipped b/scripts/kconfig/zconf.lex.c_shipped new file mode 100644 index 0000000000..37fdf61235 --- /dev/null +++ b/scripts/kconfig/zconf.lex.c_shipped @@ -0,0 +1,2473 @@ + +#line 3 "scripts/kconfig/zconf.lex.c_shipped" + +#define YY_INT_ALIGNED short int + +/* A lexical scanner generated by flex */ + +#define yy_create_buffer zconf_create_buffer +#define yy_delete_buffer zconf_delete_buffer +#define yy_flex_debug zconf_flex_debug +#define yy_init_buffer zconf_init_buffer +#define yy_flush_buffer zconf_flush_buffer +#define yy_load_buffer_state zconf_load_buffer_state +#define yy_switch_to_buffer zconf_switch_to_buffer +#define yyin zconfin +#define yyleng zconfleng +#define yylex zconflex +#define yylineno zconflineno +#define yyout zconfout +#define yyrestart zconfrestart +#define yytext zconftext +#define yywrap zconfwrap +#define yyalloc zconfalloc +#define yyrealloc zconfrealloc +#define yyfree zconffree + +#define FLEX_SCANNER +#define YY_FLEX_MAJOR_VERSION 2 +#define YY_FLEX_MINOR_VERSION 5 +#define YY_FLEX_SUBMINOR_VERSION 35 +#if YY_FLEX_SUBMINOR_VERSION > 0 +#define FLEX_BETA +#endif + +/* First, we deal with platform-specific or compiler-specific issues. */ + +/* begin standard C headers. */ +#include +#include +#include +#include + +/* end standard C headers. */ + +/* flex integer type definitions */ + +#ifndef FLEXINT_H +#define FLEXINT_H + +/* C99 systems have . Non-C99 systems may or may not. */ + +#if defined (__STDC_VERSION__) && __STDC_VERSION__ >= 199901L + +/* C99 says to define __STDC_LIMIT_MACROS before including stdint.h, + * if you want the limit (max/min) macros for int types. + */ +#ifndef __STDC_LIMIT_MACROS +#define __STDC_LIMIT_MACROS 1 +#endif + +#include +typedef int8_t flex_int8_t; +typedef uint8_t flex_uint8_t; +typedef int16_t flex_int16_t; +typedef uint16_t flex_uint16_t; +typedef int32_t flex_int32_t; +typedef uint32_t flex_uint32_t; +#else +typedef signed char flex_int8_t; +typedef short int flex_int16_t; +typedef int flex_int32_t; +typedef unsigned char flex_uint8_t; +typedef unsigned short int flex_uint16_t; +typedef unsigned int flex_uint32_t; + +/* Limits of integral types. */ +#ifndef INT8_MIN +#define INT8_MIN (-128) +#endif +#ifndef INT16_MIN +#define INT16_MIN (-32767-1) +#endif +#ifndef INT32_MIN +#define INT32_MIN (-2147483647-1) +#endif +#ifndef INT8_MAX +#define INT8_MAX (127) +#endif +#ifndef INT16_MAX +#define INT16_MAX (32767) +#endif +#ifndef INT32_MAX +#define INT32_MAX (2147483647) +#endif +#ifndef UINT8_MAX +#define UINT8_MAX (255U) +#endif +#ifndef UINT16_MAX +#define UINT16_MAX (65535U) +#endif +#ifndef UINT32_MAX +#define UINT32_MAX (4294967295U) +#endif + +#endif /* ! C99 */ + +#endif /* ! FLEXINT_H */ + +#ifdef __cplusplus + +/* The "const" storage-class-modifier is valid. */ +#define YY_USE_CONST + +#else /* ! __cplusplus */ + +/* C99 requires __STDC__ to be defined as 1. */ +#if defined (__STDC__) + +#define YY_USE_CONST + +#endif /* defined (__STDC__) */ +#endif /* ! __cplusplus */ + +#ifdef YY_USE_CONST +#define yyconst const +#else +#define yyconst +#endif + +/* Returned upon end-of-file. */ +#define YY_NULL 0 + +/* Promotes a possibly negative, possibly signed char to an unsigned + * integer for use as an array index. If the signed char is negative, + * we want to instead treat it as an 8-bit unsigned char, hence the + * double cast. + */ +#define YY_SC_TO_UI(c) ((unsigned int) (unsigned char) c) + +/* Enter a start condition. This macro really ought to take a parameter, + * but we do it the disgusting crufty way forced on us by the ()-less + * definition of BEGIN. + */ +#define BEGIN (yy_start) = 1 + 2 * + +/* Translate the current start state into a value that can be later handed + * to BEGIN to return to the state. The YYSTATE alias is for lex + * compatibility. + */ +#define YY_START (((yy_start) - 1) / 2) +#define YYSTATE YY_START + +/* Action number for EOF rule of a given start state. */ +#define YY_STATE_EOF(state) (YY_END_OF_BUFFER + state + 1) + +/* Special action meaning "start processing a new file". */ +#define YY_NEW_FILE zconfrestart(zconfin ) + +#define YY_END_OF_BUFFER_CHAR 0 + +/* Size of default input buffer. */ +#ifndef YY_BUF_SIZE +#ifdef __ia64__ +/* On IA-64, the buffer size is 16k, not 8k. + * Moreover, YY_BUF_SIZE is 2*YY_READ_BUF_SIZE in the general case. + * Ditto for the __ia64__ case accordingly. + */ +#define YY_BUF_SIZE 32768 +#else +#define YY_BUF_SIZE 16384 +#endif /* __ia64__ */ +#endif + +/* The state buf must be large enough to hold one state per character in the main buffer. + */ +#define YY_STATE_BUF_SIZE ((YY_BUF_SIZE + 2) * sizeof(yy_state_type)) + +#ifndef YY_TYPEDEF_YY_BUFFER_STATE +#define YY_TYPEDEF_YY_BUFFER_STATE +typedef struct yy_buffer_state *YY_BUFFER_STATE; +#endif + +extern int zconfleng; + +extern FILE *zconfin, *zconfout; + +#define EOB_ACT_CONTINUE_SCAN 0 +#define EOB_ACT_END_OF_FILE 1 +#define EOB_ACT_LAST_MATCH 2 + + #define YY_LESS_LINENO(n) + +/* Return all but the first "n" matched characters back to the input stream. */ +#define yyless(n) \ + do \ + { \ + /* Undo effects of setting up zconftext. */ \ + int yyless_macro_arg = (n); \ + YY_LESS_LINENO(yyless_macro_arg);\ + *yy_cp = (yy_hold_char); \ + YY_RESTORE_YY_MORE_OFFSET \ + (yy_c_buf_p) = yy_cp = yy_bp + yyless_macro_arg - YY_MORE_ADJ; \ + YY_DO_BEFORE_ACTION; /* set up zconftext again */ \ + } \ + while ( 0 ) + +#define unput(c) yyunput( c, (yytext_ptr) ) + +#ifndef YY_TYPEDEF_YY_SIZE_T +#define YY_TYPEDEF_YY_SIZE_T +typedef size_t yy_size_t; +#endif + +#ifndef YY_STRUCT_YY_BUFFER_STATE +#define YY_STRUCT_YY_BUFFER_STATE +struct yy_buffer_state + { + FILE *yy_input_file; + + char *yy_ch_buf; /* input buffer */ + char *yy_buf_pos; /* current position in input buffer */ + + /* Size of input buffer in bytes, not including room for EOB + * characters. + */ + yy_size_t yy_buf_size; + + /* Number of characters read into yy_ch_buf, not including EOB + * characters. + */ + int yy_n_chars; + + /* Whether we "own" the buffer - i.e., we know we created it, + * and can realloc() it to grow it, and should free() it to + * delete it. + */ + int yy_is_our_buffer; + + /* Whether this is an "interactive" input source; if so, and + * if we're using stdio for input, then we want to use getc() + * instead of fread(), to make sure we stop fetching input after + * each newline. + */ + int yy_is_interactive; + + /* Whether we're considered to be at the beginning of a line. + * If so, '^' rules will be active on the next match, otherwise + * not. + */ + int yy_at_bol; + + int yy_bs_lineno; /**< The line count. */ + int yy_bs_column; /**< The column count. */ + + /* Whether to try to fill the input buffer when we reach the + * end of it. + */ + int yy_fill_buffer; + + int yy_buffer_status; + +#define YY_BUFFER_NEW 0 +#define YY_BUFFER_NORMAL 1 + /* When an EOF's been seen but there's still some text to process + * then we mark the buffer as YY_EOF_PENDING, to indicate that we + * shouldn't try reading from the input source any more. We might + * still have a bunch of tokens to match, though, because of + * possible backing-up. + * + * When we actually see the EOF, we change the status to "new" + * (via zconfrestart()), so that the user can continue scanning by + * just pointing zconfin at a new input file. + */ +#define YY_BUFFER_EOF_PENDING 2 + + }; +#endif /* !YY_STRUCT_YY_BUFFER_STATE */ + +/* Stack of input buffers. */ +static size_t yy_buffer_stack_top = 0; /**< index of top of stack. */ +static size_t yy_buffer_stack_max = 0; /**< capacity of stack. */ +static YY_BUFFER_STATE * yy_buffer_stack = 0; /**< Stack as an array. */ + +/* We provide macros for accessing buffer states in case in the + * future we want to put the buffer states in a more general + * "scanner state". + * + * Returns the top of the stack, or NULL. + */ +#define YY_CURRENT_BUFFER ( (yy_buffer_stack) \ + ? (yy_buffer_stack)[(yy_buffer_stack_top)] \ + : NULL) + +/* Same as previous macro, but useful when we know that the buffer stack is not + * NULL or when we need an lvalue. For internal use only. + */ +#define YY_CURRENT_BUFFER_LVALUE (yy_buffer_stack)[(yy_buffer_stack_top)] + +/* yy_hold_char holds the character lost when zconftext is formed. */ +static char yy_hold_char; +static int yy_n_chars; /* number of characters read into yy_ch_buf */ +int zconfleng; + +/* Points to current character in buffer. */ +static char *yy_c_buf_p = (char *) 0; +static int yy_init = 0; /* whether we need to initialize */ +static int yy_start = 0; /* start state number */ + +/* Flag which is used to allow zconfwrap()'s to do buffer switches + * instead of setting up a fresh zconfin. A bit of a hack ... + */ +static int yy_did_buffer_switch_on_eof; + +void zconfrestart (FILE *input_file ); +void zconf_switch_to_buffer (YY_BUFFER_STATE new_buffer ); +YY_BUFFER_STATE zconf_create_buffer (FILE *file,int size ); +void zconf_delete_buffer (YY_BUFFER_STATE b ); +void zconf_flush_buffer (YY_BUFFER_STATE b ); +void zconfpush_buffer_state (YY_BUFFER_STATE new_buffer ); +void zconfpop_buffer_state (void ); + +static void zconfensure_buffer_stack (void ); +static void zconf_load_buffer_state (void ); +static void zconf_init_buffer (YY_BUFFER_STATE b,FILE *file ); + +#define YY_FLUSH_BUFFER zconf_flush_buffer(YY_CURRENT_BUFFER ) + +YY_BUFFER_STATE zconf_scan_buffer (char *base,yy_size_t size ); +YY_BUFFER_STATE zconf_scan_string (yyconst char *yy_str ); +YY_BUFFER_STATE zconf_scan_bytes (yyconst char *bytes,int len ); + +void *zconfalloc (yy_size_t ); +void *zconfrealloc (void *,yy_size_t ); +void zconffree (void * ); + +#define yy_new_buffer zconf_create_buffer + +#define yy_set_interactive(is_interactive) \ + { \ + if ( ! YY_CURRENT_BUFFER ){ \ + zconfensure_buffer_stack (); \ + YY_CURRENT_BUFFER_LVALUE = \ + zconf_create_buffer(zconfin,YY_BUF_SIZE ); \ + } \ + YY_CURRENT_BUFFER_LVALUE->yy_is_interactive = is_interactive; \ + } + +#define yy_set_bol(at_bol) \ + { \ + if ( ! YY_CURRENT_BUFFER ){\ + zconfensure_buffer_stack (); \ + YY_CURRENT_BUFFER_LVALUE = \ + zconf_create_buffer(zconfin,YY_BUF_SIZE ); \ + } \ + YY_CURRENT_BUFFER_LVALUE->yy_at_bol = at_bol; \ + } + +#define YY_AT_BOL() (YY_CURRENT_BUFFER_LVALUE->yy_at_bol) + +/* Begin user sect3 */ + +#define zconfwrap(n) 1 +#define YY_SKIP_YYWRAP + +typedef unsigned char YY_CHAR; + +FILE *zconfin = (FILE *) 0, *zconfout = (FILE *) 0; + +typedef int yy_state_type; + +extern int zconflineno; + +int zconflineno = 1; + +extern char *zconftext; +#define yytext_ptr zconftext +static yyconst flex_int16_t yy_nxt[][18] = + { + { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0 + }, + + { + 11, 12, 13, 14, 12, 12, 15, 12, 12, 12, + 12, 12, 12, 12, 12, 12, 12, 12 + }, + + { + 11, 12, 13, 14, 12, 12, 15, 12, 12, 12, + 12, 12, 12, 12, 12, 12, 12, 12 + }, + + { + 11, 16, 16, 17, 16, 16, 16, 16, 16, 16, + 16, 18, 16, 16, 16, 16, 16, 16 + }, + + { + 11, 16, 16, 17, 16, 16, 16, 16, 16, 16, + 16, 18, 16, 16, 16, 16, 16, 16 + + }, + + { + 11, 19, 20, 21, 19, 19, 19, 19, 19, 19, + 19, 19, 19, 19, 19, 19, 19, 19 + }, + + { + 11, 19, 20, 21, 19, 19, 19, 19, 19, 19, + 19, 19, 19, 19, 19, 19, 19, 19 + }, + + { + 11, 22, 22, 23, 22, 24, 22, 22, 24, 22, + 22, 22, 22, 22, 22, 22, 25, 22 + }, + + { + 11, 22, 22, 23, 22, 24, 22, 22, 24, 22, + 22, 22, 22, 22, 22, 22, 25, 22 + }, + + { + 11, 26, 27, 28, 29, 30, 31, 32, 30, 33, + 34, 35, 35, 36, 37, 38, 39, 40 + + }, + + { + 11, 26, 27, 28, 29, 30, 31, 32, 30, 33, + 34, 35, 35, 36, 37, 38, 39, 40 + }, + + { + -11, -11, -11, -11, -11, -11, -11, -11, -11, -11, + -11, -11, -11, -11, -11, -11, -11, -11 + }, + + { + 11, -12, -12, -12, -12, -12, -12, -12, -12, -12, + -12, -12, -12, -12, -12, -12, -12, -12 + }, + + { + 11, -13, 41, 42, -13, -13, 43, -13, -13, -13, + -13, -13, -13, -13, -13, -13, -13, -13 + }, + + { + 11, -14, -14, -14, -14, -14, -14, -14, -14, -14, + -14, -14, -14, -14, -14, -14, -14, -14 + + }, + + { + 11, 44, 44, 45, 44, 44, 44, 44, 44, 44, + 44, 44, 44, 44, 44, 44, 44, 44 + }, + + { + 11, -16, -16, -16, -16, -16, -16, -16, -16, -16, + -16, -16, -16, -16, -16, -16, -16, -16 + }, + + { + 11, -17, -17, -17, -17, -17, -17, -17, -17, -17, + -17, -17, -17, -17, -17, -17, -17, -17 + }, + + { + 11, -18, -18, -18, -18, -18, -18, -18, -18, -18, + -18, 46, -18, -18, -18, -18, -18, -18 + }, + + { + 11, 47, 47, -19, 47, 47, 47, 47, 47, 47, + 47, 47, 47, 47, 47, 47, 47, 47 + + }, + + { + 11, -20, 48, 49, -20, -20, -20, -20, -20, -20, + -20, -20, -20, -20, -20, -20, -20, -20 + }, + + { + 11, 50, -21, -21, 50, 50, 50, 50, 50, 50, + 50, 50, 50, 50, 50, 50, 50, 50 + }, + + { + 11, 51, 51, 52, 51, -22, 51, 51, -22, 51, + 51, 51, 51, 51, 51, 51, -22, 51 + }, + + { + 11, -23, -23, -23, -23, -23, -23, -23, -23, -23, + -23, -23, -23, -23, -23, -23, -23, -23 + }, + + { + 11, -24, -24, -24, -24, -24, -24, -24, -24, -24, + -24, -24, -24, -24, -24, -24, -24, -24 + + }, + + { + 11, 53, 53, 54, 53, 53, 53, 53, 53, 53, + 53, 53, 53, 53, 53, 53, 53, 53 + }, + + { + 11, -26, -26, -26, -26, -26, -26, -26, -26, -26, + -26, -26, -26, -26, -26, -26, -26, -26 + }, + + { + 11, -27, 55, -27, -27, -27, -27, -27, -27, -27, + -27, -27, -27, -27, -27, -27, -27, -27 + }, + + { + 11, -28, -28, -28, -28, -28, -28, -28, -28, -28, + -28, -28, -28, -28, -28, -28, -28, -28 + }, + + { + 11, -29, -29, -29, -29, -29, -29, -29, -29, -29, + -29, -29, -29, -29, 56, -29, -29, -29 + + }, + + { + 11, -30, -30, -30, -30, -30, -30, -30, -30, -30, + -30, -30, -30, -30, -30, -30, -30, -30 + }, + + { + 11, 57, 57, -31, 57, 57, 57, 57, 57, 57, + 57, 57, 57, 57, 57, 57, 57, 57 + }, + + { + 11, -32, -32, -32, -32, -32, -32, 58, -32, -32, + -32, -32, -32, -32, -32, -32, -32, -32 + }, + + { + 11, -33, -33, -33, -33, -33, -33, -33, -33, -33, + -33, -33, -33, -33, -33, -33, -33, -33 + }, + + { + 11, -34, -34, -34, -34, -34, -34, -34, -34, -34, + -34, -34, -34, -34, -34, -34, -34, -34 + + }, + + { + 11, -35, -35, -35, -35, -35, -35, -35, -35, -35, + -35, 59, 59, -35, -35, -35, -35, -35 + }, + + { + 11, -36, -36, -36, -36, -36, -36, -36, -36, -36, + -36, -36, -36, -36, 60, -36, -36, -36 + }, + + { + 11, -37, -37, -37, -37, -37, -37, -37, -37, -37, + -37, -37, -37, -37, -37, -37, -37, -37 + }, + + { + 11, -38, -38, -38, -38, -38, -38, -38, -38, -38, + -38, -38, -38, -38, 61, -38, -38, -38 + }, + + { + 11, -39, -39, 62, -39, -39, -39, -39, -39, -39, + -39, -39, -39, -39, -39, -39, -39, -39 + + }, + + { + 11, -40, -40, -40, -40, -40, -40, -40, -40, -40, + -40, -40, -40, -40, -40, -40, -40, 63 + }, + + { + 11, -41, 41, 42, -41, -41, 43, -41, -41, -41, + -41, -41, -41, -41, -41, -41, -41, -41 + }, + + { + 11, -42, -42, -42, -42, -42, -42, -42, -42, -42, + -42, -42, -42, -42, -42, -42, -42, -42 + }, + + { + 11, 44, 44, 45, 44, 44, 44, 44, 44, 44, + 44, 44, 44, 44, 44, 44, 44, 44 + }, + + { + 11, 44, 44, 45, 44, 44, 44, 44, 44, 44, + 44, 44, 44, 44, 44, 44, 44, 44 + + }, + + { + 11, -45, -45, -45, -45, -45, -45, -45, -45, -45, + -45, -45, -45, -45, -45, -45, -45, -45 + }, + + { + 11, -46, -46, -46, -46, -46, -46, -46, -46, -46, + -46, 46, -46, -46, -46, -46, -46, -46 + }, + + { + 11, 47, 47, -47, 47, 47, 47, 47, 47, 47, + 47, 47, 47, 47, 47, 47, 47, 47 + }, + + { + 11, -48, 48, 49, -48, -48, -48, -48, -48, -48, + -48, -48, -48, -48, -48, -48, -48, -48 + }, + + { + 11, 50, -49, -49, 50, 50, 50, 50, 50, 50, + 50, 50, 50, 50, 50, 50, 50, 50 + + }, + + { + 11, -50, -50, -50, -50, -50, -50, -50, -50, -50, + -50, -50, -50, -50, -50, -50, -50, -50 + }, + + { + 11, 51, 51, 52, 51, -51, 51, 51, -51, 51, + 51, 51, 51, 51, 51, 51, -51, 51 + }, + + { + 11, -52, -52, -52, -52, -52, -52, -52, -52, -52, + -52, -52, -52, -52, -52, -52, -52, -52 + }, + + { + 11, -53, -53, 54, -53, -53, -53, -53, -53, -53, + -53, -53, -53, -53, -53, -53, -53, -53 + }, + + { + 11, -54, -54, -54, -54, -54, -54, -54, -54, -54, + -54, -54, -54, -54, -54, -54, -54, -54 + + }, + + { + 11, -55, 55, -55, -55, -55, -55, -55, -55, -55, + -55, -55, -55, -55, -55, -55, -55, -55 + }, + + { + 11, -56, -56, -56, -56, -56, -56, -56, -56, -56, + -56, -56, -56, -56, -56, -56, -56, -56 + }, + + { + 11, 57, 57, -57, 57, 57, 57, 57, 57, 57, + 57, 57, 57, 57, 57, 57, 57, 57 + }, + + { + 11, -58, -58, -58, -58, -58, -58, -58, -58, -58, + -58, -58, -58, -58, -58, -58, -58, -58 + }, + + { + 11, -59, -59, -59, -59, -59, -59, -59, -59, -59, + -59, 59, 59, -59, -59, -59, -59, -59 + + }, + + { + 11, -60, -60, -60, -60, -60, -60, -60, -60, -60, + -60, -60, -60, -60, -60, -60, -60, -60 + }, + + { + 11, -61, -61, -61, -61, -61, -61, -61, -61, -61, + -61, -61, -61, -61, -61, -61, -61, -61 + }, + + { + 11, -62, -62, -62, -62, -62, -62, -62, -62, -62, + -62, -62, -62, -62, -62, -62, -62, -62 + }, + + { + 11, -63, -63, -63, -63, -63, -63, -63, -63, -63, + -63, -63, -63, -63, -63, -63, -63, -63 + }, + + } ; + +static yy_state_type yy_get_previous_state (void ); +static yy_state_type yy_try_NUL_trans (yy_state_type current_state ); +static int yy_get_next_buffer (void ); +static void yy_fatal_error (yyconst char msg[] ); + +/* Done after the current pattern has been matched and before the + * corresponding action - sets up zconftext. + */ +#define YY_DO_BEFORE_ACTION \ + (yytext_ptr) = yy_bp; \ + zconfleng = (size_t) (yy_cp - yy_bp); \ + (yy_hold_char) = *yy_cp; \ + *yy_cp = '\0'; \ + (yy_c_buf_p) = yy_cp; + +#define YY_NUM_RULES 37 +#define YY_END_OF_BUFFER 38 +/* This struct is not used in this scanner, + but its presence is necessary. */ +struct yy_trans_info + { + flex_int32_t yy_verify; + flex_int32_t yy_nxt; + }; +static yyconst flex_int16_t yy_accept[64] = + { 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 38, 5, 4, 2, 3, 7, 8, 6, 36, 33, + 35, 28, 32, 31, 30, 26, 25, 21, 13, 20, + 23, 26, 11, 12, 22, 18, 14, 19, 26, 26, + 4, 2, 3, 3, 1, 6, 36, 33, 35, 34, + 28, 27, 30, 29, 25, 15, 23, 9, 22, 16, + 17, 24, 10 + } ; + +static yyconst flex_int32_t yy_ec[256] = + { 0, + 1, 1, 1, 1, 1, 1, 1, 1, 2, 3, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 2, 4, 5, 6, 1, 1, 7, 8, 9, + 10, 1, 1, 1, 11, 12, 12, 11, 11, 11, + 11, 11, 11, 11, 11, 11, 11, 1, 1, 13, + 14, 15, 1, 1, 11, 11, 11, 11, 11, 11, + 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, + 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, + 1, 16, 1, 1, 11, 1, 11, 11, 11, 11, + + 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, + 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, + 11, 11, 1, 17, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1 + } ; + +extern int zconf_flex_debug; +int zconf_flex_debug = 0; + +/* The intent behind this definition is that it'll catch + * any uses of REJECT which flex missed. + */ +#define REJECT reject_used_but_not_detected +#define yymore() yymore_used_but_not_detected +#define YY_MORE_ADJ 0 +#define YY_RESTORE_YY_MORE_OFFSET +char *zconftext; +#define YY_NO_INPUT 1 + +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include + +#include "lkc.h" + +#define START_STRSIZE 16 + +static struct { + struct file *file; + int lineno; +} current_pos; + +static char *text; +static int text_size, text_asize; + +struct buffer { + struct buffer *parent; + YY_BUFFER_STATE state; +}; + +struct buffer *current_buf; + +static int last_ts, first_ts; + +static void zconf_endhelp(void); +static void zconf_endfile(void); + +static void new_string(void) +{ + text = xmalloc(START_STRSIZE); + text_asize = START_STRSIZE; + text_size = 0; + *text = 0; +} + +static void append_string(const char *str, int size) +{ + int new_size = text_size + size + 1; + if (new_size > text_asize) { + new_size += START_STRSIZE - 1; + new_size &= -START_STRSIZE; + text = realloc(text, new_size); + text_asize = new_size; + } + memcpy(text + text_size, str, size); + text_size += size; + text[text_size] = 0; +} + +static void alloc_string(const char *str, int size) +{ + text = xmalloc(size + 1); + memcpy(text, str, size); + text[size] = 0; +} + +static void warn_ignored_character(char chr) +{ + fprintf(stderr, + "%s:%d:warning: ignoring unsupported character '%c'\n", + zconf_curname(), zconf_lineno(), chr); +} + +#define INITIAL 0 +#define COMMAND 1 +#define HELP 2 +#define STRING 3 +#define PARAM 4 + +#ifndef YY_NO_UNISTD_H +/* Special case for "unistd.h", since it is non-ANSI. We include it way + * down here because we want the user's section 1 to have been scanned first. + * The user has a chance to override it with an option. + */ +#include +#endif + +#ifndef YY_EXTRA_TYPE +#define YY_EXTRA_TYPE void * +#endif + +static int yy_init_globals (void ); + +/* Accessor methods to globals. + These are made visible to non-reentrant scanners for convenience. */ + +int zconflex_destroy (void ); + +int zconfget_debug (void ); + +void zconfset_debug (int debug_flag ); + +YY_EXTRA_TYPE zconfget_extra (void ); + +void zconfset_extra (YY_EXTRA_TYPE user_defined ); + +FILE *zconfget_in (void ); + +void zconfset_in (FILE * in_str ); + +FILE *zconfget_out (void ); + +void zconfset_out (FILE * out_str ); + +int zconfget_leng (void ); + +char *zconfget_text (void ); + +int zconfget_lineno (void ); + +void zconfset_lineno (int line_number ); + +/* Macros after this point can all be overridden by user definitions in + * section 1. + */ + +#ifndef YY_SKIP_YYWRAP +#ifdef __cplusplus +extern "C" int zconfwrap (void ); +#else +extern int zconfwrap (void ); +#endif +#endif + + static void yyunput (int c,char *buf_ptr ); + +#ifndef yytext_ptr +static void yy_flex_strncpy (char *,yyconst char *,int ); +#endif + +#ifdef YY_NEED_STRLEN +static int yy_flex_strlen (yyconst char * ); +#endif + +#ifndef YY_NO_INPUT + +#ifdef __cplusplus +static int yyinput (void ); +#else +static int input (void ); +#endif + +#endif + +/* Amount of stuff to slurp up with each read. */ +#ifndef YY_READ_BUF_SIZE +#ifdef __ia64__ +/* On IA-64, the buffer size is 16k, not 8k */ +#define YY_READ_BUF_SIZE 16384 +#else +#define YY_READ_BUF_SIZE 8192 +#endif /* __ia64__ */ +#endif + +/* Copy whatever the last rule matched to the standard output. */ +#ifndef ECHO +/* This used to be an fputs(), but since the string might contain NUL's, + * we now use fwrite(). + */ +#define ECHO do { if (fwrite( zconftext, zconfleng, 1, zconfout )) {} } while (0) +#endif + +/* Gets input and stuffs it into "buf". number of characters read, or YY_NULL, + * is returned in "result". + */ +#ifndef YY_INPUT +#define YY_INPUT(buf,result,max_size) \ + errno=0; \ + while ( (result = read( fileno(zconfin), (char *) buf, max_size )) < 0 ) \ + { \ + if( errno != EINTR) \ + { \ + YY_FATAL_ERROR( "input in flex scanner failed" ); \ + break; \ + } \ + errno=0; \ + clearerr(zconfin); \ + }\ +\ + +#endif + +/* No semi-colon after return; correct usage is to write "yyterminate();" - + * we don't want an extra ';' after the "return" because that will cause + * some compilers to complain about unreachable statements. + */ +#ifndef yyterminate +#define yyterminate() return YY_NULL +#endif + +/* Number of entries by which start-condition stack grows. */ +#ifndef YY_START_STACK_INCR +#define YY_START_STACK_INCR 25 +#endif + +/* Report a fatal error. */ +#ifndef YY_FATAL_ERROR +#define YY_FATAL_ERROR(msg) yy_fatal_error( msg ) +#endif + +/* end tables serialization structures and prototypes */ + +/* Default declaration of generated scanner - a define so the user can + * easily add parameters. + */ +#ifndef YY_DECL +#define YY_DECL_IS_OURS 1 + +extern int zconflex (void); + +#define YY_DECL int zconflex (void) +#endif /* !YY_DECL */ + +/* Code executed at the beginning of each rule, after zconftext and zconfleng + * have been set up. + */ +#ifndef YY_USER_ACTION +#define YY_USER_ACTION +#endif + +/* Code executed at the end of each rule. */ +#ifndef YY_BREAK +#define YY_BREAK break; +#endif + +#define YY_RULE_SETUP \ + YY_USER_ACTION + +/** The main scanner function which does all the work. + */ +YY_DECL +{ + register yy_state_type yy_current_state; + register char *yy_cp, *yy_bp; + register int yy_act; + + int str = 0; + int ts, i; + + if ( !(yy_init) ) + { + (yy_init) = 1; + +#ifdef YY_USER_INIT + YY_USER_INIT; +#endif + + if ( ! (yy_start) ) + (yy_start) = 1; /* first start state */ + + if ( ! zconfin ) + zconfin = stdin; + + if ( ! zconfout ) + zconfout = stdout; + + if ( ! YY_CURRENT_BUFFER ) { + zconfensure_buffer_stack (); + YY_CURRENT_BUFFER_LVALUE = + zconf_create_buffer(zconfin,YY_BUF_SIZE ); + } + + zconf_load_buffer_state( ); + } + + while ( 1 ) /* loops until end-of-file is reached */ + { + yy_cp = (yy_c_buf_p); + + /* Support of zconftext. */ + *yy_cp = (yy_hold_char); + + /* yy_bp points to the position in yy_ch_buf of the start of + * the current run. + */ + yy_bp = yy_cp; + + yy_current_state = (yy_start); +yy_match: + while ( (yy_current_state = yy_nxt[yy_current_state][ yy_ec[YY_SC_TO_UI(*yy_cp)] ]) > 0 ) + ++yy_cp; + + yy_current_state = -yy_current_state; + +yy_find_action: + yy_act = yy_accept[yy_current_state]; + + YY_DO_BEFORE_ACTION; + +do_action: /* This label is used only to access EOF actions. */ + + switch ( yy_act ) + { /* beginning of action switch */ +case 1: +/* rule 1 can match eol */ +case 2: +/* rule 2 can match eol */ +YY_RULE_SETUP +{ + current_file->lineno++; + return T_EOL; +} + YY_BREAK +case 3: +YY_RULE_SETUP + + YY_BREAK +case 4: +YY_RULE_SETUP +{ + BEGIN(COMMAND); +} + YY_BREAK +case 5: +YY_RULE_SETUP +{ + unput(zconftext[0]); + BEGIN(COMMAND); +} + YY_BREAK + +case 6: +YY_RULE_SETUP +{ + const struct kconf_id *id = kconf_id_lookup(zconftext, zconfleng); + BEGIN(PARAM); + current_pos.file = current_file; + current_pos.lineno = current_file->lineno; + if (id && id->flags & TF_COMMAND) { + zconflval.id = id; + return id->token; + } + alloc_string(zconftext, zconfleng); + zconflval.string = text; + return T_WORD; + } + YY_BREAK +case 7: +YY_RULE_SETUP +warn_ignored_character(*zconftext); + YY_BREAK +case 8: +/* rule 8 can match eol */ +YY_RULE_SETUP +{ + BEGIN(INITIAL); + current_file->lineno++; + return T_EOL; + } + YY_BREAK + +case 9: +YY_RULE_SETUP +return T_AND; + YY_BREAK +case 10: +YY_RULE_SETUP +return T_OR; + YY_BREAK +case 11: +YY_RULE_SETUP +return T_OPEN_PAREN; + YY_BREAK +case 12: +YY_RULE_SETUP +return T_CLOSE_PAREN; + YY_BREAK +case 13: +YY_RULE_SETUP +return T_NOT; + YY_BREAK +case 14: +YY_RULE_SETUP +return T_EQUAL; + YY_BREAK +case 15: +YY_RULE_SETUP +return T_UNEQUAL; + YY_BREAK +case 16: +YY_RULE_SETUP +return T_LESS_EQUAL; + YY_BREAK +case 17: +YY_RULE_SETUP +return T_GREATER_EQUAL; + YY_BREAK +case 18: +YY_RULE_SETUP +return T_LESS; + YY_BREAK +case 19: +YY_RULE_SETUP +return T_GREATER; + YY_BREAK +case 20: +YY_RULE_SETUP +{ + str = zconftext[0]; + new_string(); + BEGIN(STRING); + } + YY_BREAK +case 21: +/* rule 21 can match eol */ +YY_RULE_SETUP +BEGIN(INITIAL); current_file->lineno++; return T_EOL; + YY_BREAK +case 22: +YY_RULE_SETUP +{ + const struct kconf_id *id = kconf_id_lookup(zconftext, zconfleng); + if (id && id->flags & TF_PARAM) { + zconflval.id = id; + return id->token; + } + alloc_string(zconftext, zconfleng); + zconflval.string = text; + return T_WORD; + } + YY_BREAK +case 23: +YY_RULE_SETUP +/* comment */ + YY_BREAK +case 24: +/* rule 24 can match eol */ +YY_RULE_SETUP +current_file->lineno++; + YY_BREAK +case 25: +YY_RULE_SETUP + + YY_BREAK +case 26: +YY_RULE_SETUP +warn_ignored_character(*zconftext); + YY_BREAK +case YY_STATE_EOF(PARAM): +{ + BEGIN(INITIAL); + } + YY_BREAK + +case 27: +/* rule 27 can match eol */ +*yy_cp = (yy_hold_char); /* undo effects of setting up zconftext */ +(yy_c_buf_p) = yy_cp -= 1; +YY_DO_BEFORE_ACTION; /* set up zconftext again */ +YY_RULE_SETUP +{ + append_string(zconftext, zconfleng); + zconflval.string = text; + return T_WORD_QUOTE; + } + YY_BREAK +case 28: +YY_RULE_SETUP +{ + append_string(zconftext, zconfleng); + } + YY_BREAK +case 29: +/* rule 29 can match eol */ +*yy_cp = (yy_hold_char); /* undo effects of setting up zconftext */ +(yy_c_buf_p) = yy_cp -= 1; +YY_DO_BEFORE_ACTION; /* set up zconftext again */ +YY_RULE_SETUP +{ + append_string(zconftext + 1, zconfleng - 1); + zconflval.string = text; + return T_WORD_QUOTE; + } + YY_BREAK +case 30: +YY_RULE_SETUP +{ + append_string(zconftext + 1, zconfleng - 1); + } + YY_BREAK +case 31: +YY_RULE_SETUP +{ + if (str == zconftext[0]) { + BEGIN(PARAM); + zconflval.string = text; + return T_WORD_QUOTE; + } else + append_string(zconftext, 1); + } + YY_BREAK +case 32: +/* rule 32 can match eol */ +YY_RULE_SETUP +{ + printf("%s:%d:warning: multi-line strings not supported\n", zconf_curname(), zconf_lineno()); + current_file->lineno++; + BEGIN(INITIAL); + return T_EOL; + } + YY_BREAK +case YY_STATE_EOF(STRING): +{ + BEGIN(INITIAL); + } + YY_BREAK + +case 33: +YY_RULE_SETUP +{ + ts = 0; + for (i = 0; i < zconfleng; i++) { + if (zconftext[i] == '\t') + ts = (ts & ~7) + 8; + else + ts++; + } + last_ts = ts; + if (first_ts) { + if (ts < first_ts) { + zconf_endhelp(); + return T_HELPTEXT; + } + ts -= first_ts; + while (ts > 8) { + append_string(" ", 8); + ts -= 8; + } + append_string(" ", ts); + } + } + YY_BREAK +case 34: +/* rule 34 can match eol */ +*yy_cp = (yy_hold_char); /* undo effects of setting up zconftext */ +(yy_c_buf_p) = yy_cp -= 1; +YY_DO_BEFORE_ACTION; /* set up zconftext again */ +YY_RULE_SETUP +{ + current_file->lineno++; + zconf_endhelp(); + return T_HELPTEXT; + } + YY_BREAK +case 35: +/* rule 35 can match eol */ +YY_RULE_SETUP +{ + current_file->lineno++; + append_string("\n", 1); + } + YY_BREAK +case 36: +YY_RULE_SETUP +{ + while (zconfleng) { + if ((zconftext[zconfleng-1] != ' ') && (zconftext[zconfleng-1] != '\t')) + break; + zconfleng--; + } + append_string(zconftext, zconfleng); + if (!first_ts) + first_ts = last_ts; + } + YY_BREAK +case YY_STATE_EOF(HELP): +{ + zconf_endhelp(); + return T_HELPTEXT; + } + YY_BREAK + +case YY_STATE_EOF(INITIAL): +case YY_STATE_EOF(COMMAND): +{ + if (current_file) { + zconf_endfile(); + return T_EOL; + } + fclose(zconfin); + yyterminate(); +} + YY_BREAK +case 37: +YY_RULE_SETUP +YY_FATAL_ERROR( "flex scanner jammed" ); + YY_BREAK + + case YY_END_OF_BUFFER: + { + /* Amount of text matched not including the EOB char. */ + int yy_amount_of_matched_text = (int) (yy_cp - (yytext_ptr)) - 1; + + /* Undo the effects of YY_DO_BEFORE_ACTION. */ + *yy_cp = (yy_hold_char); + YY_RESTORE_YY_MORE_OFFSET + + if ( YY_CURRENT_BUFFER_LVALUE->yy_buffer_status == YY_BUFFER_NEW ) + { + /* We're scanning a new file or input source. It's + * possible that this happened because the user + * just pointed zconfin at a new source and called + * zconflex(). If so, then we have to assure + * consistency between YY_CURRENT_BUFFER and our + * globals. Here is the right place to do so, because + * this is the first action (other than possibly a + * back-up) that will match for the new input source. + */ + (yy_n_chars) = YY_CURRENT_BUFFER_LVALUE->yy_n_chars; + YY_CURRENT_BUFFER_LVALUE->yy_input_file = zconfin; + YY_CURRENT_BUFFER_LVALUE->yy_buffer_status = YY_BUFFER_NORMAL; + } + + /* Note that here we test for yy_c_buf_p "<=" to the position + * of the first EOB in the buffer, since yy_c_buf_p will + * already have been incremented past the NUL character + * (since all states make transitions on EOB to the + * end-of-buffer state). Contrast this with the test + * in input(). + */ + if ( (yy_c_buf_p) <= &YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[(yy_n_chars)] ) + { /* This was really a NUL. */ + yy_state_type yy_next_state; + + (yy_c_buf_p) = (yytext_ptr) + yy_amount_of_matched_text; + + yy_current_state = yy_get_previous_state( ); + + /* Okay, we're now positioned to make the NUL + * transition. We couldn't have + * yy_get_previous_state() go ahead and do it + * for us because it doesn't know how to deal + * with the possibility of jamming (and we don't + * want to build jamming into it because then it + * will run more slowly). + */ + + yy_next_state = yy_try_NUL_trans( yy_current_state ); + + yy_bp = (yytext_ptr) + YY_MORE_ADJ; + + if ( yy_next_state ) + { + /* Consume the NUL. */ + yy_cp = ++(yy_c_buf_p); + yy_current_state = yy_next_state; + goto yy_match; + } + + else + { + yy_cp = (yy_c_buf_p); + goto yy_find_action; + } + } + + else switch ( yy_get_next_buffer( ) ) + { + case EOB_ACT_END_OF_FILE: + { + (yy_did_buffer_switch_on_eof) = 0; + + if ( zconfwrap( ) ) + { + /* Note: because we've taken care in + * yy_get_next_buffer() to have set up + * zconftext, we can now set up + * yy_c_buf_p so that if some total + * hoser (like flex itself) wants to + * call the scanner after we return the + * YY_NULL, it'll still work - another + * YY_NULL will get returned. + */ + (yy_c_buf_p) = (yytext_ptr) + YY_MORE_ADJ; + + yy_act = YY_STATE_EOF(YY_START); + goto do_action; + } + + else + { + if ( ! (yy_did_buffer_switch_on_eof) ) + YY_NEW_FILE; + } + break; + } + + case EOB_ACT_CONTINUE_SCAN: + (yy_c_buf_p) = + (yytext_ptr) + yy_amount_of_matched_text; + + yy_current_state = yy_get_previous_state( ); + + yy_cp = (yy_c_buf_p); + yy_bp = (yytext_ptr) + YY_MORE_ADJ; + goto yy_match; + + case EOB_ACT_LAST_MATCH: + (yy_c_buf_p) = + &YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[(yy_n_chars)]; + + yy_current_state = yy_get_previous_state( ); + + yy_cp = (yy_c_buf_p); + yy_bp = (yytext_ptr) + YY_MORE_ADJ; + goto yy_find_action; + } + break; + } + + default: + YY_FATAL_ERROR( + "fatal flex scanner internal error--no action found" ); + } /* end of action switch */ + } /* end of scanning one token */ +} /* end of zconflex */ + +/* yy_get_next_buffer - try to read in a new buffer + * + * Returns a code representing an action: + * EOB_ACT_LAST_MATCH - + * EOB_ACT_CONTINUE_SCAN - continue scanning from current position + * EOB_ACT_END_OF_FILE - end of file + */ +static int yy_get_next_buffer (void) +{ + register char *dest = YY_CURRENT_BUFFER_LVALUE->yy_ch_buf; + register char *source = (yytext_ptr); + register int number_to_move, i; + int ret_val; + + if ( (yy_c_buf_p) > &YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[(yy_n_chars) + 1] ) + YY_FATAL_ERROR( + "fatal flex scanner internal error--end of buffer missed" ); + + if ( YY_CURRENT_BUFFER_LVALUE->yy_fill_buffer == 0 ) + { /* Don't try to fill the buffer, so this is an EOF. */ + if ( (yy_c_buf_p) - (yytext_ptr) - YY_MORE_ADJ == 1 ) + { + /* We matched a single character, the EOB, so + * treat this as a final EOF. + */ + return EOB_ACT_END_OF_FILE; + } + + else + { + /* We matched some text prior to the EOB, first + * process it. + */ + return EOB_ACT_LAST_MATCH; + } + } + + /* Try to read more data. */ + + /* First move last chars to start of buffer. */ + number_to_move = (int) ((yy_c_buf_p) - (yytext_ptr)) - 1; + + for ( i = 0; i < number_to_move; ++i ) + *(dest++) = *(source++); + + if ( YY_CURRENT_BUFFER_LVALUE->yy_buffer_status == YY_BUFFER_EOF_PENDING ) + /* don't do the read, it's not guaranteed to return an EOF, + * just force an EOF + */ + YY_CURRENT_BUFFER_LVALUE->yy_n_chars = (yy_n_chars) = 0; + + else + { + int num_to_read = + YY_CURRENT_BUFFER_LVALUE->yy_buf_size - number_to_move - 1; + + while ( num_to_read <= 0 ) + { /* Not enough room in the buffer - grow it. */ + + /* just a shorter name for the current buffer */ + YY_BUFFER_STATE b = YY_CURRENT_BUFFER; + + int yy_c_buf_p_offset = + (int) ((yy_c_buf_p) - b->yy_ch_buf); + + if ( b->yy_is_our_buffer ) + { + int new_size = b->yy_buf_size * 2; + + if ( new_size <= 0 ) + b->yy_buf_size += b->yy_buf_size / 8; + else + b->yy_buf_size *= 2; + + b->yy_ch_buf = (char *) + /* Include room in for 2 EOB chars. */ + zconfrealloc((void *) b->yy_ch_buf,b->yy_buf_size + 2 ); + } + else + /* Can't grow it, we don't own it. */ + b->yy_ch_buf = 0; + + if ( ! b->yy_ch_buf ) + YY_FATAL_ERROR( + "fatal error - scanner input buffer overflow" ); + + (yy_c_buf_p) = &b->yy_ch_buf[yy_c_buf_p_offset]; + + num_to_read = YY_CURRENT_BUFFER_LVALUE->yy_buf_size - + number_to_move - 1; + + } + + if ( num_to_read > YY_READ_BUF_SIZE ) + num_to_read = YY_READ_BUF_SIZE; + + /* Read in more data. */ + YY_INPUT( (&YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[number_to_move]), + (yy_n_chars), (size_t) num_to_read ); + + YY_CURRENT_BUFFER_LVALUE->yy_n_chars = (yy_n_chars); + } + + if ( (yy_n_chars) == 0 ) + { + if ( number_to_move == YY_MORE_ADJ ) + { + ret_val = EOB_ACT_END_OF_FILE; + zconfrestart(zconfin ); + } + + else + { + ret_val = EOB_ACT_LAST_MATCH; + YY_CURRENT_BUFFER_LVALUE->yy_buffer_status = + YY_BUFFER_EOF_PENDING; + } + } + + else + ret_val = EOB_ACT_CONTINUE_SCAN; + + if ((yy_size_t) ((yy_n_chars) + number_to_move) > YY_CURRENT_BUFFER_LVALUE->yy_buf_size) { + /* Extend the array by 50%, plus the number we really need. */ + yy_size_t new_size = (yy_n_chars) + number_to_move + ((yy_n_chars) >> 1); + YY_CURRENT_BUFFER_LVALUE->yy_ch_buf = (char *) zconfrealloc((void *) YY_CURRENT_BUFFER_LVALUE->yy_ch_buf,new_size ); + if ( ! YY_CURRENT_BUFFER_LVALUE->yy_ch_buf ) + YY_FATAL_ERROR( "out of dynamic memory in yy_get_next_buffer()" ); + } + + (yy_n_chars) += number_to_move; + YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[(yy_n_chars)] = YY_END_OF_BUFFER_CHAR; + YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[(yy_n_chars) + 1] = YY_END_OF_BUFFER_CHAR; + + (yytext_ptr) = &YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[0]; + + return ret_val; +} + +/* yy_get_previous_state - get the state just before the EOB char was reached */ + + static yy_state_type yy_get_previous_state (void) +{ + register yy_state_type yy_current_state; + register char *yy_cp; + + yy_current_state = (yy_start); + + for ( yy_cp = (yytext_ptr) + YY_MORE_ADJ; yy_cp < (yy_c_buf_p); ++yy_cp ) + { + yy_current_state = yy_nxt[yy_current_state][(*yy_cp ? yy_ec[YY_SC_TO_UI(*yy_cp)] : 1)]; + } + + return yy_current_state; +} + +/* yy_try_NUL_trans - try to make a transition on the NUL character + * + * synopsis + * next_state = yy_try_NUL_trans( current_state ); + */ + static yy_state_type yy_try_NUL_trans (yy_state_type yy_current_state ) +{ + register int yy_is_jam; + + yy_current_state = yy_nxt[yy_current_state][1]; + yy_is_jam = (yy_current_state <= 0); + + return yy_is_jam ? 0 : yy_current_state; +} + + static void yyunput (int c, register char * yy_bp ) +{ + register char *yy_cp; + + yy_cp = (yy_c_buf_p); + + /* undo effects of setting up zconftext */ + *yy_cp = (yy_hold_char); + + if ( yy_cp < YY_CURRENT_BUFFER_LVALUE->yy_ch_buf + 2 ) + { /* need to shift things up to make room */ + /* +2 for EOB chars. */ + register int number_to_move = (yy_n_chars) + 2; + register char *dest = &YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[ + YY_CURRENT_BUFFER_LVALUE->yy_buf_size + 2]; + register char *source = + &YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[number_to_move]; + + while ( source > YY_CURRENT_BUFFER_LVALUE->yy_ch_buf ) + *--dest = *--source; + + yy_cp += (int) (dest - source); + yy_bp += (int) (dest - source); + YY_CURRENT_BUFFER_LVALUE->yy_n_chars = + (yy_n_chars) = YY_CURRENT_BUFFER_LVALUE->yy_buf_size; + + if ( yy_cp < YY_CURRENT_BUFFER_LVALUE->yy_ch_buf + 2 ) + YY_FATAL_ERROR( "flex scanner push-back overflow" ); + } + + *--yy_cp = (char) c; + + (yytext_ptr) = yy_bp; + (yy_hold_char) = *yy_cp; + (yy_c_buf_p) = yy_cp; +} + +#ifndef YY_NO_INPUT +#ifdef __cplusplus + static int yyinput (void) +#else + static int input (void) +#endif + +{ + int c; + + *(yy_c_buf_p) = (yy_hold_char); + + if ( *(yy_c_buf_p) == YY_END_OF_BUFFER_CHAR ) + { + /* yy_c_buf_p now points to the character we want to return. + * If this occurs *before* the EOB characters, then it's a + * valid NUL; if not, then we've hit the end of the buffer. + */ + if ( (yy_c_buf_p) < &YY_CURRENT_BUFFER_LVALUE->yy_ch_buf[(yy_n_chars)] ) + /* This was really a NUL. */ + *(yy_c_buf_p) = '\0'; + + else + { /* need more input */ + int offset = (yy_c_buf_p) - (yytext_ptr); + ++(yy_c_buf_p); + + switch ( yy_get_next_buffer( ) ) + { + case EOB_ACT_LAST_MATCH: + /* This happens because yy_g_n_b() + * sees that we've accumulated a + * token and flags that we need to + * try matching the token before + * proceeding. But for input(), + * there's no matching to consider. + * So convert the EOB_ACT_LAST_MATCH + * to EOB_ACT_END_OF_FILE. + */ + + /* Reset buffer status. */ + zconfrestart(zconfin ); + + /*FALLTHROUGH*/ + + case EOB_ACT_END_OF_FILE: + { + if ( zconfwrap( ) ) + return EOF; + + if ( ! (yy_did_buffer_switch_on_eof) ) + YY_NEW_FILE; +#ifdef __cplusplus + return yyinput(); +#else + return input(); +#endif + } + + case EOB_ACT_CONTINUE_SCAN: + (yy_c_buf_p) = (yytext_ptr) + offset; + break; + } + } + } + + c = *(unsigned char *) (yy_c_buf_p); /* cast for 8-bit char's */ + *(yy_c_buf_p) = '\0'; /* preserve zconftext */ + (yy_hold_char) = *++(yy_c_buf_p); + + return c; +} +#endif /* ifndef YY_NO_INPUT */ + +/** Immediately switch to a different input stream. + * @param input_file A readable stream. + * + * @note This function does not reset the start condition to @c INITIAL . + */ + void zconfrestart (FILE * input_file ) +{ + + if ( ! YY_CURRENT_BUFFER ){ + zconfensure_buffer_stack (); + YY_CURRENT_BUFFER_LVALUE = + zconf_create_buffer(zconfin,YY_BUF_SIZE ); + } + + zconf_init_buffer(YY_CURRENT_BUFFER,input_file ); + zconf_load_buffer_state( ); +} + +/** Switch to a different input buffer. + * @param new_buffer The new input buffer. + * + */ + void zconf_switch_to_buffer (YY_BUFFER_STATE new_buffer ) +{ + + /* TODO. We should be able to replace this entire function body + * with + * zconfpop_buffer_state(); + * zconfpush_buffer_state(new_buffer); + */ + zconfensure_buffer_stack (); + if ( YY_CURRENT_BUFFER == new_buffer ) + return; + + if ( YY_CURRENT_BUFFER ) + { + /* Flush out information for old buffer. */ + *(yy_c_buf_p) = (yy_hold_char); + YY_CURRENT_BUFFER_LVALUE->yy_buf_pos = (yy_c_buf_p); + YY_CURRENT_BUFFER_LVALUE->yy_n_chars = (yy_n_chars); + } + + YY_CURRENT_BUFFER_LVALUE = new_buffer; + zconf_load_buffer_state( ); + + /* We don't actually know whether we did this switch during + * EOF (zconfwrap()) processing, but the only time this flag + * is looked at is after zconfwrap() is called, so it's safe + * to go ahead and always set it. + */ + (yy_did_buffer_switch_on_eof) = 1; +} + +static void zconf_load_buffer_state (void) +{ + (yy_n_chars) = YY_CURRENT_BUFFER_LVALUE->yy_n_chars; + (yytext_ptr) = (yy_c_buf_p) = YY_CURRENT_BUFFER_LVALUE->yy_buf_pos; + zconfin = YY_CURRENT_BUFFER_LVALUE->yy_input_file; + (yy_hold_char) = *(yy_c_buf_p); +} + +/** Allocate and initialize an input buffer state. + * @param file A readable stream. + * @param size The character buffer size in bytes. When in doubt, use @c YY_BUF_SIZE. + * + * @return the allocated buffer state. + */ + YY_BUFFER_STATE zconf_create_buffer (FILE * file, int size ) +{ + YY_BUFFER_STATE b; + + b = (YY_BUFFER_STATE) zconfalloc(sizeof( struct yy_buffer_state ) ); + if ( ! b ) + YY_FATAL_ERROR( "out of dynamic memory in zconf_create_buffer()" ); + + b->yy_buf_size = size; + + /* yy_ch_buf has to be 2 characters longer than the size given because + * we need to put in 2 end-of-buffer characters. + */ + b->yy_ch_buf = (char *) zconfalloc(b->yy_buf_size + 2 ); + if ( ! b->yy_ch_buf ) + YY_FATAL_ERROR( "out of dynamic memory in zconf_create_buffer()" ); + + b->yy_is_our_buffer = 1; + + zconf_init_buffer(b,file ); + + return b; +} + +/** Destroy the buffer. + * @param b a buffer created with zconf_create_buffer() + * + */ + void zconf_delete_buffer (YY_BUFFER_STATE b ) +{ + + if ( ! b ) + return; + + if ( b == YY_CURRENT_BUFFER ) /* Not sure if we should pop here. */ + YY_CURRENT_BUFFER_LVALUE = (YY_BUFFER_STATE) 0; + + if ( b->yy_is_our_buffer ) + zconffree((void *) b->yy_ch_buf ); + + zconffree((void *) b ); +} + +/* Initializes or reinitializes a buffer. + * This function is sometimes called more than once on the same buffer, + * such as during a zconfrestart() or at EOF. + */ + static void zconf_init_buffer (YY_BUFFER_STATE b, FILE * file ) + +{ + int oerrno = errno; + + zconf_flush_buffer(b ); + + b->yy_input_file = file; + b->yy_fill_buffer = 1; + + /* If b is the current buffer, then zconf_init_buffer was _probably_ + * called from zconfrestart() or through yy_get_next_buffer. + * In that case, we don't want to reset the lineno or column. + */ + if (b != YY_CURRENT_BUFFER){ + b->yy_bs_lineno = 1; + b->yy_bs_column = 0; + } + + b->yy_is_interactive = 0; + + errno = oerrno; +} + +/** Discard all buffered characters. On the next scan, YY_INPUT will be called. + * @param b the buffer state to be flushed, usually @c YY_CURRENT_BUFFER. + * + */ + void zconf_flush_buffer (YY_BUFFER_STATE b ) +{ + if ( ! b ) + return; + + b->yy_n_chars = 0; + + /* We always need two end-of-buffer characters. The first causes + * a transition to the end-of-buffer state. The second causes + * a jam in that state. + */ + b->yy_ch_buf[0] = YY_END_OF_BUFFER_CHAR; + b->yy_ch_buf[1] = YY_END_OF_BUFFER_CHAR; + + b->yy_buf_pos = &b->yy_ch_buf[0]; + + b->yy_at_bol = 1; + b->yy_buffer_status = YY_BUFFER_NEW; + + if ( b == YY_CURRENT_BUFFER ) + zconf_load_buffer_state( ); +} + +/** Pushes the new state onto the stack. The new state becomes + * the current state. This function will allocate the stack + * if necessary. + * @param new_buffer The new state. + * + */ +void zconfpush_buffer_state (YY_BUFFER_STATE new_buffer ) +{ + if (new_buffer == NULL) + return; + + zconfensure_buffer_stack(); + + /* This block is copied from zconf_switch_to_buffer. */ + if ( YY_CURRENT_BUFFER ) + { + /* Flush out information for old buffer. */ + *(yy_c_buf_p) = (yy_hold_char); + YY_CURRENT_BUFFER_LVALUE->yy_buf_pos = (yy_c_buf_p); + YY_CURRENT_BUFFER_LVALUE->yy_n_chars = (yy_n_chars); + } + + /* Only push if top exists. Otherwise, replace top. */ + if (YY_CURRENT_BUFFER) + (yy_buffer_stack_top)++; + YY_CURRENT_BUFFER_LVALUE = new_buffer; + + /* copied from zconf_switch_to_buffer. */ + zconf_load_buffer_state( ); + (yy_did_buffer_switch_on_eof) = 1; +} + +/** Removes and deletes the top of the stack, if present. + * The next element becomes the new top. + * + */ +void zconfpop_buffer_state (void) +{ + if (!YY_CURRENT_BUFFER) + return; + + zconf_delete_buffer(YY_CURRENT_BUFFER ); + YY_CURRENT_BUFFER_LVALUE = NULL; + if ((yy_buffer_stack_top) > 0) + --(yy_buffer_stack_top); + + if (YY_CURRENT_BUFFER) { + zconf_load_buffer_state( ); + (yy_did_buffer_switch_on_eof) = 1; + } +} + +/* Allocates the stack if it does not exist. + * Guarantees space for at least one push. + */ +static void zconfensure_buffer_stack (void) +{ + int num_to_alloc; + + if (!(yy_buffer_stack)) { + + /* First allocation is just for 2 elements, since we don't know if this + * scanner will even need a stack. We use 2 instead of 1 to avoid an + * immediate realloc on the next call. + */ + num_to_alloc = 1; + (yy_buffer_stack) = (struct yy_buffer_state**)zconfalloc + (num_to_alloc * sizeof(struct yy_buffer_state*) + ); + if ( ! (yy_buffer_stack) ) + YY_FATAL_ERROR( "out of dynamic memory in zconfensure_buffer_stack()" ); + + memset((yy_buffer_stack), 0, num_to_alloc * sizeof(struct yy_buffer_state*)); + + (yy_buffer_stack_max) = num_to_alloc; + (yy_buffer_stack_top) = 0; + return; + } + + if ((yy_buffer_stack_top) >= ((yy_buffer_stack_max)) - 1){ + + /* Increase the buffer to prepare for a possible push. */ + int grow_size = 8 /* arbitrary grow size */; + + num_to_alloc = (yy_buffer_stack_max) + grow_size; + (yy_buffer_stack) = (struct yy_buffer_state**)zconfrealloc + ((yy_buffer_stack), + num_to_alloc * sizeof(struct yy_buffer_state*) + ); + if ( ! (yy_buffer_stack) ) + YY_FATAL_ERROR( "out of dynamic memory in zconfensure_buffer_stack()" ); + + /* zero only the new slots.*/ + memset((yy_buffer_stack) + (yy_buffer_stack_max), 0, grow_size * sizeof(struct yy_buffer_state*)); + (yy_buffer_stack_max) = num_to_alloc; + } +} + +/** Setup the input buffer state to scan directly from a user-specified character buffer. + * @param base the character buffer + * @param size the size in bytes of the character buffer + * + * @return the newly allocated buffer state object. + */ +YY_BUFFER_STATE zconf_scan_buffer (char * base, yy_size_t size ) +{ + YY_BUFFER_STATE b; + + if ( size < 2 || + base[size-2] != YY_END_OF_BUFFER_CHAR || + base[size-1] != YY_END_OF_BUFFER_CHAR ) + /* They forgot to leave room for the EOB's. */ + return 0; + + b = (YY_BUFFER_STATE) zconfalloc(sizeof( struct yy_buffer_state ) ); + if ( ! b ) + YY_FATAL_ERROR( "out of dynamic memory in zconf_scan_buffer()" ); + + b->yy_buf_size = size - 2; /* "- 2" to take care of EOB's */ + b->yy_buf_pos = b->yy_ch_buf = base; + b->yy_is_our_buffer = 0; + b->yy_input_file = 0; + b->yy_n_chars = b->yy_buf_size; + b->yy_is_interactive = 0; + b->yy_at_bol = 1; + b->yy_fill_buffer = 0; + b->yy_buffer_status = YY_BUFFER_NEW; + + zconf_switch_to_buffer(b ); + + return b; +} + +/** Setup the input buffer state to scan a string. The next call to zconflex() will + * scan from a @e copy of @a str. + * @param yystr a NUL-terminated string to scan + * + * @return the newly allocated buffer state object. + * @note If you want to scan bytes that may contain NUL values, then use + * zconf_scan_bytes() instead. + */ +YY_BUFFER_STATE zconf_scan_string (yyconst char * yystr ) +{ + + return zconf_scan_bytes(yystr,strlen(yystr) ); +} + +/** Setup the input buffer state to scan the given bytes. The next call to zconflex() will + * scan from a @e copy of @a bytes. + * @param yybytes the byte buffer to scan + * @param _yybytes_len the number of bytes in the buffer pointed to by @a bytes. + * + * @return the newly allocated buffer state object. + */ +YY_BUFFER_STATE zconf_scan_bytes (yyconst char * yybytes, int _yybytes_len ) +{ + YY_BUFFER_STATE b; + char *buf; + yy_size_t n; + int i; + + /* Get memory for full buffer, including space for trailing EOB's. */ + n = _yybytes_len + 2; + buf = (char *) zconfalloc(n ); + if ( ! buf ) + YY_FATAL_ERROR( "out of dynamic memory in zconf_scan_bytes()" ); + + for ( i = 0; i < _yybytes_len; ++i ) + buf[i] = yybytes[i]; + + buf[_yybytes_len] = buf[_yybytes_len+1] = YY_END_OF_BUFFER_CHAR; + + b = zconf_scan_buffer(buf,n ); + if ( ! b ) + YY_FATAL_ERROR( "bad buffer in zconf_scan_bytes()" ); + + /* It's okay to grow etc. this buffer, and we should throw it + * away when we're done. + */ + b->yy_is_our_buffer = 1; + + return b; +} + +#ifndef YY_EXIT_FAILURE +#define YY_EXIT_FAILURE 2 +#endif + +static void yy_fatal_error (yyconst char* msg ) +{ + (void) fprintf( stderr, "%s\n", msg ); + exit( YY_EXIT_FAILURE ); +} + +/* Redefine yyless() so it works in section 3 code. */ + +#undef yyless +#define yyless(n) \ + do \ + { \ + /* Undo effects of setting up zconftext. */ \ + int yyless_macro_arg = (n); \ + YY_LESS_LINENO(yyless_macro_arg);\ + zconftext[zconfleng] = (yy_hold_char); \ + (yy_c_buf_p) = zconftext + yyless_macro_arg; \ + (yy_hold_char) = *(yy_c_buf_p); \ + *(yy_c_buf_p) = '\0'; \ + zconfleng = yyless_macro_arg; \ + } \ + while ( 0 ) + +/* Accessor methods (get/set functions) to struct members. */ + +/** Get the current line number. + * + */ +int zconfget_lineno (void) +{ + + return zconflineno; +} + +/** Get the input stream. + * + */ +FILE *zconfget_in (void) +{ + return zconfin; +} + +/** Get the output stream. + * + */ +FILE *zconfget_out (void) +{ + return zconfout; +} + +/** Get the length of the current token. + * + */ +int zconfget_leng (void) +{ + return zconfleng; +} + +/** Get the current token. + * + */ + +char *zconfget_text (void) +{ + return zconftext; +} + +/** Set the current line number. + * @param line_number + * + */ +void zconfset_lineno (int line_number ) +{ + + zconflineno = line_number; +} + +/** Set the input stream. This does not discard the current + * input buffer. + * @param in_str A readable stream. + * + * @see zconf_switch_to_buffer + */ +void zconfset_in (FILE * in_str ) +{ + zconfin = in_str ; +} + +void zconfset_out (FILE * out_str ) +{ + zconfout = out_str ; +} + +int zconfget_debug (void) +{ + return zconf_flex_debug; +} + +void zconfset_debug (int bdebug ) +{ + zconf_flex_debug = bdebug ; +} + +static int yy_init_globals (void) +{ + /* Initialization is the same as for the non-reentrant scanner. + * This function is called from zconflex_destroy(), so don't allocate here. + */ + + (yy_buffer_stack) = 0; + (yy_buffer_stack_top) = 0; + (yy_buffer_stack_max) = 0; + (yy_c_buf_p) = (char *) 0; + (yy_init) = 0; + (yy_start) = 0; + +/* Defined in main.c */ +#ifdef YY_STDINIT + zconfin = stdin; + zconfout = stdout; +#else + zconfin = (FILE *) 0; + zconfout = (FILE *) 0; +#endif + + /* For future reference: Set errno on error, since we are called by + * zconflex_init() + */ + return 0; +} + +/* zconflex_destroy is for both reentrant and non-reentrant scanners. */ +int zconflex_destroy (void) +{ + + /* Pop the buffer stack, destroying each element. */ + while(YY_CURRENT_BUFFER){ + zconf_delete_buffer(YY_CURRENT_BUFFER ); + YY_CURRENT_BUFFER_LVALUE = NULL; + zconfpop_buffer_state(); + } + + /* Destroy the stack itself. */ + zconffree((yy_buffer_stack) ); + (yy_buffer_stack) = NULL; + + /* Reset the globals. This is important in a non-reentrant scanner so the next time + * zconflex() is called, initialization will occur. */ + yy_init_globals( ); + + return 0; +} + +/* + * Internal utility routines. + */ + +#ifndef yytext_ptr +static void yy_flex_strncpy (char* s1, yyconst char * s2, int n ) +{ + register int i; + for ( i = 0; i < n; ++i ) + s1[i] = s2[i]; +} +#endif + +#ifdef YY_NEED_STRLEN +static int yy_flex_strlen (yyconst char * s ) +{ + register int n; + for ( n = 0; s[n]; ++n ) + ; + + return n; +} +#endif + +void *zconfalloc (yy_size_t size ) +{ + return (void *) malloc( size ); +} + +void *zconfrealloc (void * ptr, yy_size_t size ) +{ + /* The cast to (char *) in the following accommodates both + * implementations that use char* generic pointers, and those + * that use void* generic pointers. It works with the latter + * because both ANSI C and C++ allow castless assignment from + * any pointer type to void*, and deal with argument conversions + * as though doing an assignment. + */ + return (void *) realloc( (char *) ptr, size ); +} + +void zconffree (void * ptr ) +{ + free( (char *) ptr ); /* see zconfrealloc() for (char *) cast */ +} + +#define YYTABLES_NAME "yytables" + +void zconf_starthelp(void) +{ + new_string(); + last_ts = first_ts = 0; + BEGIN(HELP); +} + +static void zconf_endhelp(void) +{ + zconflval.string = text; + BEGIN(INITIAL); +} + +/* + * Try to open specified file with following names: + * ./name + * $(srctree)/name + * The latter is used when srctree is separate from objtree + * when compiling the kernel. + * Return NULL if file is not found. + */ +FILE *zconf_fopen(const char *name) +{ + char *env, fullname[PATH_MAX+1]; + FILE *f; + + f = fopen(name, "r"); + if (!f && name != NULL && name[0] != '/') { + env = getenv(SRCTREE); + if (env) { + sprintf(fullname, "%s/%s", env, name); + f = fopen(fullname, "r"); + } + } + return f; +} + +void zconf_initscan(const char *name) +{ + zconfin = zconf_fopen(name); + if (!zconfin) { + printf("can't find file %s\n", name); + exit(1); + } + + current_buf = xmalloc(sizeof(*current_buf)); + memset(current_buf, 0, sizeof(*current_buf)); + + current_file = file_lookup(name); + current_file->lineno = 1; +} + +void zconf_nextfile(const char *name) +{ + struct file *iter; + struct file *file = file_lookup(name); + struct buffer *buf = xmalloc(sizeof(*buf)); + memset(buf, 0, sizeof(*buf)); + + current_buf->state = YY_CURRENT_BUFFER; + zconfin = zconf_fopen(file->name); + if (!zconfin) { + printf("%s:%d: can't open file \"%s\"\n", + zconf_curname(), zconf_lineno(), file->name); + exit(1); + } + zconf_switch_to_buffer(zconf_create_buffer(zconfin,YY_BUF_SIZE)); + buf->parent = current_buf; + current_buf = buf; + + for (iter = current_file->parent; iter; iter = iter->parent ) { + if (!strcmp(current_file->name,iter->name) ) { + printf("%s:%d: recursive inclusion detected. " + "Inclusion path:\n current file : '%s'\n", + zconf_curname(), zconf_lineno(), + zconf_curname()); + iter = current_file->parent; + while (iter && \ + strcmp(iter->name,current_file->name)) { + printf(" included from: '%s:%d'\n", + iter->name, iter->lineno-1); + iter = iter->parent; + } + if (iter) + printf(" included from: '%s:%d'\n", + iter->name, iter->lineno+1); + exit(1); + } + } + file->lineno = 1; + file->parent = current_file; + current_file = file; +} + +static void zconf_endfile(void) +{ + struct buffer *parent; + + current_file = current_file->parent; + + parent = current_buf->parent; + if (parent) { + fclose(zconfin); + zconf_delete_buffer(YY_CURRENT_BUFFER); + zconf_switch_to_buffer(parent->state); + } + free(current_buf); + current_buf = parent; +} + +int zconf_lineno(void) +{ + return current_pos.lineno; +} + +const char *zconf_curname(void) +{ + return current_pos.file ? current_pos.file->name : ""; +} + diff --git a/scripts/kconfig/zconf.tab.c_shipped b/scripts/kconfig/zconf.tab.c_shipped new file mode 100644 index 0000000000..5c5c238c23 --- /dev/null +++ b/scripts/kconfig/zconf.tab.c_shipped @@ -0,0 +1,2580 @@ +/* A Bison parser, made by GNU Bison 2.5.1. */ + +/* Bison implementation for Yacc-like parsers in C + + Copyright (C) 1984, 1989-1990, 2000-2012 Free Software Foundation, Inc. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . */ + +/* As a special exception, you may create a larger work that contains + part or all of the Bison parser skeleton and distribute that work + under terms of your choice, so long as that work isn't itself a + parser generator using the skeleton or a modified version thereof + as a parser skeleton. Alternatively, if you modify or redistribute + the parser skeleton itself, you may (at your option) remove this + special exception, which will cause the skeleton and the resulting + Bison output files to be licensed under the GNU General Public + License without this special exception. + + This special exception was added by the Free Software Foundation in + version 2.2 of Bison. */ + +/* C LALR(1) parser skeleton written by Richard Stallman, by + simplifying the original so-called "semantic" parser. */ + +/* All symbols defined below should begin with yy or YY, to avoid + infringing on user name space. This should be done even for local + variables, as they might otherwise be expanded by user macros. + There are some unavoidable exceptions within include files to + define necessary library symbols; they are noted "INFRINGES ON + USER NAME SPACE" below. */ + +/* Identify Bison output. */ +#define YYBISON 1 + +/* Bison version. */ +#define YYBISON_VERSION "2.5.1" + +/* Skeleton name. */ +#define YYSKELETON_NAME "yacc.c" + +/* Pure parsers. */ +#define YYPURE 0 + +/* Push parsers. */ +#define YYPUSH 0 + +/* Pull parsers. */ +#define YYPULL 1 + +/* Using locations. */ +#define YYLSP_NEEDED 0 + +/* Substitute the variable and function names. */ +#define yyparse zconfparse +#define yylex zconflex +#define yyerror zconferror +#define yylval zconflval +#define yychar zconfchar +#define yydebug zconfdebug +#define yynerrs zconfnerrs + + +/* Copy the first part of user declarations. */ + + +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include +#include + +#include "lkc.h" + +#define printd(mask, fmt...) if (cdebug & (mask)) printf(fmt) + +#define PRINTD 0x0001 +#define DEBUG_PARSE 0x0002 + +int cdebug = PRINTD; + +extern int zconflex(void); +static void zconfprint(const char *err, ...); +static void zconf_error(const char *err, ...); +static void zconferror(const char *err); +static bool zconf_endtoken(const struct kconf_id *id, int starttoken, int endtoken); + +struct symbol *symbol_hash[SYMBOL_HASHSIZE]; + +static struct menu *current_menu, *current_entry; + + + + +# ifndef YY_NULL +# if defined __cplusplus && 201103L <= __cplusplus +# define YY_NULL nullptr +# else +# define YY_NULL 0 +# endif +# endif + +/* Enabling traces. */ +#ifndef YYDEBUG +# define YYDEBUG 1 +#endif + +/* Enabling verbose error messages. */ +#ifdef YYERROR_VERBOSE +# undef YYERROR_VERBOSE +# define YYERROR_VERBOSE 1 +#else +# define YYERROR_VERBOSE 0 +#endif + +/* Enabling the token table. */ +#ifndef YYTOKEN_TABLE +# define YYTOKEN_TABLE 0 +#endif + + +/* Tokens. */ +#ifndef YYTOKENTYPE +# define YYTOKENTYPE + /* Put the tokens into the symbol table, so that GDB and other debuggers + know about them. */ + enum yytokentype { + T_MAINMENU = 258, + T_MENU = 259, + T_ENDMENU = 260, + T_SOURCE = 261, + T_CHOICE = 262, + T_ENDCHOICE = 263, + T_COMMENT = 264, + T_CONFIG = 265, + T_MENUCONFIG = 266, + T_HELP = 267, + T_HELPTEXT = 268, + T_IF = 269, + T_ENDIF = 270, + T_DEPENDS = 271, + T_OPTIONAL = 272, + T_PROMPT = 273, + T_TYPE = 274, + T_DEFAULT = 275, + T_SELECT = 276, + T_RANGE = 277, + T_VISIBLE = 278, + T_OPTION = 279, + T_ON = 280, + T_WORD = 281, + T_WORD_QUOTE = 282, + T_UNEQUAL = 283, + T_LESS = 284, + T_LESS_EQUAL = 285, + T_GREATER = 286, + T_GREATER_EQUAL = 287, + T_CLOSE_PAREN = 288, + T_OPEN_PAREN = 289, + T_EOL = 290, + T_OR = 291, + T_AND = 292, + T_EQUAL = 293, + T_NOT = 294 + }; +#endif + + + +#if ! defined YYSTYPE && ! defined YYSTYPE_IS_DECLARED +typedef union YYSTYPE +{ + + + char *string; + struct file *file; + struct symbol *symbol; + struct expr *expr; + struct menu *menu; + const struct kconf_id *id; + + + +} YYSTYPE; +# define YYSTYPE_IS_TRIVIAL 1 +# define yystype YYSTYPE /* obsolescent; will be withdrawn */ +# define YYSTYPE_IS_DECLARED 1 +#endif + + +/* Copy the second part of user declarations. */ + + +/* Include zconf.hash.c here so it can see the token constants. */ +#include "zconf.hash.c" + + + +#ifdef short +# undef short +#endif + +#ifdef YYTYPE_UINT8 +typedef YYTYPE_UINT8 yytype_uint8; +#else +typedef unsigned char yytype_uint8; +#endif + +#ifdef YYTYPE_INT8 +typedef YYTYPE_INT8 yytype_int8; +#elif (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +typedef signed char yytype_int8; +#else +typedef short int yytype_int8; +#endif + +#ifdef YYTYPE_UINT16 +typedef YYTYPE_UINT16 yytype_uint16; +#else +typedef unsigned short int yytype_uint16; +#endif + +#ifdef YYTYPE_INT16 +typedef YYTYPE_INT16 yytype_int16; +#else +typedef short int yytype_int16; +#endif + +#ifndef YYSIZE_T +# ifdef __SIZE_TYPE__ +# define YYSIZE_T __SIZE_TYPE__ +# elif defined size_t +# define YYSIZE_T size_t +# elif ! defined YYSIZE_T && (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +# include /* INFRINGES ON USER NAME SPACE */ +# define YYSIZE_T size_t +# else +# define YYSIZE_T unsigned int +# endif +#endif + +#define YYSIZE_MAXIMUM ((YYSIZE_T) -1) + +#ifndef YY_ +# if defined YYENABLE_NLS && YYENABLE_NLS +# if ENABLE_NLS +# include /* INFRINGES ON USER NAME SPACE */ +# define YY_(msgid) dgettext ("bison-runtime", msgid) +# endif +# endif +# ifndef YY_ +# define YY_(msgid) msgid +# endif +#endif + +/* Suppress unused-variable warnings by "using" E. */ +#if ! defined lint || defined __GNUC__ +# define YYUSE(e) ((void) (e)) +#else +# define YYUSE(e) /* empty */ +#endif + +/* Identity function, used to suppress warnings about constant conditions. */ +#ifndef lint +# define YYID(n) (n) +#else +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static int +YYID (int yyi) +#else +static int +YYID (yyi) + int yyi; +#endif +{ + return yyi; +} +#endif + +#if ! defined yyoverflow || YYERROR_VERBOSE + +/* The parser invokes alloca or malloc; define the necessary symbols. */ + +# ifdef YYSTACK_USE_ALLOCA +# if YYSTACK_USE_ALLOCA +# ifdef __GNUC__ +# define YYSTACK_ALLOC __builtin_alloca +# elif defined __BUILTIN_VA_ARG_INCR +# include /* INFRINGES ON USER NAME SPACE */ +# elif defined _AIX +# define YYSTACK_ALLOC __alloca +# elif defined _MSC_VER +# include /* INFRINGES ON USER NAME SPACE */ +# define alloca _alloca +# else +# define YYSTACK_ALLOC alloca +# if ! defined _ALLOCA_H && ! defined EXIT_SUCCESS && (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +# include /* INFRINGES ON USER NAME SPACE */ + /* Use EXIT_SUCCESS as a witness for stdlib.h. */ +# ifndef EXIT_SUCCESS +# define EXIT_SUCCESS 0 +# endif +# endif +# endif +# endif +# endif + +# ifdef YYSTACK_ALLOC + /* Pacify GCC's `empty if-body' warning. */ +# define YYSTACK_FREE(Ptr) do { /* empty */; } while (YYID (0)) +# ifndef YYSTACK_ALLOC_MAXIMUM + /* The OS might guarantee only one guard page at the bottom of the stack, + and a page size can be as small as 4096 bytes. So we cannot safely + invoke alloca (N) if N exceeds 4096. Use a slightly smaller number + to allow for a few compiler-allocated temporary stack slots. */ +# define YYSTACK_ALLOC_MAXIMUM 4032 /* reasonable circa 2006 */ +# endif +# else +# define YYSTACK_ALLOC YYMALLOC +# define YYSTACK_FREE YYFREE +# ifndef YYSTACK_ALLOC_MAXIMUM +# define YYSTACK_ALLOC_MAXIMUM YYSIZE_MAXIMUM +# endif +# if (defined __cplusplus && ! defined EXIT_SUCCESS \ + && ! ((defined YYMALLOC || defined malloc) \ + && (defined YYFREE || defined free))) +# include /* INFRINGES ON USER NAME SPACE */ +# ifndef EXIT_SUCCESS +# define EXIT_SUCCESS 0 +# endif +# endif +# ifndef YYMALLOC +# define YYMALLOC malloc +# if ! defined malloc && ! defined EXIT_SUCCESS && (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +void *malloc (YYSIZE_T); /* INFRINGES ON USER NAME SPACE */ +# endif +# endif +# ifndef YYFREE +# define YYFREE free +# if ! defined free && ! defined EXIT_SUCCESS && (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +void free (void *); /* INFRINGES ON USER NAME SPACE */ +# endif +# endif +# endif +#endif /* ! defined yyoverflow || YYERROR_VERBOSE */ + + +#if (! defined yyoverflow \ + && (! defined __cplusplus \ + || (defined YYSTYPE_IS_TRIVIAL && YYSTYPE_IS_TRIVIAL))) + +/* A type that is properly aligned for any stack member. */ +union yyalloc +{ + yytype_int16 yyss_alloc; + YYSTYPE yyvs_alloc; +}; + +/* The size of the maximum gap between one aligned stack and the next. */ +# define YYSTACK_GAP_MAXIMUM (sizeof (union yyalloc) - 1) + +/* The size of an array large to enough to hold all stacks, each with + N elements. */ +# define YYSTACK_BYTES(N) \ + ((N) * (sizeof (yytype_int16) + sizeof (YYSTYPE)) \ + + YYSTACK_GAP_MAXIMUM) + +# define YYCOPY_NEEDED 1 + +/* Relocate STACK from its old location to the new one. The + local variables YYSIZE and YYSTACKSIZE give the old and new number of + elements in the stack, and YYPTR gives the new location of the + stack. Advance YYPTR to a properly aligned location for the next + stack. */ +# define YYSTACK_RELOCATE(Stack_alloc, Stack) \ + do \ + { \ + YYSIZE_T yynewbytes; \ + YYCOPY (&yyptr->Stack_alloc, Stack, yysize); \ + Stack = &yyptr->Stack_alloc; \ + yynewbytes = yystacksize * sizeof (*Stack) + YYSTACK_GAP_MAXIMUM; \ + yyptr += yynewbytes / sizeof (*yyptr); \ + } \ + while (YYID (0)) + +#endif + +#if defined YYCOPY_NEEDED && YYCOPY_NEEDED +/* Copy COUNT objects from SRC to DST. The source and destination do + not overlap. */ +# ifndef YYCOPY +# if defined __GNUC__ && 1 < __GNUC__ +# define YYCOPY(Dst, Src, Count) \ + __builtin_memcpy (Dst, Src, (Count) * sizeof (*(Src))) +# else +# define YYCOPY(Dst, Src, Count) \ + do \ + { \ + YYSIZE_T yyi; \ + for (yyi = 0; yyi < (Count); yyi++) \ + (Dst)[yyi] = (Src)[yyi]; \ + } \ + while (YYID (0)) +# endif +# endif +#endif /* !YYCOPY_NEEDED */ + +/* YYFINAL -- State number of the termination state. */ +#define YYFINAL 11 +/* YYLAST -- Last index in YYTABLE. */ +#define YYLAST 298 + +/* YYNTOKENS -- Number of terminals. */ +#define YYNTOKENS 40 +/* YYNNTS -- Number of nonterminals. */ +#define YYNNTS 50 +/* YYNRULES -- Number of rules. */ +#define YYNRULES 122 +/* YYNRULES -- Number of states. */ +#define YYNSTATES 199 + +/* YYTRANSLATE(YYLEX) -- Bison symbol number corresponding to YYLEX. */ +#define YYUNDEFTOK 2 +#define YYMAXUTOK 294 + +#define YYTRANSLATE(YYX) \ + ((unsigned int) (YYX) <= YYMAXUTOK ? yytranslate[YYX] : YYUNDEFTOK) + +/* YYTRANSLATE[YYLEX] -- Bison symbol number corresponding to YYLEX. */ +static const yytype_uint8 yytranslate[] = +{ + 0, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, + 2, 2, 2, 2, 2, 2, 1, 2, 3, 4, + 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, + 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, + 35, 36, 37, 38, 39 +}; + +#if YYDEBUG +/* YYPRHS[YYN] -- Index of the first RHS symbol of rule number YYN in + YYRHS. */ +static const yytype_uint16 yyprhs[] = +{ + 0, 0, 3, 6, 8, 11, 13, 14, 17, 20, + 23, 26, 31, 36, 40, 42, 44, 46, 48, 50, + 52, 54, 56, 58, 60, 62, 64, 66, 68, 72, + 75, 79, 82, 86, 89, 90, 93, 96, 99, 102, + 105, 108, 112, 117, 122, 127, 133, 137, 138, 142, + 143, 146, 150, 153, 155, 159, 160, 163, 166, 169, + 172, 175, 180, 184, 187, 192, 193, 196, 200, 202, + 206, 207, 210, 213, 216, 220, 224, 228, 230, 234, + 235, 238, 241, 244, 248, 252, 255, 258, 261, 262, + 265, 268, 271, 276, 277, 280, 283, 286, 287, 290, + 292, 294, 297, 300, 303, 305, 308, 309, 312, 314, + 318, 322, 326, 330, 334, 338, 342, 345, 349, 353, + 355, 357, 358 +}; + +/* YYRHS -- A `-1'-separated list of the rules' RHS. */ +static const yytype_int8 yyrhs[] = +{ + 41, 0, -1, 85, 42, -1, 42, -1, 67, 43, + -1, 43, -1, -1, 43, 45, -1, 43, 59, -1, + 43, 71, -1, 43, 84, -1, 43, 26, 1, 35, + -1, 43, 44, 1, 35, -1, 43, 1, 35, -1, + 16, -1, 18, -1, 19, -1, 21, -1, 17, -1, + 22, -1, 20, -1, 23, -1, 35, -1, 65, -1, + 75, -1, 48, -1, 50, -1, 73, -1, 26, 1, + 35, -1, 1, 35, -1, 10, 26, 35, -1, 47, + 51, -1, 11, 26, 35, -1, 49, 51, -1, -1, + 51, 52, -1, 51, 53, -1, 51, 79, -1, 51, + 77, -1, 51, 46, -1, 51, 35, -1, 19, 82, + 35, -1, 18, 83, 86, 35, -1, 20, 87, 86, + 35, -1, 21, 26, 86, 35, -1, 22, 88, 88, + 86, 35, -1, 24, 54, 35, -1, -1, 54, 26, + 55, -1, -1, 38, 83, -1, 7, 89, 35, -1, + 56, 60, -1, 84, -1, 57, 62, 58, -1, -1, + 60, 61, -1, 60, 79, -1, 60, 77, -1, 60, + 35, -1, 60, 46, -1, 18, 83, 86, 35, -1, + 19, 82, 35, -1, 17, 35, -1, 20, 26, 86, + 35, -1, -1, 62, 45, -1, 14, 87, 85, -1, + 84, -1, 63, 66, 64, -1, -1, 66, 45, -1, + 66, 71, -1, 66, 59, -1, 3, 83, 85, -1, + 4, 83, 35, -1, 68, 80, 78, -1, 84, -1, + 69, 72, 70, -1, -1, 72, 45, -1, 72, 71, + -1, 72, 59, -1, 6, 83, 35, -1, 9, 83, + 35, -1, 74, 78, -1, 12, 35, -1, 76, 13, + -1, -1, 78, 79, -1, 78, 35, -1, 78, 46, + -1, 16, 25, 87, 35, -1, -1, 80, 81, -1, + 80, 35, -1, 23, 86, -1, -1, 83, 86, -1, + 26, -1, 27, -1, 5, 35, -1, 8, 35, -1, + 15, 35, -1, 35, -1, 85, 35, -1, -1, 14, + 87, -1, 88, -1, 88, 29, 88, -1, 88, 30, + 88, -1, 88, 31, 88, -1, 88, 32, 88, -1, + 88, 38, 88, -1, 88, 28, 88, -1, 34, 87, + 33, -1, 39, 87, -1, 87, 36, 87, -1, 87, + 37, 87, -1, 26, -1, 27, -1, -1, 26, -1 +}; + +/* YYRLINE[YYN] -- source line where rule number YYN was defined. */ +static const yytype_uint16 yyrline[] = +{ + 0, 108, 108, 108, 110, 110, 112, 114, 115, 116, + 117, 118, 119, 123, 127, 127, 127, 127, 127, 127, + 127, 127, 131, 132, 133, 134, 135, 136, 140, 141, + 147, 155, 161, 169, 179, 181, 182, 183, 184, 185, + 186, 189, 197, 203, 213, 219, 225, 228, 230, 241, + 242, 247, 256, 261, 269, 272, 274, 275, 276, 277, + 278, 281, 287, 298, 304, 314, 316, 321, 329, 337, + 340, 342, 343, 344, 349, 356, 363, 368, 376, 379, + 381, 382, 383, 386, 394, 401, 408, 414, 421, 423, + 424, 425, 428, 436, 438, 439, 442, 449, 451, 456, + 457, 460, 461, 462, 466, 467, 470, 471, 474, 475, + 476, 477, 478, 479, 480, 481, 482, 483, 484, 487, + 488, 491, 492 +}; +#endif + +#if YYDEBUG || YYERROR_VERBOSE || YYTOKEN_TABLE +/* YYTNAME[SYMBOL-NUM] -- String name of the symbol SYMBOL-NUM. + First, the terminals, then, starting at YYNTOKENS, nonterminals. */ +static const char *const yytname[] = +{ + "$end", "error", "$undefined", "T_MAINMENU", "T_MENU", "T_ENDMENU", + "T_SOURCE", "T_CHOICE", "T_ENDCHOICE", "T_COMMENT", "T_CONFIG", + "T_MENUCONFIG", "T_HELP", "T_HELPTEXT", "T_IF", "T_ENDIF", "T_DEPENDS", + "T_OPTIONAL", "T_PROMPT", "T_TYPE", "T_DEFAULT", "T_SELECT", "T_RANGE", + "T_VISIBLE", "T_OPTION", "T_ON", "T_WORD", "T_WORD_QUOTE", "T_UNEQUAL", + "T_LESS", "T_LESS_EQUAL", "T_GREATER", "T_GREATER_EQUAL", + "T_CLOSE_PAREN", "T_OPEN_PAREN", "T_EOL", "T_OR", "T_AND", "T_EQUAL", + "T_NOT", "$accept", "input", "start", "stmt_list", "option_name", + "common_stmt", "option_error", "config_entry_start", "config_stmt", + "menuconfig_entry_start", "menuconfig_stmt", "config_option_list", + "config_option", "symbol_option", "symbol_option_list", + "symbol_option_arg", "choice", "choice_entry", "choice_end", + "choice_stmt", "choice_option_list", "choice_option", "choice_block", + "if_entry", "if_end", "if_stmt", "if_block", "mainmenu_stmt", "menu", + "menu_entry", "menu_end", "menu_stmt", "menu_block", "source_stmt", + "comment", "comment_stmt", "help_start", "help", "depends_list", + "depends", "visibility_list", "visible", "prompt_stmt_opt", "prompt", + "end", "nl", "if_expr", "expr", "symbol", "word_opt", YY_NULL +}; +#endif + +# ifdef YYPRINT +/* YYTOKNUM[YYLEX-NUM] -- Internal token number corresponding to + token YYLEX-NUM. */ +static const yytype_uint16 yytoknum[] = +{ + 0, 256, 257, 258, 259, 260, 261, 262, 263, 264, + 265, 266, 267, 268, 269, 270, 271, 272, 273, 274, + 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, + 285, 286, 287, 288, 289, 290, 291, 292, 293, 294 +}; +# endif + +/* YYR1[YYN] -- Symbol number of symbol that rule YYN derives. */ +static const yytype_uint8 yyr1[] = +{ + 0, 40, 41, 41, 42, 42, 43, 43, 43, 43, + 43, 43, 43, 43, 44, 44, 44, 44, 44, 44, + 44, 44, 45, 45, 45, 45, 45, 45, 46, 46, + 47, 48, 49, 50, 51, 51, 51, 51, 51, 51, + 51, 52, 52, 52, 52, 52, 53, 54, 54, 55, + 55, 56, 57, 58, 59, 60, 60, 60, 60, 60, + 60, 61, 61, 61, 61, 62, 62, 63, 64, 65, + 66, 66, 66, 66, 67, 68, 69, 70, 71, 72, + 72, 72, 72, 73, 74, 75, 76, 77, 78, 78, + 78, 78, 79, 80, 80, 80, 81, 82, 82, 83, + 83, 84, 84, 84, 85, 85, 86, 86, 87, 87, + 87, 87, 87, 87, 87, 87, 87, 87, 87, 88, + 88, 89, 89 +}; + +/* YYR2[YYN] -- Number of symbols composing right hand side of rule YYN. */ +static const yytype_uint8 yyr2[] = +{ + 0, 2, 2, 1, 2, 1, 0, 2, 2, 2, + 2, 4, 4, 3, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 3, 2, + 3, 2, 3, 2, 0, 2, 2, 2, 2, 2, + 2, 3, 4, 4, 4, 5, 3, 0, 3, 0, + 2, 3, 2, 1, 3, 0, 2, 2, 2, 2, + 2, 4, 3, 2, 4, 0, 2, 3, 1, 3, + 0, 2, 2, 2, 3, 3, 3, 1, 3, 0, + 2, 2, 2, 3, 3, 2, 2, 2, 0, 2, + 2, 2, 4, 0, 2, 2, 2, 0, 2, 1, + 1, 2, 2, 2, 1, 2, 0, 2, 1, 3, + 3, 3, 3, 3, 3, 3, 2, 3, 3, 1, + 1, 0, 1 +}; + +/* YYDEFACT[STATE-NAME] -- Default reduction number in state STATE-NUM. + Performed when YYTABLE doesn't specify something else to do. Zero + means the default is an error. */ +static const yytype_uint8 yydefact[] = +{ + 6, 0, 104, 0, 3, 0, 6, 6, 99, 100, + 0, 1, 0, 0, 0, 0, 121, 0, 0, 0, + 0, 0, 0, 14, 18, 15, 16, 20, 17, 19, + 21, 0, 22, 0, 7, 34, 25, 34, 26, 55, + 65, 8, 70, 23, 93, 79, 9, 27, 88, 24, + 10, 0, 105, 2, 74, 13, 0, 101, 0, 122, + 0, 102, 0, 0, 0, 119, 120, 0, 0, 0, + 108, 103, 0, 0, 0, 0, 0, 0, 0, 88, + 0, 0, 75, 83, 51, 84, 30, 32, 0, 116, + 0, 0, 67, 0, 0, 0, 0, 0, 0, 11, + 12, 0, 0, 0, 0, 97, 0, 0, 0, 47, + 0, 40, 39, 35, 36, 0, 38, 37, 0, 0, + 97, 0, 59, 60, 56, 58, 57, 66, 54, 53, + 71, 73, 69, 72, 68, 106, 95, 0, 94, 80, + 82, 78, 81, 77, 90, 91, 89, 115, 117, 118, + 114, 109, 110, 111, 112, 113, 29, 86, 0, 106, + 0, 106, 106, 106, 0, 0, 0, 87, 63, 106, + 0, 106, 0, 96, 0, 0, 41, 98, 0, 0, + 106, 49, 46, 28, 0, 62, 0, 107, 92, 42, + 43, 44, 0, 0, 48, 61, 64, 45, 50 +}; + +/* YYDEFGOTO[NTERM-NUM]. */ +static const yytype_int16 yydefgoto[] = +{ + -1, 3, 4, 5, 33, 34, 112, 35, 36, 37, + 38, 74, 113, 114, 165, 194, 39, 40, 128, 41, + 76, 124, 77, 42, 132, 43, 78, 6, 44, 45, + 141, 46, 80, 47, 48, 49, 115, 116, 81, 117, + 79, 138, 160, 161, 50, 7, 173, 69, 70, 60 +}; + +/* YYPACT[STATE-NUM] -- Index in YYTABLE of the portion describing + STATE-NUM. */ +#define YYPACT_NINF -91 +static const yytype_int16 yypact[] = +{ + 19, 37, -91, 13, -91, 79, -91, 20, -91, -91, + -16, -91, 21, 37, 25, 37, 41, 36, 37, 78, + 83, 31, 56, -91, -91, -91, -91, -91, -91, -91, + -91, 116, -91, 127, -91, -91, -91, -91, -91, -91, + -91, -91, -91, -91, -91, -91, -91, -91, -91, -91, + -91, 147, -91, -91, 105, -91, 109, -91, 111, -91, + 114, -91, 136, 137, 142, -91, -91, 31, 31, 76, + 254, -91, 143, 146, 27, 115, 207, 258, 243, -14, + 243, 179, -91, -91, -91, -91, -91, -91, -7, -91, + 31, 31, 105, 51, 51, 51, 51, 51, 51, -91, + -91, 156, 168, 181, 37, 37, 31, 178, 51, -91, + 206, -91, -91, -91, -91, 196, -91, -91, 175, 37, + 37, 185, -91, -91, -91, -91, -91, -91, -91, -91, + -91, -91, -91, -91, -91, 214, -91, 230, -91, -91, + -91, -91, -91, -91, -91, -91, -91, -91, 183, -91, + -91, -91, -91, -91, -91, -91, -91, -91, 31, 214, + 194, 214, 45, 214, 51, 26, 195, -91, -91, 214, + 197, 214, 31, -91, 139, 208, -91, -91, 220, 224, + 214, 222, -91, -91, 226, -91, 227, 123, -91, -91, + -91, -91, 235, 37, -91, -91, -91, -91, -91 +}; + +/* YYPGOTO[NTERM-NUM]. */ +static const yytype_int16 yypgoto[] = +{ + -91, -91, 264, 268, -91, 30, -65, -91, -91, -91, + -91, 238, -91, -91, -91, -91, -91, -91, -91, -12, + -91, -91, -91, -91, -91, -91, -91, -91, -91, -91, + -91, -5, -91, -91, -91, -91, -91, 200, 209, -61, + -91, -91, 170, -1, 65, 0, 118, -66, -90, -91 +}; + +/* YYTABLE[YYPACT[STATE-NUM]]. What to do in state STATE-NUM. If + positive, shift that token. If negative, reduce the rule which + number is the opposite. If YYTABLE_NINF, syntax error. */ +#define YYTABLE_NINF -86 +static const yytype_int16 yytable[] = +{ + 10, 88, 89, 150, 151, 152, 153, 154, 155, 135, + 54, 123, 56, 11, 58, 126, 145, 62, 164, 2, + 146, 136, 1, 1, 148, 149, 147, -31, 101, 90, + 91, -31, -31, -31, -31, -31, -31, -31, -31, 102, + 162, -31, -31, 103, -31, 104, 105, 106, 107, 108, + -31, 109, 181, 110, 2, 52, 55, 65, 66, 172, + 57, 182, 111, 8, 9, 67, 131, 59, 140, 92, + 68, 61, 145, 133, 180, 142, 146, 65, 66, -5, + 12, 90, 91, 13, 14, 15, 16, 17, 18, 19, + 20, 71, 174, 21, 22, 23, 24, 25, 26, 27, + 28, 29, 30, 159, 63, 31, 187, 127, 130, 64, + 139, 2, 90, 91, 32, -33, 101, 72, 169, -33, + -33, -33, -33, -33, -33, -33, -33, 102, 73, -33, + -33, 103, -33, 104, 105, 106, 107, 108, -33, 109, + 52, 110, 129, 134, 82, 143, 83, -4, 12, 84, + 111, 13, 14, 15, 16, 17, 18, 19, 20, 90, + 91, 21, 22, 23, 24, 25, 26, 27, 28, 29, + 30, 85, 86, 31, 188, 90, 91, 87, 99, -85, + 101, 100, 32, -85, -85, -85, -85, -85, -85, -85, + -85, 156, 198, -85, -85, 103, -85, -85, -85, -85, + -85, -85, -85, 157, 163, 110, 158, 166, 101, 167, + 168, 171, -52, -52, 144, -52, -52, -52, -52, 102, + 91, -52, -52, 103, 118, 119, 120, 121, 172, 176, + 183, 101, 185, 110, -76, -76, -76, -76, -76, -76, + -76, -76, 122, 189, -76, -76, 103, 13, 14, 15, + 16, 17, 18, 19, 20, 190, 110, 21, 22, 191, + 193, 195, 196, 14, 15, 144, 17, 18, 19, 20, + 197, 53, 21, 22, 51, 75, 125, 175, 32, 177, + 178, 179, 93, 94, 95, 96, 97, 184, 137, 186, + 170, 0, 98, 32, 0, 0, 0, 0, 192 +}; + +#define yypact_value_is_default(yystate) \ + ((yystate) == (-91)) + +#define yytable_value_is_error(yytable_value) \ + YYID (0) + +static const yytype_int16 yycheck[] = +{ + 1, 67, 68, 93, 94, 95, 96, 97, 98, 23, + 10, 76, 13, 0, 15, 76, 81, 18, 108, 35, + 81, 35, 3, 3, 90, 91, 33, 0, 1, 36, + 37, 4, 5, 6, 7, 8, 9, 10, 11, 12, + 106, 14, 15, 16, 17, 18, 19, 20, 21, 22, + 23, 24, 26, 26, 35, 35, 35, 26, 27, 14, + 35, 35, 35, 26, 27, 34, 78, 26, 80, 69, + 39, 35, 137, 78, 164, 80, 137, 26, 27, 0, + 1, 36, 37, 4, 5, 6, 7, 8, 9, 10, + 11, 35, 158, 14, 15, 16, 17, 18, 19, 20, + 21, 22, 23, 104, 26, 26, 172, 77, 78, 26, + 80, 35, 36, 37, 35, 0, 1, 1, 119, 4, + 5, 6, 7, 8, 9, 10, 11, 12, 1, 14, + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, + 35, 26, 77, 78, 35, 80, 35, 0, 1, 35, + 35, 4, 5, 6, 7, 8, 9, 10, 11, 36, + 37, 14, 15, 16, 17, 18, 19, 20, 21, 22, + 23, 35, 35, 26, 35, 36, 37, 35, 35, 0, + 1, 35, 35, 4, 5, 6, 7, 8, 9, 10, + 11, 35, 193, 14, 15, 16, 17, 18, 19, 20, + 21, 22, 23, 35, 26, 26, 25, 1, 1, 13, + 35, 26, 5, 6, 35, 8, 9, 10, 11, 12, + 37, 14, 15, 16, 17, 18, 19, 20, 14, 35, + 35, 1, 35, 26, 4, 5, 6, 7, 8, 9, + 10, 11, 35, 35, 14, 15, 16, 4, 5, 6, + 7, 8, 9, 10, 11, 35, 26, 14, 15, 35, + 38, 35, 35, 5, 6, 35, 8, 9, 10, 11, + 35, 7, 14, 15, 6, 37, 76, 159, 35, 161, + 162, 163, 28, 29, 30, 31, 32, 169, 79, 171, + 120, -1, 38, 35, -1, -1, -1, -1, 180 +}; + +/* YYSTOS[STATE-NUM] -- The (internal number of the) accessing + symbol of state STATE-NUM. */ +static const yytype_uint8 yystos[] = +{ + 0, 3, 35, 41, 42, 43, 67, 85, 26, 27, + 83, 0, 1, 4, 5, 6, 7, 8, 9, 10, + 11, 14, 15, 16, 17, 18, 19, 20, 21, 22, + 23, 26, 35, 44, 45, 47, 48, 49, 50, 56, + 57, 59, 63, 65, 68, 69, 71, 73, 74, 75, + 84, 43, 35, 42, 85, 35, 83, 35, 83, 26, + 89, 35, 83, 26, 26, 26, 27, 34, 39, 87, + 88, 35, 1, 1, 51, 51, 60, 62, 66, 80, + 72, 78, 35, 35, 35, 35, 35, 35, 87, 87, + 36, 37, 85, 28, 29, 30, 31, 32, 38, 35, + 35, 1, 12, 16, 18, 19, 20, 21, 22, 24, + 26, 35, 46, 52, 53, 76, 77, 79, 17, 18, + 19, 20, 35, 46, 61, 77, 79, 45, 58, 84, + 45, 59, 64, 71, 84, 23, 35, 78, 81, 45, + 59, 70, 71, 84, 35, 46, 79, 33, 87, 87, + 88, 88, 88, 88, 88, 88, 35, 35, 25, 83, + 82, 83, 87, 26, 88, 54, 1, 13, 35, 83, + 82, 26, 14, 86, 87, 86, 35, 86, 86, 86, + 88, 26, 35, 35, 86, 35, 86, 87, 35, 35, + 35, 35, 86, 38, 55, 35, 35, 35, 83 +}; + +#define yyerrok (yyerrstatus = 0) +#define yyclearin (yychar = YYEMPTY) +#define YYEMPTY (-2) +#define YYEOF 0 + +#define YYACCEPT goto yyacceptlab +#define YYABORT goto yyabortlab +#define YYERROR goto yyerrorlab + + +/* Like YYERROR except do call yyerror. This remains here temporarily + to ease the transition to the new meaning of YYERROR, for GCC. + Once GCC version 2 has supplanted version 1, this can go. However, + YYFAIL appears to be in use. Nevertheless, it is formally deprecated + in Bison 2.4.2's NEWS entry, where a plan to phase it out is + discussed. */ + +#define YYFAIL goto yyerrlab +#if defined YYFAIL + /* This is here to suppress warnings from the GCC cpp's + -Wunused-macros. Normally we don't worry about that warning, but + some users do, and we want to make it easy for users to remove + YYFAIL uses, which will produce warnings from Bison 2.5. */ +#endif + +#define YYRECOVERING() (!!yyerrstatus) + +#define YYBACKUP(Token, Value) \ +do \ + if (yychar == YYEMPTY) \ + { \ + yychar = (Token); \ + yylval = (Value); \ + YYPOPSTACK (yylen); \ + yystate = *yyssp; \ + goto yybackup; \ + } \ + else \ + { \ + yyerror (YY_("syntax error: cannot back up")); \ + YYERROR; \ + } \ +while (YYID (0)) + + +#define YYTERROR 1 +#define YYERRCODE 256 + + +/* YYLLOC_DEFAULT -- Set CURRENT to span from RHS[1] to RHS[N]. + If N is 0, then set CURRENT to the empty location which ends + the previous symbol: RHS[0] (always defined). */ + +#define YYRHSLOC(Rhs, K) ((Rhs)[K]) +#ifndef YYLLOC_DEFAULT +# define YYLLOC_DEFAULT(Current, Rhs, N) \ + do \ + if (YYID (N)) \ + { \ + (Current).first_line = YYRHSLOC (Rhs, 1).first_line; \ + (Current).first_column = YYRHSLOC (Rhs, 1).first_column; \ + (Current).last_line = YYRHSLOC (Rhs, N).last_line; \ + (Current).last_column = YYRHSLOC (Rhs, N).last_column; \ + } \ + else \ + { \ + (Current).first_line = (Current).last_line = \ + YYRHSLOC (Rhs, 0).last_line; \ + (Current).first_column = (Current).last_column = \ + YYRHSLOC (Rhs, 0).last_column; \ + } \ + while (YYID (0)) +#endif + + +/* This macro is provided for backward compatibility. */ + +#ifndef YY_LOCATION_PRINT +# define YY_LOCATION_PRINT(File, Loc) ((void) 0) +#endif + + +/* YYLEX -- calling `yylex' with the right arguments. */ + +#ifdef YYLEX_PARAM +# define YYLEX yylex (YYLEX_PARAM) +#else +# define YYLEX yylex () +#endif + +/* Enable debugging if requested. */ +#if YYDEBUG + +# ifndef YYFPRINTF +# include /* INFRINGES ON USER NAME SPACE */ +# define YYFPRINTF fprintf +# endif + +# define YYDPRINTF(Args) \ +do { \ + if (yydebug) \ + YYFPRINTF Args; \ +} while (YYID (0)) + +# define YY_SYMBOL_PRINT(Title, Type, Value, Location) \ +do { \ + if (yydebug) \ + { \ + YYFPRINTF (stderr, "%s ", Title); \ + yy_symbol_print (stderr, \ + Type, Value); \ + YYFPRINTF (stderr, "\n"); \ + } \ +} while (YYID (0)) + + +/*--------------------------------. +| Print this symbol on YYOUTPUT. | +`--------------------------------*/ + +/*ARGSUSED*/ +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static void +yy_symbol_value_print (FILE *yyoutput, int yytype, YYSTYPE const * const yyvaluep) +#else +static void +yy_symbol_value_print (yyoutput, yytype, yyvaluep) + FILE *yyoutput; + int yytype; + YYSTYPE const * const yyvaluep; +#endif +{ + FILE *yyo = yyoutput; + YYUSE (yyo); + if (!yyvaluep) + return; +# ifdef YYPRINT + if (yytype < YYNTOKENS) + YYPRINT (yyoutput, yytoknum[yytype], *yyvaluep); +# else + YYUSE (yyoutput); +# endif + switch (yytype) + { + default: + break; + } +} + + +/*--------------------------------. +| Print this symbol on YYOUTPUT. | +`--------------------------------*/ + +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static void +yy_symbol_print (FILE *yyoutput, int yytype, YYSTYPE const * const yyvaluep) +#else +static void +yy_symbol_print (yyoutput, yytype, yyvaluep) + FILE *yyoutput; + int yytype; + YYSTYPE const * const yyvaluep; +#endif +{ + if (yytype < YYNTOKENS) + YYFPRINTF (yyoutput, "token %s (", yytname[yytype]); + else + YYFPRINTF (yyoutput, "nterm %s (", yytname[yytype]); + + yy_symbol_value_print (yyoutput, yytype, yyvaluep); + YYFPRINTF (yyoutput, ")"); +} + +/*------------------------------------------------------------------. +| yy_stack_print -- Print the state stack from its BOTTOM up to its | +| TOP (included). | +`------------------------------------------------------------------*/ + +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static void +yy_stack_print (yytype_int16 *yybottom, yytype_int16 *yytop) +#else +static void +yy_stack_print (yybottom, yytop) + yytype_int16 *yybottom; + yytype_int16 *yytop; +#endif +{ + YYFPRINTF (stderr, "Stack now"); + for (; yybottom <= yytop; yybottom++) + { + int yybot = *yybottom; + YYFPRINTF (stderr, " %d", yybot); + } + YYFPRINTF (stderr, "\n"); +} + +# define YY_STACK_PRINT(Bottom, Top) \ +do { \ + if (yydebug) \ + yy_stack_print ((Bottom), (Top)); \ +} while (YYID (0)) + + +/*------------------------------------------------. +| Report that the YYRULE is going to be reduced. | +`------------------------------------------------*/ + +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static void +yy_reduce_print (YYSTYPE *yyvsp, int yyrule) +#else +static void +yy_reduce_print (yyvsp, yyrule) + YYSTYPE *yyvsp; + int yyrule; +#endif +{ + int yynrhs = yyr2[yyrule]; + int yyi; + unsigned long int yylno = yyrline[yyrule]; + YYFPRINTF (stderr, "Reducing stack by rule %d (line %lu):\n", + yyrule - 1, yylno); + /* The symbols being reduced. */ + for (yyi = 0; yyi < yynrhs; yyi++) + { + YYFPRINTF (stderr, " $%d = ", yyi + 1); + yy_symbol_print (stderr, yyrhs[yyprhs[yyrule] + yyi], + &(yyvsp[(yyi + 1) - (yynrhs)]) + ); + YYFPRINTF (stderr, "\n"); + } +} + +# define YY_REDUCE_PRINT(Rule) \ +do { \ + if (yydebug) \ + yy_reduce_print (yyvsp, Rule); \ +} while (YYID (0)) + +/* Nonzero means print parse trace. It is left uninitialized so that + multiple parsers can coexist. */ +int yydebug; +#else /* !YYDEBUG */ +# define YYDPRINTF(Args) +# define YY_SYMBOL_PRINT(Title, Type, Value, Location) +# define YY_STACK_PRINT(Bottom, Top) +# define YY_REDUCE_PRINT(Rule) +#endif /* !YYDEBUG */ + + +/* YYINITDEPTH -- initial size of the parser's stacks. */ +#ifndef YYINITDEPTH +# define YYINITDEPTH 200 +#endif + +/* YYMAXDEPTH -- maximum size the stacks can grow to (effective only + if the built-in stack extension method is used). + + Do not make this value too large; the results are undefined if + YYSTACK_ALLOC_MAXIMUM < YYSTACK_BYTES (YYMAXDEPTH) + evaluated with infinite-precision integer arithmetic. */ + +#ifndef YYMAXDEPTH +# define YYMAXDEPTH 10000 +#endif + + +#if YYERROR_VERBOSE + +# ifndef yystrlen +# if defined __GLIBC__ && defined _STRING_H +# define yystrlen strlen +# else +/* Return the length of YYSTR. */ +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static YYSIZE_T +yystrlen (const char *yystr) +#else +static YYSIZE_T +yystrlen (yystr) + const char *yystr; +#endif +{ + YYSIZE_T yylen; + for (yylen = 0; yystr[yylen]; yylen++) + continue; + return yylen; +} +# endif +# endif + +# ifndef yystpcpy +# if defined __GLIBC__ && defined _STRING_H && defined _GNU_SOURCE +# define yystpcpy stpcpy +# else +/* Copy YYSRC to YYDEST, returning the address of the terminating '\0' in + YYDEST. */ +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static char * +yystpcpy (char *yydest, const char *yysrc) +#else +static char * +yystpcpy (yydest, yysrc) + char *yydest; + const char *yysrc; +#endif +{ + char *yyd = yydest; + const char *yys = yysrc; + + while ((*yyd++ = *yys++) != '\0') + continue; + + return yyd - 1; +} +# endif +# endif + +# ifndef yytnamerr +/* Copy to YYRES the contents of YYSTR after stripping away unnecessary + quotes and backslashes, so that it's suitable for yyerror. The + heuristic is that double-quoting is unnecessary unless the string + contains an apostrophe, a comma, or backslash (other than + backslash-backslash). YYSTR is taken from yytname. If YYRES is + null, do not copy; instead, return the length of what the result + would have been. */ +static YYSIZE_T +yytnamerr (char *yyres, const char *yystr) +{ + if (*yystr == '"') + { + YYSIZE_T yyn = 0; + char const *yyp = yystr; + + for (;;) + switch (*++yyp) + { + case '\'': + case ',': + goto do_not_strip_quotes; + + case '\\': + if (*++yyp != '\\') + goto do_not_strip_quotes; + /* Fall through. */ + default: + if (yyres) + yyres[yyn] = *yyp; + yyn++; + break; + + case '"': + if (yyres) + yyres[yyn] = '\0'; + return yyn; + } + do_not_strip_quotes: ; + } + + if (! yyres) + return yystrlen (yystr); + + return yystpcpy (yyres, yystr) - yyres; +} +# endif + +/* Copy into *YYMSG, which is of size *YYMSG_ALLOC, an error message + about the unexpected token YYTOKEN for the state stack whose top is + YYSSP. + + Return 0 if *YYMSG was successfully written. Return 1 if *YYMSG is + not large enough to hold the message. In that case, also set + *YYMSG_ALLOC to the required number of bytes. Return 2 if the + required number of bytes is too large to store. */ +static int +yysyntax_error (YYSIZE_T *yymsg_alloc, char **yymsg, + yytype_int16 *yyssp, int yytoken) +{ + YYSIZE_T yysize0 = yytnamerr (YY_NULL, yytname[yytoken]); + YYSIZE_T yysize = yysize0; + YYSIZE_T yysize1; + enum { YYERROR_VERBOSE_ARGS_MAXIMUM = 5 }; + /* Internationalized format string. */ + const char *yyformat = YY_NULL; + /* Arguments of yyformat. */ + char const *yyarg[YYERROR_VERBOSE_ARGS_MAXIMUM]; + /* Number of reported tokens (one for the "unexpected", one per + "expected"). */ + int yycount = 0; + + /* There are many possibilities here to consider: + - Assume YYFAIL is not used. It's too flawed to consider. See + + for details. YYERROR is fine as it does not invoke this + function. + - If this state is a consistent state with a default action, then + the only way this function was invoked is if the default action + is an error action. In that case, don't check for expected + tokens because there are none. + - The only way there can be no lookahead present (in yychar) is if + this state is a consistent state with a default action. Thus, + detecting the absence of a lookahead is sufficient to determine + that there is no unexpected or expected token to report. In that + case, just report a simple "syntax error". + - Don't assume there isn't a lookahead just because this state is a + consistent state with a default action. There might have been a + previous inconsistent state, consistent state with a non-default + action, or user semantic action that manipulated yychar. + - Of course, the expected token list depends on states to have + correct lookahead information, and it depends on the parser not + to perform extra reductions after fetching a lookahead from the + scanner and before detecting a syntax error. Thus, state merging + (from LALR or IELR) and default reductions corrupt the expected + token list. However, the list is correct for canonical LR with + one exception: it will still contain any token that will not be + accepted due to an error action in a later state. + */ + if (yytoken != YYEMPTY) + { + int yyn = yypact[*yyssp]; + yyarg[yycount++] = yytname[yytoken]; + if (!yypact_value_is_default (yyn)) + { + /* Start YYX at -YYN if negative to avoid negative indexes in + YYCHECK. In other words, skip the first -YYN actions for + this state because they are default actions. */ + int yyxbegin = yyn < 0 ? -yyn : 0; + /* Stay within bounds of both yycheck and yytname. */ + int yychecklim = YYLAST - yyn + 1; + int yyxend = yychecklim < YYNTOKENS ? yychecklim : YYNTOKENS; + int yyx; + + for (yyx = yyxbegin; yyx < yyxend; ++yyx) + if (yycheck[yyx + yyn] == yyx && yyx != YYTERROR + && !yytable_value_is_error (yytable[yyx + yyn])) + { + if (yycount == YYERROR_VERBOSE_ARGS_MAXIMUM) + { + yycount = 1; + yysize = yysize0; + break; + } + yyarg[yycount++] = yytname[yyx]; + yysize1 = yysize + yytnamerr (YY_NULL, yytname[yyx]); + if (! (yysize <= yysize1 + && yysize1 <= YYSTACK_ALLOC_MAXIMUM)) + return 2; + yysize = yysize1; + } + } + } + + switch (yycount) + { +# define YYCASE_(N, S) \ + case N: \ + yyformat = S; \ + break + YYCASE_(0, YY_("syntax error")); + YYCASE_(1, YY_("syntax error, unexpected %s")); + YYCASE_(2, YY_("syntax error, unexpected %s, expecting %s")); + YYCASE_(3, YY_("syntax error, unexpected %s, expecting %s or %s")); + YYCASE_(4, YY_("syntax error, unexpected %s, expecting %s or %s or %s")); + YYCASE_(5, YY_("syntax error, unexpected %s, expecting %s or %s or %s or %s")); +# undef YYCASE_ + } + + yysize1 = yysize + yystrlen (yyformat); + if (! (yysize <= yysize1 && yysize1 <= YYSTACK_ALLOC_MAXIMUM)) + return 2; + yysize = yysize1; + + if (*yymsg_alloc < yysize) + { + *yymsg_alloc = 2 * yysize; + if (! (yysize <= *yymsg_alloc + && *yymsg_alloc <= YYSTACK_ALLOC_MAXIMUM)) + *yymsg_alloc = YYSTACK_ALLOC_MAXIMUM; + return 1; + } + + /* Avoid sprintf, as that infringes on the user's name space. + Don't have undefined behavior even if the translation + produced a string with the wrong number of "%s"s. */ + { + char *yyp = *yymsg; + int yyi = 0; + while ((*yyp = *yyformat) != '\0') + if (*yyp == '%' && yyformat[1] == 's' && yyi < yycount) + { + yyp += yytnamerr (yyp, yyarg[yyi++]); + yyformat += 2; + } + else + { + yyp++; + yyformat++; + } + } + return 0; +} +#endif /* YYERROR_VERBOSE */ + +/*-----------------------------------------------. +| Release the memory associated to this symbol. | +`-----------------------------------------------*/ + +/*ARGSUSED*/ +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +static void +yydestruct (const char *yymsg, int yytype, YYSTYPE *yyvaluep) +#else +static void +yydestruct (yymsg, yytype, yyvaluep) + const char *yymsg; + int yytype; + YYSTYPE *yyvaluep; +#endif +{ + YYUSE (yyvaluep); + + if (!yymsg) + yymsg = "Deleting"; + YY_SYMBOL_PRINT (yymsg, yytype, yyvaluep, yylocationp); + + switch (yytype) + { + case 57: /* "choice_entry" */ + + { + fprintf(stderr, "%s:%d: missing end statement for this entry\n", + (yyvaluep->menu)->file->name, (yyvaluep->menu)->lineno); + if (current_menu == (yyvaluep->menu)) + menu_end_menu(); +}; + + break; + case 63: /* "if_entry" */ + + { + fprintf(stderr, "%s:%d: missing end statement for this entry\n", + (yyvaluep->menu)->file->name, (yyvaluep->menu)->lineno); + if (current_menu == (yyvaluep->menu)) + menu_end_menu(); +}; + + break; + case 69: /* "menu_entry" */ + + { + fprintf(stderr, "%s:%d: missing end statement for this entry\n", + (yyvaluep->menu)->file->name, (yyvaluep->menu)->lineno); + if (current_menu == (yyvaluep->menu)) + menu_end_menu(); +}; + + break; + + default: + break; + } +} + + +/* Prevent warnings from -Wmissing-prototypes. */ +#ifdef YYPARSE_PARAM +#if defined __STDC__ || defined __cplusplus +int yyparse (void *YYPARSE_PARAM); +#else +int yyparse (); +#endif +#else /* ! YYPARSE_PARAM */ +#if defined __STDC__ || defined __cplusplus +int yyparse (void); +#else +int yyparse (); +#endif +#endif /* ! YYPARSE_PARAM */ + + +/* The lookahead symbol. */ +int yychar; + +/* The semantic value of the lookahead symbol. */ +YYSTYPE yylval; + +/* Number of syntax errors so far. */ +int yynerrs; + + +/*----------. +| yyparse. | +`----------*/ + +#ifdef YYPARSE_PARAM +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +int +yyparse (void *YYPARSE_PARAM) +#else +int +yyparse (YYPARSE_PARAM) + void *YYPARSE_PARAM; +#endif +#else /* ! YYPARSE_PARAM */ +#if (defined __STDC__ || defined __C99__FUNC__ \ + || defined __cplusplus || defined _MSC_VER) +int +yyparse (void) +#else +int +yyparse () + +#endif +#endif +{ + int yystate; + /* Number of tokens to shift before error messages enabled. */ + int yyerrstatus; + + /* The stacks and their tools: + `yyss': related to states. + `yyvs': related to semantic values. + + Refer to the stacks through separate pointers, to allow yyoverflow + to reallocate them elsewhere. */ + + /* The state stack. */ + yytype_int16 yyssa[YYINITDEPTH]; + yytype_int16 *yyss; + yytype_int16 *yyssp; + + /* The semantic value stack. */ + YYSTYPE yyvsa[YYINITDEPTH]; + YYSTYPE *yyvs; + YYSTYPE *yyvsp; + + YYSIZE_T yystacksize; + + int yyn; + int yyresult; + /* Lookahead token as an internal (translated) token number. */ + int yytoken; + /* The variables used to return semantic value and location from the + action routines. */ + YYSTYPE yyval; + +#if YYERROR_VERBOSE + /* Buffer for error messages, and its allocated size. */ + char yymsgbuf[128]; + char *yymsg = yymsgbuf; + YYSIZE_T yymsg_alloc = sizeof yymsgbuf; +#endif + +#define YYPOPSTACK(N) (yyvsp -= (N), yyssp -= (N)) + + /* The number of symbols on the RHS of the reduced rule. + Keep to zero when no symbol should be popped. */ + int yylen = 0; + + yytoken = 0; + yyss = yyssa; + yyvs = yyvsa; + yystacksize = YYINITDEPTH; + + YYDPRINTF ((stderr, "Starting parse\n")); + + yystate = 0; + yyerrstatus = 0; + yynerrs = 0; + yychar = YYEMPTY; /* Cause a token to be read. */ + + /* Initialize stack pointers. + Waste one element of value and location stack + so that they stay on the same level as the state stack. + The wasted elements are never initialized. */ + yyssp = yyss; + yyvsp = yyvs; + + goto yysetstate; + +/*------------------------------------------------------------. +| yynewstate -- Push a new state, which is found in yystate. | +`------------------------------------------------------------*/ + yynewstate: + /* In all cases, when you get here, the value and location stacks + have just been pushed. So pushing a state here evens the stacks. */ + yyssp++; + + yysetstate: + *yyssp = yystate; + + if (yyss + yystacksize - 1 <= yyssp) + { + /* Get the current used size of the three stacks, in elements. */ + YYSIZE_T yysize = yyssp - yyss + 1; + +#ifdef yyoverflow + { + /* Give user a chance to reallocate the stack. Use copies of + these so that the &'s don't force the real ones into + memory. */ + YYSTYPE *yyvs1 = yyvs; + yytype_int16 *yyss1 = yyss; + + /* Each stack pointer address is followed by the size of the + data in use in that stack, in bytes. This used to be a + conditional around just the two extra args, but that might + be undefined if yyoverflow is a macro. */ + yyoverflow (YY_("memory exhausted"), + &yyss1, yysize * sizeof (*yyssp), + &yyvs1, yysize * sizeof (*yyvsp), + &yystacksize); + + yyss = yyss1; + yyvs = yyvs1; + } +#else /* no yyoverflow */ +# ifndef YYSTACK_RELOCATE + goto yyexhaustedlab; +# else + /* Extend the stack our own way. */ + if (YYMAXDEPTH <= yystacksize) + goto yyexhaustedlab; + yystacksize *= 2; + if (YYMAXDEPTH < yystacksize) + yystacksize = YYMAXDEPTH; + + { + yytype_int16 *yyss1 = yyss; + union yyalloc *yyptr = + (union yyalloc *) YYSTACK_ALLOC (YYSTACK_BYTES (yystacksize)); + if (! yyptr) + goto yyexhaustedlab; + YYSTACK_RELOCATE (yyss_alloc, yyss); + YYSTACK_RELOCATE (yyvs_alloc, yyvs); +# undef YYSTACK_RELOCATE + if (yyss1 != yyssa) + YYSTACK_FREE (yyss1); + } +# endif +#endif /* no yyoverflow */ + + yyssp = yyss + yysize - 1; + yyvsp = yyvs + yysize - 1; + + YYDPRINTF ((stderr, "Stack size increased to %lu\n", + (unsigned long int) yystacksize)); + + if (yyss + yystacksize - 1 <= yyssp) + YYABORT; + } + + YYDPRINTF ((stderr, "Entering state %d\n", yystate)); + + if (yystate == YYFINAL) + YYACCEPT; + + goto yybackup; + +/*-----------. +| yybackup. | +`-----------*/ +yybackup: + + /* Do appropriate processing given the current state. Read a + lookahead token if we need one and don't already have one. */ + + /* First try to decide what to do without reference to lookahead token. */ + yyn = yypact[yystate]; + if (yypact_value_is_default (yyn)) + goto yydefault; + + /* Not known => get a lookahead token if don't already have one. */ + + /* YYCHAR is either YYEMPTY or YYEOF or a valid lookahead symbol. */ + if (yychar == YYEMPTY) + { + YYDPRINTF ((stderr, "Reading a token: ")); + yychar = YYLEX; + } + + if (yychar <= YYEOF) + { + yychar = yytoken = YYEOF; + YYDPRINTF ((stderr, "Now at end of input.\n")); + } + else + { + yytoken = YYTRANSLATE (yychar); + YY_SYMBOL_PRINT ("Next token is", yytoken, &yylval, &yylloc); + } + + /* If the proper action on seeing token YYTOKEN is to reduce or to + detect an error, take that action. */ + yyn += yytoken; + if (yyn < 0 || YYLAST < yyn || yycheck[yyn] != yytoken) + goto yydefault; + yyn = yytable[yyn]; + if (yyn <= 0) + { + if (yytable_value_is_error (yyn)) + goto yyerrlab; + yyn = -yyn; + goto yyreduce; + } + + /* Count tokens shifted since error; after three, turn off error + status. */ + if (yyerrstatus) + yyerrstatus--; + + /* Shift the lookahead token. */ + YY_SYMBOL_PRINT ("Shifting", yytoken, &yylval, &yylloc); + + /* Discard the shifted token. */ + yychar = YYEMPTY; + + yystate = yyn; + *++yyvsp = yylval; + + goto yynewstate; + + +/*-----------------------------------------------------------. +| yydefault -- do the default action for the current state. | +`-----------------------------------------------------------*/ +yydefault: + yyn = yydefact[yystate]; + if (yyn == 0) + goto yyerrlab; + goto yyreduce; + + +/*-----------------------------. +| yyreduce -- Do a reduction. | +`-----------------------------*/ +yyreduce: + /* yyn is the number of a rule to reduce with. */ + yylen = yyr2[yyn]; + + /* If YYLEN is nonzero, implement the default value of the action: + `$$ = $1'. + + Otherwise, the following line sets YYVAL to garbage. + This behavior is undocumented and Bison + users should not rely upon it. Assigning to YYVAL + unconditionally makes the parser a bit smaller, and it avoids a + GCC warning that YYVAL may be used uninitialized. */ + yyval = yyvsp[1-yylen]; + + + YY_REDUCE_PRINT (yyn); + switch (yyn) + { + case 10: + + { zconf_error("unexpected end statement"); } + break; + + case 11: + + { zconf_error("unknown statement \"%s\"", (yyvsp[(2) - (4)].string)); } + break; + + case 12: + + { + zconf_error("unexpected option \"%s\"", kconf_id_strings + (yyvsp[(2) - (4)].id)->name); +} + break; + + case 13: + + { zconf_error("invalid statement"); } + break; + + case 28: + + { zconf_error("unknown option \"%s\"", (yyvsp[(1) - (3)].string)); } + break; + + case 29: + + { zconf_error("invalid option"); } + break; + + case 30: + + { + struct symbol *sym = sym_lookup((yyvsp[(2) - (3)].string), 0); + sym->flags |= SYMBOL_OPTIONAL; + menu_add_entry(sym); + printd(DEBUG_PARSE, "%s:%d:config %s\n", zconf_curname(), zconf_lineno(), (yyvsp[(2) - (3)].string)); +} + break; + + case 31: + + { + menu_end_entry(); + printd(DEBUG_PARSE, "%s:%d:endconfig\n", zconf_curname(), zconf_lineno()); +} + break; + + case 32: + + { + struct symbol *sym = sym_lookup((yyvsp[(2) - (3)].string), 0); + sym->flags |= SYMBOL_OPTIONAL; + menu_add_entry(sym); + printd(DEBUG_PARSE, "%s:%d:menuconfig %s\n", zconf_curname(), zconf_lineno(), (yyvsp[(2) - (3)].string)); +} + break; + + case 33: + + { + if (current_entry->prompt) + current_entry->prompt->type = P_MENU; + else + zconfprint("warning: menuconfig statement without prompt"); + menu_end_entry(); + printd(DEBUG_PARSE, "%s:%d:endconfig\n", zconf_curname(), zconf_lineno()); +} + break; + + case 41: + + { + menu_set_type((yyvsp[(1) - (3)].id)->stype); + printd(DEBUG_PARSE, "%s:%d:type(%u)\n", + zconf_curname(), zconf_lineno(), + (yyvsp[(1) - (3)].id)->stype); +} + break; + + case 42: + + { + menu_add_prompt(P_PROMPT, (yyvsp[(2) - (4)].string), (yyvsp[(3) - (4)].expr)); + printd(DEBUG_PARSE, "%s:%d:prompt\n", zconf_curname(), zconf_lineno()); +} + break; + + case 43: + + { + menu_add_expr(P_DEFAULT, (yyvsp[(2) - (4)].expr), (yyvsp[(3) - (4)].expr)); + if ((yyvsp[(1) - (4)].id)->stype != S_UNKNOWN) + menu_set_type((yyvsp[(1) - (4)].id)->stype); + printd(DEBUG_PARSE, "%s:%d:default(%u)\n", + zconf_curname(), zconf_lineno(), + (yyvsp[(1) - (4)].id)->stype); +} + break; + + case 44: + + { + menu_add_symbol(P_SELECT, sym_lookup((yyvsp[(2) - (4)].string), 0), (yyvsp[(3) - (4)].expr)); + printd(DEBUG_PARSE, "%s:%d:select\n", zconf_curname(), zconf_lineno()); +} + break; + + case 45: + + { + menu_add_expr(P_RANGE, expr_alloc_comp(E_RANGE,(yyvsp[(2) - (5)].symbol), (yyvsp[(3) - (5)].symbol)), (yyvsp[(4) - (5)].expr)); + printd(DEBUG_PARSE, "%s:%d:range\n", zconf_curname(), zconf_lineno()); +} + break; + + case 48: + + { + const struct kconf_id *id = kconf_id_lookup((yyvsp[(2) - (3)].string), strlen((yyvsp[(2) - (3)].string))); + if (id && id->flags & TF_OPTION) + menu_add_option(id->token, (yyvsp[(3) - (3)].string)); + else + zconfprint("warning: ignoring unknown option %s", (yyvsp[(2) - (3)].string)); + free((yyvsp[(2) - (3)].string)); +} + break; + + case 49: + + { (yyval.string) = NULL; } + break; + + case 50: + + { (yyval.string) = (yyvsp[(2) - (2)].string); } + break; + + case 51: + + { + struct symbol *sym = sym_lookup((yyvsp[(2) - (3)].string), SYMBOL_CHOICE); + sym->flags |= SYMBOL_AUTO; + menu_add_entry(sym); + menu_add_expr(P_CHOICE, NULL, NULL); + printd(DEBUG_PARSE, "%s:%d:choice\n", zconf_curname(), zconf_lineno()); +} + break; + + case 52: + + { + (yyval.menu) = menu_add_menu(); +} + break; + + case 53: + + { + if (zconf_endtoken((yyvsp[(1) - (1)].id), T_CHOICE, T_ENDCHOICE)) { + menu_end_menu(); + printd(DEBUG_PARSE, "%s:%d:endchoice\n", zconf_curname(), zconf_lineno()); + } +} + break; + + case 61: + + { + menu_add_prompt(P_PROMPT, (yyvsp[(2) - (4)].string), (yyvsp[(3) - (4)].expr)); + printd(DEBUG_PARSE, "%s:%d:prompt\n", zconf_curname(), zconf_lineno()); +} + break; + + case 62: + + { + if ((yyvsp[(1) - (3)].id)->stype == S_BOOLEAN || (yyvsp[(1) - (3)].id)->stype == S_TRISTATE) { + menu_set_type((yyvsp[(1) - (3)].id)->stype); + printd(DEBUG_PARSE, "%s:%d:type(%u)\n", + zconf_curname(), zconf_lineno(), + (yyvsp[(1) - (3)].id)->stype); + } else + YYERROR; +} + break; + + case 63: + + { + current_entry->sym->flags |= SYMBOL_OPTIONAL; + printd(DEBUG_PARSE, "%s:%d:optional\n", zconf_curname(), zconf_lineno()); +} + break; + + case 64: + + { + if ((yyvsp[(1) - (4)].id)->stype == S_UNKNOWN) { + menu_add_symbol(P_DEFAULT, sym_lookup((yyvsp[(2) - (4)].string), 0), (yyvsp[(3) - (4)].expr)); + printd(DEBUG_PARSE, "%s:%d:default\n", + zconf_curname(), zconf_lineno()); + } else + YYERROR; +} + break; + + case 67: + + { + printd(DEBUG_PARSE, "%s:%d:if\n", zconf_curname(), zconf_lineno()); + menu_add_entry(NULL); + menu_add_dep((yyvsp[(2) - (3)].expr)); + (yyval.menu) = menu_add_menu(); +} + break; + + case 68: + + { + if (zconf_endtoken((yyvsp[(1) - (1)].id), T_IF, T_ENDIF)) { + menu_end_menu(); + printd(DEBUG_PARSE, "%s:%d:endif\n", zconf_curname(), zconf_lineno()); + } +} + break; + + case 74: + + { + menu_add_prompt(P_MENU, (yyvsp[(2) - (3)].string), NULL); +} + break; + + case 75: + + { + menu_add_entry(NULL); + menu_add_prompt(P_MENU, (yyvsp[(2) - (3)].string), NULL); + printd(DEBUG_PARSE, "%s:%d:menu\n", zconf_curname(), zconf_lineno()); +} + break; + + case 76: + + { + (yyval.menu) = menu_add_menu(); +} + break; + + case 77: + + { + if (zconf_endtoken((yyvsp[(1) - (1)].id), T_MENU, T_ENDMENU)) { + menu_end_menu(); + printd(DEBUG_PARSE, "%s:%d:endmenu\n", zconf_curname(), zconf_lineno()); + } +} + break; + + case 83: + + { + printd(DEBUG_PARSE, "%s:%d:source %s\n", zconf_curname(), zconf_lineno(), (yyvsp[(2) - (3)].string)); + zconf_nextfile((yyvsp[(2) - (3)].string)); +} + break; + + case 84: + + { + menu_add_entry(NULL); + menu_add_prompt(P_COMMENT, (yyvsp[(2) - (3)].string), NULL); + printd(DEBUG_PARSE, "%s:%d:comment\n", zconf_curname(), zconf_lineno()); +} + break; + + case 85: + + { + menu_end_entry(); +} + break; + + case 86: + + { + printd(DEBUG_PARSE, "%s:%d:help\n", zconf_curname(), zconf_lineno()); + zconf_starthelp(); +} + break; + + case 87: + + { + current_entry->help = (yyvsp[(2) - (2)].string); +} + break; + + case 92: + + { + menu_add_dep((yyvsp[(3) - (4)].expr)); + printd(DEBUG_PARSE, "%s:%d:depends on\n", zconf_curname(), zconf_lineno()); +} + break; + + case 96: + + { + menu_add_visibility((yyvsp[(2) - (2)].expr)); +} + break; + + case 98: + + { + menu_add_prompt(P_PROMPT, (yyvsp[(1) - (2)].string), (yyvsp[(2) - (2)].expr)); +} + break; + + case 101: + + { (yyval.id) = (yyvsp[(1) - (2)].id); } + break; + + case 102: + + { (yyval.id) = (yyvsp[(1) - (2)].id); } + break; + + case 103: + + { (yyval.id) = (yyvsp[(1) - (2)].id); } + break; + + case 106: + + { (yyval.expr) = NULL; } + break; + + case 107: + + { (yyval.expr) = (yyvsp[(2) - (2)].expr); } + break; + + case 108: + + { (yyval.expr) = expr_alloc_symbol((yyvsp[(1) - (1)].symbol)); } + break; + + case 109: + + { (yyval.expr) = expr_alloc_comp(E_LTH, (yyvsp[(1) - (3)].symbol), (yyvsp[(3) - (3)].symbol)); } + break; + + case 110: + + { (yyval.expr) = expr_alloc_comp(E_LEQ, (yyvsp[(1) - (3)].symbol), (yyvsp[(3) - (3)].symbol)); } + break; + + case 111: + + { (yyval.expr) = expr_alloc_comp(E_GTH, (yyvsp[(1) - (3)].symbol), (yyvsp[(3) - (3)].symbol)); } + break; + + case 112: + + { (yyval.expr) = expr_alloc_comp(E_GEQ, (yyvsp[(1) - (3)].symbol), (yyvsp[(3) - (3)].symbol)); } + break; + + case 113: + + { (yyval.expr) = expr_alloc_comp(E_EQUAL, (yyvsp[(1) - (3)].symbol), (yyvsp[(3) - (3)].symbol)); } + break; + + case 114: + + { (yyval.expr) = expr_alloc_comp(E_UNEQUAL, (yyvsp[(1) - (3)].symbol), (yyvsp[(3) - (3)].symbol)); } + break; + + case 115: + + { (yyval.expr) = (yyvsp[(2) - (3)].expr); } + break; + + case 116: + + { (yyval.expr) = expr_alloc_one(E_NOT, (yyvsp[(2) - (2)].expr)); } + break; + + case 117: + + { (yyval.expr) = expr_alloc_two(E_OR, (yyvsp[(1) - (3)].expr), (yyvsp[(3) - (3)].expr)); } + break; + + case 118: + + { (yyval.expr) = expr_alloc_two(E_AND, (yyvsp[(1) - (3)].expr), (yyvsp[(3) - (3)].expr)); } + break; + + case 119: + + { (yyval.symbol) = sym_lookup((yyvsp[(1) - (1)].string), 0); free((yyvsp[(1) - (1)].string)); } + break; + + case 120: + + { (yyval.symbol) = sym_lookup((yyvsp[(1) - (1)].string), SYMBOL_CONST); free((yyvsp[(1) - (1)].string)); } + break; + + case 121: + + { (yyval.string) = NULL; } + break; + + + + default: break; + } + /* User semantic actions sometimes alter yychar, and that requires + that yytoken be updated with the new translation. We take the + approach of translating immediately before every use of yytoken. + One alternative is translating here after every semantic action, + but that translation would be missed if the semantic action invokes + YYABORT, YYACCEPT, or YYERROR immediately after altering yychar or + if it invokes YYBACKUP. In the case of YYABORT or YYACCEPT, an + incorrect destructor might then be invoked immediately. In the + case of YYERROR or YYBACKUP, subsequent parser actions might lead + to an incorrect destructor call or verbose syntax error message + before the lookahead is translated. */ + YY_SYMBOL_PRINT ("-> $$ =", yyr1[yyn], &yyval, &yyloc); + + YYPOPSTACK (yylen); + yylen = 0; + YY_STACK_PRINT (yyss, yyssp); + + *++yyvsp = yyval; + + /* Now `shift' the result of the reduction. Determine what state + that goes to, based on the state we popped back to and the rule + number reduced by. */ + + yyn = yyr1[yyn]; + + yystate = yypgoto[yyn - YYNTOKENS] + *yyssp; + if (0 <= yystate && yystate <= YYLAST && yycheck[yystate] == *yyssp) + yystate = yytable[yystate]; + else + yystate = yydefgoto[yyn - YYNTOKENS]; + + goto yynewstate; + + +/*------------------------------------. +| yyerrlab -- here on detecting error | +`------------------------------------*/ +yyerrlab: + /* Make sure we have latest lookahead translation. See comments at + user semantic actions for why this is necessary. */ + yytoken = yychar == YYEMPTY ? YYEMPTY : YYTRANSLATE (yychar); + + /* If not already recovering from an error, report this error. */ + if (!yyerrstatus) + { + ++yynerrs; +#if ! YYERROR_VERBOSE + yyerror (YY_("syntax error")); +#else +# define YYSYNTAX_ERROR yysyntax_error (&yymsg_alloc, &yymsg, \ + yyssp, yytoken) + { + char const *yymsgp = YY_("syntax error"); + int yysyntax_error_status; + yysyntax_error_status = YYSYNTAX_ERROR; + if (yysyntax_error_status == 0) + yymsgp = yymsg; + else if (yysyntax_error_status == 1) + { + if (yymsg != yymsgbuf) + YYSTACK_FREE (yymsg); + yymsg = (char *) YYSTACK_ALLOC (yymsg_alloc); + if (!yymsg) + { + yymsg = yymsgbuf; + yymsg_alloc = sizeof yymsgbuf; + yysyntax_error_status = 2; + } + else + { + yysyntax_error_status = YYSYNTAX_ERROR; + yymsgp = yymsg; + } + } + yyerror (yymsgp); + if (yysyntax_error_status == 2) + goto yyexhaustedlab; + } +# undef YYSYNTAX_ERROR +#endif + } + + + + if (yyerrstatus == 3) + { + /* If just tried and failed to reuse lookahead token after an + error, discard it. */ + + if (yychar <= YYEOF) + { + /* Return failure if at end of input. */ + if (yychar == YYEOF) + YYABORT; + } + else + { + yydestruct ("Error: discarding", + yytoken, &yylval); + yychar = YYEMPTY; + } + } + + /* Else will try to reuse lookahead token after shifting the error + token. */ + goto yyerrlab1; + + +/*---------------------------------------------------. +| yyerrorlab -- error raised explicitly by YYERROR. | +`---------------------------------------------------*/ +yyerrorlab: + + /* Pacify compilers like GCC when the user code never invokes + YYERROR and the label yyerrorlab therefore never appears in user + code. */ + if (/*CONSTCOND*/ 0) + goto yyerrorlab; + + /* Do not reclaim the symbols of the rule which action triggered + this YYERROR. */ + YYPOPSTACK (yylen); + yylen = 0; + YY_STACK_PRINT (yyss, yyssp); + yystate = *yyssp; + goto yyerrlab1; + + +/*-------------------------------------------------------------. +| yyerrlab1 -- common code for both syntax error and YYERROR. | +`-------------------------------------------------------------*/ +yyerrlab1: + yyerrstatus = 3; /* Each real token shifted decrements this. */ + + for (;;) + { + yyn = yypact[yystate]; + if (!yypact_value_is_default (yyn)) + { + yyn += YYTERROR; + if (0 <= yyn && yyn <= YYLAST && yycheck[yyn] == YYTERROR) + { + yyn = yytable[yyn]; + if (0 < yyn) + break; + } + } + + /* Pop the current state because it cannot handle the error token. */ + if (yyssp == yyss) + YYABORT; + + + yydestruct ("Error: popping", + yystos[yystate], yyvsp); + YYPOPSTACK (1); + yystate = *yyssp; + YY_STACK_PRINT (yyss, yyssp); + } + + *++yyvsp = yylval; + + + /* Shift the error token. */ + YY_SYMBOL_PRINT ("Shifting", yystos[yyn], yyvsp, yylsp); + + yystate = yyn; + goto yynewstate; + + +/*-------------------------------------. +| yyacceptlab -- YYACCEPT comes here. | +`-------------------------------------*/ +yyacceptlab: + yyresult = 0; + goto yyreturn; + +/*-----------------------------------. +| yyabortlab -- YYABORT comes here. | +`-----------------------------------*/ +yyabortlab: + yyresult = 1; + goto yyreturn; + +#if !defined yyoverflow || YYERROR_VERBOSE +/*-------------------------------------------------. +| yyexhaustedlab -- memory exhaustion comes here. | +`-------------------------------------------------*/ +yyexhaustedlab: + yyerror (YY_("memory exhausted")); + yyresult = 2; + /* Fall through. */ +#endif + +yyreturn: + if (yychar != YYEMPTY) + { + /* Make sure we have latest lookahead translation. See comments at + user semantic actions for why this is necessary. */ + yytoken = YYTRANSLATE (yychar); + yydestruct ("Cleanup: discarding lookahead", + yytoken, &yylval); + } + /* Do not reclaim the symbols of the rule which action triggered + this YYABORT or YYACCEPT. */ + YYPOPSTACK (yylen); + YY_STACK_PRINT (yyss, yyssp); + while (yyssp != yyss) + { + yydestruct ("Cleanup: popping", + yystos[*yyssp], yyvsp); + YYPOPSTACK (1); + } +#ifndef yyoverflow + if (yyss != yyssa) + YYSTACK_FREE (yyss); +#endif +#if YYERROR_VERBOSE + if (yymsg != yymsgbuf) + YYSTACK_FREE (yymsg); +#endif + /* Make sure YYID is used. */ + return YYID (yyresult); +} + + + + + +void conf_parse(const char *name) +{ + struct symbol *sym; + int i; + + zconf_initscan(name); + + sym_init(); + _menu_init(); + rootmenu.prompt = menu_add_prompt(P_MENU, "Crazyflie Platform Configuration", NULL); + + if (getenv("ZCONF_DEBUG")) + zconfdebug = 1; + zconfparse(); + if (zconfnerrs) + exit(1); + if (!modules_sym) + modules_sym = sym_find( "n" ); + + rootmenu.prompt->text = _(rootmenu.prompt->text); + rootmenu.prompt->text = sym_expand_string_value(rootmenu.prompt->text); + + menu_finalize(&rootmenu); + for_all_symbols(i, sym) { + if (sym_check_deps(sym)) + zconfnerrs++; + } + if (zconfnerrs) + exit(1); + sym_set_change_count(1); +} + +static const char *zconf_tokenname(int token) +{ + switch (token) { + case T_MENU: return "menu"; + case T_ENDMENU: return "endmenu"; + case T_CHOICE: return "choice"; + case T_ENDCHOICE: return "endchoice"; + case T_IF: return "if"; + case T_ENDIF: return "endif"; + case T_DEPENDS: return "depends"; + case T_VISIBLE: return "visible"; + } + return ""; +} + +static bool zconf_endtoken(const struct kconf_id *id, int starttoken, int endtoken) +{ + if (id->token != endtoken) { + zconf_error("unexpected '%s' within %s block", + kconf_id_strings + id->name, zconf_tokenname(starttoken)); + zconfnerrs++; + return false; + } + if (current_menu->file != current_file) { + zconf_error("'%s' in different file than '%s'", + kconf_id_strings + id->name, zconf_tokenname(starttoken)); + fprintf(stderr, "%s:%d: location of the '%s'\n", + current_menu->file->name, current_menu->lineno, + zconf_tokenname(starttoken)); + zconfnerrs++; + return false; + } + return true; +} + +static void zconfprint(const char *err, ...) +{ + va_list ap; + + fprintf(stderr, "%s:%d: ", zconf_curname(), zconf_lineno()); + va_start(ap, err); + vfprintf(stderr, err, ap); + va_end(ap); + fprintf(stderr, "\n"); +} + +static void zconf_error(const char *err, ...) +{ + va_list ap; + + zconfnerrs++; + fprintf(stderr, "%s:%d: ", zconf_curname(), zconf_lineno()); + va_start(ap, err); + vfprintf(stderr, err, ap); + va_end(ap); + fprintf(stderr, "\n"); +} + +static void zconferror(const char *err) +{ + fprintf(stderr, "%s:%d: %s\n", zconf_curname(), zconf_lineno() + 1, err); +} + +static void print_quoted_string(FILE *out, const char *str) +{ + const char *p; + int len; + + putc('"', out); + while ((p = strchr(str, '"'))) { + len = p - str; + if (len) + fprintf(out, "%.*s", len, str); + fputs("\\\"", out); + str = p + 1; + } + fputs(str, out); + putc('"', out); +} + +static void print_symbol(FILE *out, struct menu *menu) +{ + struct symbol *sym = menu->sym; + struct property *prop; + + if (sym_is_choice(sym)) + fprintf(out, "\nchoice\n"); + else + fprintf(out, "\nconfig %s\n", sym->name); + switch (sym->type) { + case S_BOOLEAN: + fputs(" boolean\n", out); + break; + case S_TRISTATE: + fputs(" tristate\n", out); + break; + case S_STRING: + fputs(" string\n", out); + break; + case S_INT: + fputs(" integer\n", out); + break; + case S_HEX: + fputs(" hex\n", out); + break; + default: + fputs(" ???\n", out); + break; + } + for (prop = sym->prop; prop; prop = prop->next) { + if (prop->menu != menu) + continue; + switch (prop->type) { + case P_PROMPT: + fputs(" prompt ", out); + print_quoted_string(out, prop->text); + if (!expr_is_yes(prop->visible.expr)) { + fputs(" if ", out); + expr_fprint(prop->visible.expr, out); + } + fputc('\n', out); + break; + case P_DEFAULT: + fputs( " default ", out); + expr_fprint(prop->expr, out); + if (!expr_is_yes(prop->visible.expr)) { + fputs(" if ", out); + expr_fprint(prop->visible.expr, out); + } + fputc('\n', out); + break; + case P_CHOICE: + fputs(" #choice value\n", out); + break; + case P_SELECT: + fputs( " select ", out); + expr_fprint(prop->expr, out); + fputc('\n', out); + break; + case P_RANGE: + fputs( " range ", out); + expr_fprint(prop->expr, out); + fputc('\n', out); + break; + case P_MENU: + fputs( " menu ", out); + print_quoted_string(out, prop->text); + fputc('\n', out); + break; + default: + fprintf(out, " unknown prop %d!\n", prop->type); + break; + } + } + if (menu->help) { + int len = strlen(menu->help); + while (menu->help[--len] == '\n') + menu->help[len] = 0; + fprintf(out, " help\n%s\n", menu->help); + } +} + +void zconfdump(FILE *out) +{ + struct property *prop; + struct symbol *sym; + struct menu *menu; + + menu = rootmenu.list; + while (menu) { + if ((sym = menu->sym)) + print_symbol(out, menu); + else if ((prop = menu->prompt)) { + switch (prop->type) { + case P_COMMENT: + fputs("\ncomment ", out); + print_quoted_string(out, prop->text); + fputs("\n", out); + break; + case P_MENU: + fputs("\nmenu ", out); + print_quoted_string(out, prop->text); + fputs("\n", out); + break; + default: + ; + } + if (!expr_is_yes(prop->visible.expr)) { + fputs(" depends ", out); + expr_fprint(prop->visible.expr, out); + fputc('\n', out); + } + } + + if (menu->list) + menu = menu->list; + else if (menu->next) + menu = menu->next; + else while ((menu = menu->parent)) { + if (menu->prompt && menu->prompt->type == P_MENU) + fputs("\nendmenu\n", out); + if (menu->next) { + menu = menu->next; + break; + } + } + } +} + +#include "zconf.lex.c" +#include "util.c" +#include "confdata.c" +#include "expr.c" +#include "symbol.c" +#include "menu.c" + diff --git a/scripts/kconfig/zconf.y b/scripts/kconfig/zconf.y new file mode 100644 index 0000000000..b82ae12842 --- /dev/null +++ b/scripts/kconfig/zconf.y @@ -0,0 +1,742 @@ +%{ +/* + * Copyright (C) 2002 Roman Zippel + * Released under the terms of the GNU GPL v2.0. + */ + +#include +#include +#include +#include +#include +#include + +#include "lkc.h" + +#define printd(mask, fmt...) if (cdebug & (mask)) printf(fmt) + +#define PRINTD 0x0001 +#define DEBUG_PARSE 0x0002 + +int cdebug = PRINTD; + +extern int zconflex(void); +static void zconfprint(const char *err, ...); +static void zconf_error(const char *err, ...); +static void zconferror(const char *err); +static bool zconf_endtoken(const struct kconf_id *id, int starttoken, int endtoken); + +struct symbol *symbol_hash[SYMBOL_HASHSIZE]; + +static struct menu *current_menu, *current_entry; + +%} +%expect 30 + +%union +{ + char *string; + struct file *file; + struct symbol *symbol; + struct expr *expr; + struct menu *menu; + const struct kconf_id *id; +} + +%token T_MAINMENU +%token T_MENU +%token T_ENDMENU +%token T_SOURCE +%token T_CHOICE +%token T_ENDCHOICE +%token T_COMMENT +%token T_CONFIG +%token T_MENUCONFIG +%token T_HELP +%token T_HELPTEXT +%token T_IF +%token T_ENDIF +%token T_DEPENDS +%token T_OPTIONAL +%token T_PROMPT +%token T_TYPE +%token T_DEFAULT +%token T_SELECT +%token T_RANGE +%token T_VISIBLE +%token T_OPTION +%token T_ON +%token T_WORD +%token T_WORD_QUOTE +%token T_UNEQUAL +%token T_LESS +%token T_LESS_EQUAL +%token T_GREATER +%token T_GREATER_EQUAL +%token T_CLOSE_PAREN +%token T_OPEN_PAREN +%token T_EOL + +%left T_OR +%left T_AND +%left T_EQUAL T_UNEQUAL +%left T_LESS T_LESS_EQUAL T_GREATER T_GREATER_EQUAL +%nonassoc T_NOT + +%type prompt +%type symbol +%type expr +%type if_expr +%type end +%type option_name +%type if_entry menu_entry choice_entry +%type symbol_option_arg word_opt + +%destructor { + fprintf(stderr, "%s:%d: missing end statement for this entry\n", + $$->file->name, $$->lineno); + if (current_menu == $$) + menu_end_menu(); +} if_entry menu_entry choice_entry + +%{ +/* Include zconf.hash.c here so it can see the token constants. */ +#include "zconf.hash.c" +%} + +%% +input: nl start | start; + +start: mainmenu_stmt stmt_list | stmt_list; + +stmt_list: + /* empty */ + | stmt_list common_stmt + | stmt_list choice_stmt + | stmt_list menu_stmt + | stmt_list end { zconf_error("unexpected end statement"); } + | stmt_list T_WORD error T_EOL { zconf_error("unknown statement \"%s\"", $2); } + | stmt_list option_name error T_EOL +{ + zconf_error("unexpected option \"%s\"", kconf_id_strings + $2->name); +} + | stmt_list error T_EOL { zconf_error("invalid statement"); } +; + +option_name: + T_DEPENDS | T_PROMPT | T_TYPE | T_SELECT | T_OPTIONAL | T_RANGE | T_DEFAULT | T_VISIBLE +; + +common_stmt: + T_EOL + | if_stmt + | comment_stmt + | config_stmt + | menuconfig_stmt + | source_stmt +; + +option_error: + T_WORD error T_EOL { zconf_error("unknown option \"%s\"", $1); } + | error T_EOL { zconf_error("invalid option"); } +; + + +/* config/menuconfig entry */ + +config_entry_start: T_CONFIG T_WORD T_EOL +{ + struct symbol *sym = sym_lookup($2, 0); + sym->flags |= SYMBOL_OPTIONAL; + menu_add_entry(sym); + printd(DEBUG_PARSE, "%s:%d:config %s\n", zconf_curname(), zconf_lineno(), $2); +}; + +config_stmt: config_entry_start config_option_list +{ + menu_end_entry(); + printd(DEBUG_PARSE, "%s:%d:endconfig\n", zconf_curname(), zconf_lineno()); +}; + +menuconfig_entry_start: T_MENUCONFIG T_WORD T_EOL +{ + struct symbol *sym = sym_lookup($2, 0); + sym->flags |= SYMBOL_OPTIONAL; + menu_add_entry(sym); + printd(DEBUG_PARSE, "%s:%d:menuconfig %s\n", zconf_curname(), zconf_lineno(), $2); +}; + +menuconfig_stmt: menuconfig_entry_start config_option_list +{ + if (current_entry->prompt) + current_entry->prompt->type = P_MENU; + else + zconfprint("warning: menuconfig statement without prompt"); + menu_end_entry(); + printd(DEBUG_PARSE, "%s:%d:endconfig\n", zconf_curname(), zconf_lineno()); +}; + +config_option_list: + /* empty */ + | config_option_list config_option + | config_option_list symbol_option + | config_option_list depends + | config_option_list help + | config_option_list option_error + | config_option_list T_EOL +; + +config_option: T_TYPE prompt_stmt_opt T_EOL +{ + menu_set_type($1->stype); + printd(DEBUG_PARSE, "%s:%d:type(%u)\n", + zconf_curname(), zconf_lineno(), + $1->stype); +}; + +config_option: T_PROMPT prompt if_expr T_EOL +{ + menu_add_prompt(P_PROMPT, $2, $3); + printd(DEBUG_PARSE, "%s:%d:prompt\n", zconf_curname(), zconf_lineno()); +}; + +config_option: T_DEFAULT expr if_expr T_EOL +{ + menu_add_expr(P_DEFAULT, $2, $3); + if ($1->stype != S_UNKNOWN) + menu_set_type($1->stype); + printd(DEBUG_PARSE, "%s:%d:default(%u)\n", + zconf_curname(), zconf_lineno(), + $1->stype); +}; + +config_option: T_SELECT T_WORD if_expr T_EOL +{ + menu_add_symbol(P_SELECT, sym_lookup($2, 0), $3); + printd(DEBUG_PARSE, "%s:%d:select\n", zconf_curname(), zconf_lineno()); +}; + +config_option: T_RANGE symbol symbol if_expr T_EOL +{ + menu_add_expr(P_RANGE, expr_alloc_comp(E_RANGE,$2, $3), $4); + printd(DEBUG_PARSE, "%s:%d:range\n", zconf_curname(), zconf_lineno()); +}; + +symbol_option: T_OPTION symbol_option_list T_EOL +; + +symbol_option_list: + /* empty */ + | symbol_option_list T_WORD symbol_option_arg +{ + const struct kconf_id *id = kconf_id_lookup($2, strlen($2)); + if (id && id->flags & TF_OPTION) + menu_add_option(id->token, $3); + else + zconfprint("warning: ignoring unknown option %s", $2); + free($2); +}; + +symbol_option_arg: + /* empty */ { $$ = NULL; } + | T_EQUAL prompt { $$ = $2; } +; + +/* choice entry */ + +choice: T_CHOICE word_opt T_EOL +{ + struct symbol *sym = sym_lookup($2, SYMBOL_CHOICE); + sym->flags |= SYMBOL_AUTO; + menu_add_entry(sym); + menu_add_expr(P_CHOICE, NULL, NULL); + printd(DEBUG_PARSE, "%s:%d:choice\n", zconf_curname(), zconf_lineno()); +}; + +choice_entry: choice choice_option_list +{ + $$ = menu_add_menu(); +}; + +choice_end: end +{ + if (zconf_endtoken($1, T_CHOICE, T_ENDCHOICE)) { + menu_end_menu(); + printd(DEBUG_PARSE, "%s:%d:endchoice\n", zconf_curname(), zconf_lineno()); + } +}; + +choice_stmt: choice_entry choice_block choice_end +; + +choice_option_list: + /* empty */ + | choice_option_list choice_option + | choice_option_list depends + | choice_option_list help + | choice_option_list T_EOL + | choice_option_list option_error +; + +choice_option: T_PROMPT prompt if_expr T_EOL +{ + menu_add_prompt(P_PROMPT, $2, $3); + printd(DEBUG_PARSE, "%s:%d:prompt\n", zconf_curname(), zconf_lineno()); +}; + +choice_option: T_TYPE prompt_stmt_opt T_EOL +{ + if ($1->stype == S_BOOLEAN || $1->stype == S_TRISTATE) { + menu_set_type($1->stype); + printd(DEBUG_PARSE, "%s:%d:type(%u)\n", + zconf_curname(), zconf_lineno(), + $1->stype); + } else + YYERROR; +}; + +choice_option: T_OPTIONAL T_EOL +{ + current_entry->sym->flags |= SYMBOL_OPTIONAL; + printd(DEBUG_PARSE, "%s:%d:optional\n", zconf_curname(), zconf_lineno()); +}; + +choice_option: T_DEFAULT T_WORD if_expr T_EOL +{ + if ($1->stype == S_UNKNOWN) { + menu_add_symbol(P_DEFAULT, sym_lookup($2, 0), $3); + printd(DEBUG_PARSE, "%s:%d:default\n", + zconf_curname(), zconf_lineno()); + } else + YYERROR; +}; + +choice_block: + /* empty */ + | choice_block common_stmt +; + +/* if entry */ + +if_entry: T_IF expr nl +{ + printd(DEBUG_PARSE, "%s:%d:if\n", zconf_curname(), zconf_lineno()); + menu_add_entry(NULL); + menu_add_dep($2); + $$ = menu_add_menu(); +}; + +if_end: end +{ + if (zconf_endtoken($1, T_IF, T_ENDIF)) { + menu_end_menu(); + printd(DEBUG_PARSE, "%s:%d:endif\n", zconf_curname(), zconf_lineno()); + } +}; + +if_stmt: if_entry if_block if_end +; + +if_block: + /* empty */ + | if_block common_stmt + | if_block menu_stmt + | if_block choice_stmt +; + +/* mainmenu entry */ + +mainmenu_stmt: T_MAINMENU prompt nl +{ + menu_add_prompt(P_MENU, $2, NULL); +}; + +/* menu entry */ + +menu: T_MENU prompt T_EOL +{ + menu_add_entry(NULL); + menu_add_prompt(P_MENU, $2, NULL); + printd(DEBUG_PARSE, "%s:%d:menu\n", zconf_curname(), zconf_lineno()); +}; + +menu_entry: menu visibility_list depends_list +{ + $$ = menu_add_menu(); +}; + +menu_end: end +{ + if (zconf_endtoken($1, T_MENU, T_ENDMENU)) { + menu_end_menu(); + printd(DEBUG_PARSE, "%s:%d:endmenu\n", zconf_curname(), zconf_lineno()); + } +}; + +menu_stmt: menu_entry menu_block menu_end +; + +menu_block: + /* empty */ + | menu_block common_stmt + | menu_block menu_stmt + | menu_block choice_stmt +; + +source_stmt: T_SOURCE prompt T_EOL +{ + printd(DEBUG_PARSE, "%s:%d:source %s\n", zconf_curname(), zconf_lineno(), $2); + zconf_nextfile($2); +}; + +/* comment entry */ + +comment: T_COMMENT prompt T_EOL +{ + menu_add_entry(NULL); + menu_add_prompt(P_COMMENT, $2, NULL); + printd(DEBUG_PARSE, "%s:%d:comment\n", zconf_curname(), zconf_lineno()); +}; + +comment_stmt: comment depends_list +{ + menu_end_entry(); +}; + +/* help option */ + +help_start: T_HELP T_EOL +{ + printd(DEBUG_PARSE, "%s:%d:help\n", zconf_curname(), zconf_lineno()); + zconf_starthelp(); +}; + +help: help_start T_HELPTEXT +{ + current_entry->help = $2; +}; + +/* depends option */ + +depends_list: + /* empty */ + | depends_list depends + | depends_list T_EOL + | depends_list option_error +; + +depends: T_DEPENDS T_ON expr T_EOL +{ + menu_add_dep($3); + printd(DEBUG_PARSE, "%s:%d:depends on\n", zconf_curname(), zconf_lineno()); +}; + +/* visibility option */ + +visibility_list: + /* empty */ + | visibility_list visible + | visibility_list T_EOL +; + +visible: T_VISIBLE if_expr +{ + menu_add_visibility($2); +}; + +/* prompt statement */ + +prompt_stmt_opt: + /* empty */ + | prompt if_expr +{ + menu_add_prompt(P_PROMPT, $1, $2); +}; + +prompt: T_WORD + | T_WORD_QUOTE +; + +end: T_ENDMENU T_EOL { $$ = $1; } + | T_ENDCHOICE T_EOL { $$ = $1; } + | T_ENDIF T_EOL { $$ = $1; } +; + +nl: + T_EOL + | nl T_EOL +; + +if_expr: /* empty */ { $$ = NULL; } + | T_IF expr { $$ = $2; } +; + +expr: symbol { $$ = expr_alloc_symbol($1); } + | symbol T_LESS symbol { $$ = expr_alloc_comp(E_LTH, $1, $3); } + | symbol T_LESS_EQUAL symbol { $$ = expr_alloc_comp(E_LEQ, $1, $3); } + | symbol T_GREATER symbol { $$ = expr_alloc_comp(E_GTH, $1, $3); } + | symbol T_GREATER_EQUAL symbol { $$ = expr_alloc_comp(E_GEQ, $1, $3); } + | symbol T_EQUAL symbol { $$ = expr_alloc_comp(E_EQUAL, $1, $3); } + | symbol T_UNEQUAL symbol { $$ = expr_alloc_comp(E_UNEQUAL, $1, $3); } + | T_OPEN_PAREN expr T_CLOSE_PAREN { $$ = $2; } + | T_NOT expr { $$ = expr_alloc_one(E_NOT, $2); } + | expr T_OR expr { $$ = expr_alloc_two(E_OR, $1, $3); } + | expr T_AND expr { $$ = expr_alloc_two(E_AND, $1, $3); } +; + +symbol: T_WORD { $$ = sym_lookup($1, 0); free($1); } + | T_WORD_QUOTE { $$ = sym_lookup($1, SYMBOL_CONST); free($1); } +; + +word_opt: /* empty */ { $$ = NULL; } + | T_WORD + +%% + +void conf_parse(const char *name) +{ + struct symbol *sym; + int i; + + zconf_initscan(name); + + sym_init(); + _menu_init(); + rootmenu.prompt = menu_add_prompt(P_MENU, "Crazyflie Platform Configuration", NULL); + + if (getenv("ZCONF_DEBUG")) + zconfdebug = 1; + zconfparse(); + if (zconfnerrs) + exit(1); + if (!modules_sym) + modules_sym = sym_find( "n" ); + + rootmenu.prompt->text = _(rootmenu.prompt->text); + rootmenu.prompt->text = sym_expand_string_value(rootmenu.prompt->text); + + menu_finalize(&rootmenu); + for_all_symbols(i, sym) { + if (sym_check_deps(sym)) + zconfnerrs++; + } + if (zconfnerrs) + exit(1); + sym_set_change_count(1); +} + +static const char *zconf_tokenname(int token) +{ + switch (token) { + case T_MENU: return "menu"; + case T_ENDMENU: return "endmenu"; + case T_CHOICE: return "choice"; + case T_ENDCHOICE: return "endchoice"; + case T_IF: return "if"; + case T_ENDIF: return "endif"; + case T_DEPENDS: return "depends"; + case T_VISIBLE: return "visible"; + } + return ""; +} + +static bool zconf_endtoken(const struct kconf_id *id, int starttoken, int endtoken) +{ + if (id->token != endtoken) { + zconf_error("unexpected '%s' within %s block", + kconf_id_strings + id->name, zconf_tokenname(starttoken)); + zconfnerrs++; + return false; + } + if (current_menu->file != current_file) { + zconf_error("'%s' in different file than '%s'", + kconf_id_strings + id->name, zconf_tokenname(starttoken)); + fprintf(stderr, "%s:%d: location of the '%s'\n", + current_menu->file->name, current_menu->lineno, + zconf_tokenname(starttoken)); + zconfnerrs++; + return false; + } + return true; +} + +static void zconfprint(const char *err, ...) +{ + va_list ap; + + fprintf(stderr, "%s:%d: ", zconf_curname(), zconf_lineno()); + va_start(ap, err); + vfprintf(stderr, err, ap); + va_end(ap); + fprintf(stderr, "\n"); +} + +static void zconf_error(const char *err, ...) +{ + va_list ap; + + zconfnerrs++; + fprintf(stderr, "%s:%d: ", zconf_curname(), zconf_lineno()); + va_start(ap, err); + vfprintf(stderr, err, ap); + va_end(ap); + fprintf(stderr, "\n"); +} + +static void zconferror(const char *err) +{ + fprintf(stderr, "%s:%d: %s\n", zconf_curname(), zconf_lineno() + 1, err); +} + +static void print_quoted_string(FILE *out, const char *str) +{ + const char *p; + int len; + + putc('"', out); + while ((p = strchr(str, '"'))) { + len = p - str; + if (len) + fprintf(out, "%.*s", len, str); + fputs("\\\"", out); + str = p + 1; + } + fputs(str, out); + putc('"', out); +} + +static void print_symbol(FILE *out, struct menu *menu) +{ + struct symbol *sym = menu->sym; + struct property *prop; + + if (sym_is_choice(sym)) + fprintf(out, "\nchoice\n"); + else + fprintf(out, "\nconfig %s\n", sym->name); + switch (sym->type) { + case S_BOOLEAN: + fputs(" boolean\n", out); + break; + case S_TRISTATE: + fputs(" tristate\n", out); + break; + case S_STRING: + fputs(" string\n", out); + break; + case S_INT: + fputs(" integer\n", out); + break; + case S_HEX: + fputs(" hex\n", out); + break; + default: + fputs(" ???\n", out); + break; + } + for (prop = sym->prop; prop; prop = prop->next) { + if (prop->menu != menu) + continue; + switch (prop->type) { + case P_PROMPT: + fputs(" prompt ", out); + print_quoted_string(out, prop->text); + if (!expr_is_yes(prop->visible.expr)) { + fputs(" if ", out); + expr_fprint(prop->visible.expr, out); + } + fputc('\n', out); + break; + case P_DEFAULT: + fputs( " default ", out); + expr_fprint(prop->expr, out); + if (!expr_is_yes(prop->visible.expr)) { + fputs(" if ", out); + expr_fprint(prop->visible.expr, out); + } + fputc('\n', out); + break; + case P_CHOICE: + fputs(" #choice value\n", out); + break; + case P_SELECT: + fputs( " select ", out); + expr_fprint(prop->expr, out); + fputc('\n', out); + break; + case P_RANGE: + fputs( " range ", out); + expr_fprint(prop->expr, out); + fputc('\n', out); + break; + case P_MENU: + fputs( " menu ", out); + print_quoted_string(out, prop->text); + fputc('\n', out); + break; + default: + fprintf(out, " unknown prop %d!\n", prop->type); + break; + } + } + if (menu->help) { + int len = strlen(menu->help); + while (menu->help[--len] == '\n') + menu->help[len] = 0; + fprintf(out, " help\n%s\n", menu->help); + } +} + +void zconfdump(FILE *out) +{ + struct property *prop; + struct symbol *sym; + struct menu *menu; + + menu = rootmenu.list; + while (menu) { + if ((sym = menu->sym)) + print_symbol(out, menu); + else if ((prop = menu->prompt)) { + switch (prop->type) { + case P_COMMENT: + fputs("\ncomment ", out); + print_quoted_string(out, prop->text); + fputs("\n", out); + break; + case P_MENU: + fputs("\nmenu ", out); + print_quoted_string(out, prop->text); + fputs("\n", out); + break; + default: + ; + } + if (!expr_is_yes(prop->visible.expr)) { + fputs(" depends ", out); + expr_fprint(prop->visible.expr, out); + fputc('\n', out); + } + } + + if (menu->list) + menu = menu->list; + else if (menu->next) + menu = menu->next; + else while ((menu = menu->parent)) { + if (menu->prompt && menu->prompt->type == P_MENU) + fputs("\nendmenu\n", out); + if (menu->next) { + menu = menu->next; + break; + } + } + } +} + +#include "zconf.lex.c" +#include "util.c" +#include "confdata.c" +#include "expr.c" +#include "symbol.c" +#include "menu.c" diff --git a/scripts/mkmakefile b/scripts/mkmakefile new file mode 100755 index 0000000000..84af27bf0f --- /dev/null +++ b/scripts/mkmakefile @@ -0,0 +1,52 @@ +#!/bin/sh +# Generates a small Makefile used in the root of the output +# directory, to allow make to be started from there. +# The Makefile also allow for more convinient build of external modules + +# Usage +# $1 - Kernel src directory +# $2 - Output directory +# $3 - version +# $4 - patchlevel + + +test ! -r $2/Makefile -o -O $2/Makefile || exit 0 +# Only overwrite automatically generated Makefiles +# (so we do not overwrite kernel Makefile) +if test -e $2/Makefile && ! grep -q Automatically $2/Makefile +then + exit 0 +fi +if [ "${quiet}" != "silent_" ]; then + echo " GEN $2/Makefile" +fi + +cat << EOF > $2/Makefile +# Automatically generated by $0: don't edit + +VERSION = $3 +PATCHLEVEL = $4 + +lastword = \$(word \$(words \$(1)),\$(1)) +makedir := \$(dir \$(call lastword,\$(MAKEFILE_LIST))) + +ifeq ("\$(origin V)", "command line") +VERBOSE := \$(V) +endif +ifneq (\$(VERBOSE),1) +Q := @ +endif + +MAKEARGS := -C $1 +MAKEARGS += O=\$(if \$(patsubst /%,,\$(makedir)),\$(CURDIR)/)\$(patsubst %/,%,\$(makedir)) + +MAKEFLAGS += --no-print-directory + +.PHONY: __sub-make \$(MAKECMDGOALS) + +__sub-make: + \$(Q)\$(MAKE) \$(MAKEARGS) \$(MAKECMDGOALS) + +\$(filter-out __sub-make, \$(MAKECMDGOALS)): __sub-make + @: +EOF diff --git a/src/Kbuild b/src/Kbuild new file mode 100644 index 0000000000..cfdd389277 --- /dev/null +++ b/src/Kbuild @@ -0,0 +1,8 @@ +obj-y += deck/ +obj-y += drivers/ +obj-y += hal/ +obj-y += init/ +obj-y += lib/ +obj-y += modules/ +obj-y += platform/ +obj-y += utils/ diff --git a/src/config/FreeRTOSConfig.h b/src/config/FreeRTOSConfig.h index 8c4735d791..79b442fca5 100644 --- a/src/config/FreeRTOSConfig.h +++ b/src/config/FreeRTOSConfig.h @@ -157,13 +157,13 @@ to exclude the API function. */ #define configSUPPORT_STATIC_ALLOCATION 1 // Queue monitoring -#ifdef DEBUG_QUEUE_MONITOR +#ifdef CONFIG_DEBUG_QUEUE_MONITOR #undef traceQUEUE_SEND #undef traceQUEUE_SEND_FAILED #define traceQUEUE_SEND(xQueue) qm_traceQUEUE_SEND(xQueue) void qm_traceQUEUE_SEND(void* xQueue); #define traceQUEUE_SEND_FAILED(xQueue) qm_traceQUEUE_SEND_FAILED(xQueue) void qm_traceQUEUE_SEND_FAILED(void* xQueue); -#endif // DEBUG_QUEUE_MONITOR +#endif // CONFIG_DEBUG_QUEUE_MONITOR #endif /* FREERTOS_CONFIG_H */ diff --git a/src/config/config.h b/src/config/config.h index 92d7d92e9b..ebdf2a429b 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -46,7 +46,7 @@ #include "trace.h" #include "usec_time.h" -#define PROTOCOL_VERSION 4 +#define PROTOCOL_VERSION 5 #ifdef STM32F4XX #define QUAD_FORMATION_X @@ -210,12 +210,6 @@ */ #define BAT_LOADING_SAG_THRESHOLD 0.95f -/** - * \def ACTIVATE_AUTO_SHUTDOWN - * Will automatically shot of system if no radio activity - */ -//#define ACTIVATE_AUTO_SHUTDOWN - /** * \def ACTIVATE_STARTUP_SOUND * Playes a startup melody using the motors and PWM modulation diff --git a/src/deck/Kbuild b/src/deck/Kbuild new file mode 100644 index 0000000000..0f505f38a4 --- /dev/null +++ b/src/deck/Kbuild @@ -0,0 +1,3 @@ +obj-y += api/ +obj-y += core/ +obj-y += drivers/src/ diff --git a/src/deck/api/Kbuild b/src/deck/api/Kbuild new file mode 100644 index 0000000000..c534d5b4d4 --- /dev/null +++ b/src/deck/api/Kbuild @@ -0,0 +1,5 @@ +obj-y += deck_analog.o +obj-y += deck_constants.o +obj-y += deck_digital.o +obj-y += deck_spi3.o +obj-y += deck_spi.o diff --git a/src/deck/api/deck_spi3.c b/src/deck/api/deck_spi3.c index 75bb87026e..8344003b26 100644 --- a/src/deck/api/deck_spi3.c +++ b/src/deck/api/deck_spi3.c @@ -34,6 +34,7 @@ #include "FreeRTOS.h" #include "semphr.h" +#include "autoconf.h" #include "cfassert.h" #include "config.h" #include "nvicconf.h" @@ -264,7 +265,7 @@ void spi3EndTransaction() xSemaphoreGive(spiMutex); } -#ifdef USDDECK_USE_ALT_PINS_AND_SPI +#ifdef CONFIG_DECK_USD_USE_ALT_PINS_AND_SPI void __attribute__((used)) SPI_TX_DMA_IRQHandler(void) { portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; diff --git a/src/deck/core/Kbuild b/src/deck/core/Kbuild new file mode 100644 index 0000000000..da542a936e --- /dev/null +++ b/src/deck/core/Kbuild @@ -0,0 +1,5 @@ +obj-y += deck_drivers.o +obj-y += deck_info.o +obj-y += deck_memory.o +obj-y += deck.o +obj-y += deck_test.o diff --git a/src/deck/core/deck.c b/src/deck/core/deck.c index 7201c105b9..02fc62de3c 100644 --- a/src/deck/core/deck.c +++ b/src/deck/core/deck.c @@ -32,7 +32,7 @@ #include "deck_memory.h" #include "debug.h" -#ifdef DEBUG +#ifdef CONFIG_DEBUG #define DECK_CORE_DBG_PRINT(fmt, ...) DEBUG_PRINT(fmt, ## __VA_ARGS__) #else #define DECK_CORE_DBG_PRINT(...) diff --git a/src/deck/core/deck_drivers.c b/src/deck/core/deck_drivers.c index 8cd762ee63..724d61952e 100644 --- a/src/deck/core/deck_drivers.c +++ b/src/deck/core/deck_drivers.c @@ -32,7 +32,7 @@ #include "deck.h" #include "debug.h" -#ifdef DEBUG +#ifdef CONFIG_DEBUG #define DECK_DRV_DBG_PRINT(fmt, ...) DEBUG_PRINT(fmt, ## __VA_ARGS__) #else #define DECK_DRV_DBG_PRINT(...) diff --git a/src/deck/core/deck_info.c b/src/deck/core/deck_info.c index 2669f6c41b..3961b3d1a7 100644 --- a/src/deck/core/deck_info.c +++ b/src/deck/core/deck_info.c @@ -37,7 +37,9 @@ #include "debug.h" #include "static_mem.h" -#ifdef DEBUG +#include "autoconf.h" + +#ifdef CONFIG_DEBUG #define DECK_INFO_DBG_PRINT(fmt, ...) DEBUG_PRINT(fmt, ## __VA_ARGS__) #else #define DECK_INFO_DBG_PRINT(...) @@ -54,14 +56,7 @@ static StateEstimatorType requiredEstimator = anyEstimator; static bool registerRequiredEstimator(StateEstimatorType estimator); static bool requiredLowInterferenceRadioMode = false; -#ifndef DECK_FORCE -#define DECK_FORCE -#endif - -#define xstr(s) str(s) -#define str(s) #s - -static char* deck_force = xstr(DECK_FORCE); +static char* deck_force = CONFIG_DECK_FORCE; void deckInfoInit() { @@ -93,7 +88,7 @@ DeckInfo * deckInfo(int i) // Dummy driver for decks that do not have a driver implemented static const DeckDriver dummyDriver; -#ifndef IGNORE_OW_DECKS +#ifndef CONFIG_DEBUG_DECK_IGNORE_OWS static const DeckDriver * findDriver(DeckInfo *deck) { char name[30]; @@ -138,7 +133,7 @@ void printDeckInfo(DeckInfo *info) } } -#ifndef IGNORE_OW_DECKS +#ifndef CONFIG_DEBUG_DECK_IGNORE_OWS static bool infoDecode(DeckInfo * info) { uint8_t crcHeader; @@ -190,7 +185,7 @@ static void enumerateDecks(void) nDecks = 0; } -#ifndef IGNORE_OW_DECKS +#ifndef CONFIG_DEBUG_DECK_IGNORE_OWS for (int i = 0; i < nDecks; i++) { DECK_INFO_DBG_PRINT("Enumerating deck %i\n", i); @@ -201,7 +196,7 @@ static void enumerateDecks(void) deckInfos[i].driver = findDriver(&deckInfos[i]); printDeckInfo(&deckInfos[i]); } else { -#ifdef DEBUG +#ifdef CONFIG_DEBUG DEBUG_PRINT("Deck %i has corrupt OW memory. " "Ignoring the deck in DEBUG mode.\n", i); deckInfos[i].driver = &dummyDriver; @@ -225,8 +220,8 @@ static void enumerateDecks(void) #endif // Add build-forced driver - if (strlen(deck_force) > 0) { - DEBUG_PRINT("DECK_FORCE=%s found\n", deck_force); + if (strlen(deck_force) > 0 && strncmp(deck_force, "none", 4) != 0) { + DEBUG_PRINT("CONFIG_DECK_FORCE=%s found\n", deck_force); //split deck_force into multiple, separated by colons, if available char delim[] = ":"; diff --git a/src/deck/core/deck_memory.c b/src/deck/core/deck_memory.c index 2ccb68eed0..f6d5e56ec7 100644 --- a/src/deck/core/deck_memory.c +++ b/src/deck/core/deck_memory.c @@ -47,7 +47,12 @@ static const MemoryHandlerDef_t memoryDef = { .write = handleMemWrite, }; -static const uint8_t VERSION = 1; +typedef enum { + MEM_PRIMARY = 0, + MEM_SECONDARY = 1 +} MemSelector; + +static const uint8_t VERSION = 2; static const int DECK_MEMORY_INFO_SIZE = 0x20; @@ -189,14 +194,19 @@ static bool handleInfoSectionRead(const uint32_t memAddr, const uint8_t readLen, return true; } -static bool handleDeckSectionRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer, const int deckNr) { +static bool handleDeckSectionRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer, const int deckNr, const MemSelector selector) { bool result = false; const DeckInfo* info = deckInfo(deckNr); + const DeckMemDef_t* deckMemDef = info->driver->memoryDef; + if (selector == MEM_SECONDARY) { + deckMemDef = info->driver->memoryDefSecondary; + } + if (deckMemDef) { if (deckMemDef->read) { - uint32_t baseAddress = (deckNr + 1) * DECK_MEM_MAX_SIZE; + uint32_t baseAddress = (deckNr + 1) * DECK_MEM_MAX_SIZE + selector * DECK_MEM_MAX_SIZE; uint32_t deckAddress = memAddr - baseAddress; result = deckMemDef->read(deckAddress, readLen, buffer); } @@ -205,14 +215,19 @@ static bool handleDeckSectionRead(const uint32_t memAddr, const uint8_t readLen, return result; } -static bool handleDeckSectionWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer, const int deckNr) { +static bool handleDeckSectionWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer, const int deckNr, const MemSelector selector) { bool result = false; const DeckInfo* info = deckInfo(deckNr); + const DeckMemDef_t* deckMemDef = info->driver->memoryDef; + if (selector == MEM_SECONDARY) { + deckMemDef = info->driver->memoryDefSecondary; + } + if (deckMemDef) { if (deckMemDef->write) { - uint32_t baseAddress = (deckNr + 1) * DECK_MEM_MAX_SIZE; + uint32_t baseAddress = (deckNr + 1) * DECK_MEM_MAX_SIZE + selector * DECK_MEM_MAX_SIZE; uint32_t deckAddress = memAddr - baseAddress; result = deckMemDef->write(deckAddress, writeLen, buffer); } @@ -231,9 +246,10 @@ TESTABLE_STATIC bool handleMemRead(const uint32_t memAddr, const uint8_t readLen if (section == 0) { result = handleInfoSectionRead(memAddr, readLen, buffer, nrOfDecks); } else { - int deckNr = section - 1; - if (deckNr < nrOfDecks) { - result = handleDeckSectionRead(memAddr, readLen, buffer, deckNr); + MemSelector selector = (section - 1) % 2; + int deckNr = (section - 1) / 2; + if (deckNr < nrOfDecks * 2) { + result = handleDeckSectionRead(memAddr, readLen, buffer, deckNr, selector); } } @@ -248,9 +264,10 @@ TESTABLE_STATIC bool handleMemWrite(const uint32_t memAddr, const uint8_t writeL const uint32_t section = memAddr / DECK_MEM_MAX_SIZE; if (section > 0) { int nrOfDecks = deckCount(); - int deckNr = section - 1; - if (deckNr < nrOfDecks) { - result = handleDeckSectionWrite(memAddr, writeLen, buffer, deckNr); + MemSelector selector = (section - 1) % 2; + int deckNr = (section - 1) / 2; + if (deckNr < nrOfDecks * 2) { + result = handleDeckSectionWrite(memAddr, writeLen, buffer, deckNr, selector); } } diff --git a/src/deck/core/deck_test.c b/src/deck/core/deck_test.c index 98eeeaf640..2a54073b1d 100644 --- a/src/deck/core/deck_test.c +++ b/src/deck/core/deck_test.c @@ -31,13 +31,7 @@ #include "deck_test.h" #include "debug.h" - -#ifndef DECK_TEST_PRINT_ALL_FAILED - #define STATUS_EVAL (*status) -#else - #define STATUS_EVAL 1 -#endif - +#define STATUS_EVAL (*status) void decktestEval(bool result, char *failString, bool *status) { diff --git a/src/deck/drivers/interface/aideck-router.h b/src/deck/drivers/interface/aideck-router.h new file mode 100644 index 0000000000..a72ff71ce0 --- /dev/null +++ b/src/deck/drivers/interface/aideck-router.h @@ -0,0 +1,37 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2022 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#pragma once +#include +#include + +/** + * @brief Initialize the AI-deck CPX router. + * + * Connect to Wifi (if configured) and start receiving CPX packages from + * the AI-deck. + * + */ +void aideckRouterInit(void); \ No newline at end of file diff --git a/src/deck/drivers/interface/aideck.h b/src/deck/drivers/interface/aideck.h index c0d8c5cf5b..b2c3784d69 100644 --- a/src/deck/drivers/interface/aideck.h +++ b/src/deck/drivers/interface/aideck.h @@ -36,69 +36,78 @@ #define ESP_BITSTREAM_SIZE 610576 #define AIDECK_UART_TRANSPORT_MTU (100) -#define CPX_HEADER_SIZE (2) -// No values in this enum can be larger than 0xF (15) +// This enum is used to identify source and destination for CPX routing information typedef enum { - STM32 = 1, - ESP32 = 2, - HOST = 3, - GAP8 = 4 + CPX_T_STM32 = 1, // The STM in the Crazyflie + CPX_T_ESP32 = 2, // The ESP on the AI-deck + CPX_T_HOST = 3, // A remote computer connected via Wifi + CPX_T_GAP8 = 4 // The GAP8 on the AI-deck } CPXTarget_t; -// No values in this enum can be larger than 0xFF (255) typedef enum { - SYSTEM = 1, - CONSOLE = 2, - CRTP = 3, - WIFI_CTRL = 4, - APP = 5, - TEST = 0x0E, - BOOTLOADER = 0x0F, + CPX_F_SYSTEM = 1, + CPX_F_CONSOLE = 2, + CPX_F_CRTP = 3, + CPX_F_WIFI_CTRL = 4, + CPX_F_APP = 5, + CPX_F_TEST = 0x0E, + CPX_F_BOOTLOADER = 0x0F, } CPXFunction_t; typedef struct { CPXTarget_t destination; CPXTarget_t source; + bool lastPacket; CPXFunction_t function; } CPXRouting_t; typedef struct { - CPXRouting_t route; - uint8_t data[AIDECK_UART_TRANSPORT_MTU-2]; + CPXRouting_t route; + uint16_t dataLength; + uint8_t data[AIDECK_UART_TRANSPORT_MTU-2]; } CPXPacket_t; /** * @brief Receive a CPX packet from the ESP32 - * + * * This function will block until a packet is availale from CPX. The * function will return all packets routed to the STM32. - * + * * @param packet received packet will be stored here - * @return uint32_t size of the data in the packet */ -uint32_t cpxReceivePacketBlocking(CPXPacket_t * packet); +void cpxReceivePacketBlocking(CPXPacket_t * packet); /** * @brief Send a CPX packet to the ESP32 - * + * * This will send a packet to the ESP32 to be routed using CPX. This * will block until the packet can be queued up for sending. - * + * * @param packet packet to be sent - * @param size size of data in packet */ -void cpxSendPacketBlocking(CPXPacket_t * packet, uint32_t size); +void cpxSendPacketBlocking(const CPXPacket_t * packet); /** * @brief Send a CPX packet to the ESP32 - * + * * This will send a packet to the ESP32 to be routed using CPX. - * + * * @param packet packet to be sent - * @param size size of data in packet * @param timeoutInMS timeout before giving up if packet cannot be queued * @return true if package could be queued for sending * @return false if package could not be queued for sending within timeout */ -bool cpxSendPacket(CPXPacket_t * packet, uint32_t size, uint32_t timeoutInMS); \ No newline at end of file +bool cpxSendPacket(const CPXPacket_t * packet, uint32_t timeoutInMS); + +/** + * @brief Initialize CPX routing data. + * + * Initialize values and set lastPacket to true. + * + * @param source The starting point of the packet + * @param destination The destination to send the packet to + * @param function The function of the content + * @param route Pointer to the route data to initialize + */ +void cpxInitRoute(const CPXTarget_t source, const CPXTarget_t destination, const CPXFunction_t function, CPXRouting_t* route); diff --git a/src/deck/drivers/interface/lpsTdma.h b/src/deck/drivers/interface/lpsTdma.h index c6de98e0bf..4cb2824e01 100644 --- a/src/deck/drivers/interface/lpsTdma.h +++ b/src/deck/drivers/interface/lpsTdma.h @@ -32,11 +32,13 @@ #ifndef __LPS_TDMA_H__ #define __LPS_TDMA_H__ -#ifndef TDMA_NSLOTS_BITS -#ifdef LPS_TDMA_ENABLE +#include "autoconf.h" + +#ifndef CONFIG_DECK_LOCO_TDMA_SLOTS +#ifdef CONFIG_DECK_LOCO_TDMA #warning "Number of slots bits for TDMA not defined! Defaulting to 1 (2 slots)." #endif -#define TDMA_NSLOTS_BITS 1 +#define CONFIG_DECK_LOCO_TDMA_SLOTS 1 #endif #ifndef TDMA_SLOT @@ -45,8 +47,8 @@ #define TDMA_SLOT_BITS 27 -#define TDMA_FRAME_BITS (TDMA_SLOT_BITS + TDMA_NSLOTS_BITS) -#define TDMA_SLOT_LEN (1ull<<(unsigned long long)(TDMA_SLOT_BITS+1)) +#define TDMA_FRAME_BITS (TDMA_SLOT_BITS + CONFIG_DECK_LOCO_TDMA_SLOTS) +#define TDMA_SLOT_LEN (1ull<<(unsigned long long)(CONFIG_DECK_LOCO_TDMA_SLOTS+1)) #define TDMA_FRAME_LEN (1ull<<(unsigned long long)(TDMA_FRAME_BITS+1)) #define TDMA_LAST_FRAME(NOW) ( NOW & ~(TDMA_FRAME_LEN-1) ) diff --git a/src/deck/drivers/interface/lpsTdoa2Tag.h b/src/deck/drivers/interface/lpsTdoa2Tag.h index c0f76596e8..5c8d78d771 100644 --- a/src/deck/drivers/interface/lpsTdoa2Tag.h +++ b/src/deck/drivers/interface/lpsTdoa2Tag.h @@ -6,13 +6,11 @@ #include "mac.h" +#include "autoconf.h" + extern uwbAlgorithm_t uwbTdoa2TagAlgorithm; -#ifdef LOCODECK_NR_OF_ANCHORS -#define LOCODECK_NR_OF_TDOA2_ANCHORS LOCODECK_NR_OF_ANCHORS -#else -#define LOCODECK_NR_OF_TDOA2_ANCHORS 8 -#endif +#define LOCODECK_NR_OF_TDOA2_ANCHORS CONFIG_DECK_LOCO_NR_OF_ANCHORS typedef struct { const locoAddress_t anchorAddress[LOCODECK_NR_OF_TDOA2_ANCHORS]; diff --git a/src/deck/drivers/src/Kbuild b/src/deck/drivers/src/Kbuild new file mode 100644 index 0000000000..77ab03bd09 --- /dev/null +++ b/src/deck/drivers/src/Kbuild @@ -0,0 +1,20 @@ +obj-y += test/ +obj-$(CONFIG_DECK_ACTIVE_MARKER) += activeMarkerDeck.o +obj-$(CONFIG_DECK_AI) += aideck.o +obj-$(CONFIG_DECK_AI) += aideck-router.o +obj-$(CONFIG_DECK_BIGQUAD) += bigquad.o +obj-$(CONFIG_DECK_BUZZ) += buzzdeck.o +obj-$(CONFIG_DECK_CPPM) += cppmdeck.o +obj-$(CONFIG_DECK_FLOW) += flowdeck_v1v2.o +obj-$(CONFIG_DECK_GTGPS) += gtgps.o +obj-$(CONFIG_DECK_LEDRING) += ledring12.o +obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse.o +obj-$(CONFIG_DECK_LOCO) += locodeck.o +obj-$(CONFIG_DECK_LOCO) += lpsTdoa2Tag.o +obj-$(CONFIG_DECK_LOCO) += lpsTdoa3Tag.o +obj-$(CONFIG_DECK_LOCO) += lpsTwrTag.o +obj-$(CONFIG_DECK_MULTIRANGER) += multiranger.o +obj-$(CONFIG_DECK_OA) += oa.o +obj-$(CONFIG_DECK_USD) += usddeck.o +obj-$(CONFIG_DECK_ZRANGER) += zranger.o +obj-$(CONFIG_DECK_ZRANGER2) += zranger2.o diff --git a/src/deck/drivers/src/Kconfig b/src/deck/drivers/src/Kconfig new file mode 100644 index 0000000000..383d468a5f --- /dev/null +++ b/src/deck/drivers/src/Kconfig @@ -0,0 +1,320 @@ +config DECK_ACTIVE_MARKER + bool "Support the Active marker deck" + default y + help + The Active marker deck together with Qualisys QTM gives you long + distance and solid tracking. Thanks to the 4 high-power LEDs you + can set up to 4 different IDs giving you the possibility to uniqly + identify a large number of rigid bodies, without the need of many + reflective markers. + +config DECK_AI + bool "Support the AI deck" + default y + depends on !DEBUG_PRINT_ON_UART1 + help + The AI-deck 1.1 extends the computational capabilities and will + enable complex artificial intelligence-based workloads to run + onboard, with the possibility to achieve fully autonomous + navigation capabilities. The ESP32 adds WiFi connectivity with + the possibility to stream images as well as handling control. + +choice + prompt "WiFi setup at startup" + depends on DECK_AI + default DECK_AI_WIFI_NO_SETUP + help + Select if, and how, the WiFi should be set up with on the AI-deck + + config DECK_AI_WIFI_NO_SETUP + bool "No initial WiFi setup" + + config DECK_AI_WIFI_SETUP_AP + bool "Act as Access Point" + + config DECK_AI_WIFI_SETUP_STA + bool "Connect to a WiFi network" +endchoice + +menu "Credentials for access-point" + depends on DECK_AI + depends on !DECK_AI_WIFI_NO_SETUP + config DECK_AI_SSID + string "WiFi SSID" + default "myssid" + help + SSID (network name) for the AI-deck to connect to at startup + + config DECK_AI_PASSWORD + string "WiFi Password" + default "mypassword" + help + WiFi password +endmenu + +config DECK_BIGQUAD + bool "Support the BigQuad deck" + default y + help + With the BigQuad deck you can transform your Crazyflie 2.X to a + bigger quad by connecting external Electronic speed controllers + (ESCs) to the breakout connectors on the deck. The autodetection + of the deck makes this transformation possible to do in seconds. + The deck also contains breakout header connectors for additional + accessories such as external receiver (CPPM input) and GPS. + It can also monitor battery voltage and current. + +config DECK_BIGQUAD_ENABLE + bool "Enable Bigquad deck capabilities" + depends on DECK_BIGQUAD + default n + +config DECK_BIGQUAD_ENABLE_OSD + depends on DECK_BIGQUAD_ENABLE + bool "Enable Bigquad deck OSD" + default n + +config DECK_BIGQUAD_ENABLE_PM + depends on DECK_BIGQUAD_ENABLE + bool "Enable Bigquad deck PM" + default n + +config DECK_BUZZ + bool "Support the Buzzer deck" + default y + help + Sometimes LED feedback isn’t enough, like when you’re flying + around in a lab and living room. With the Buzzer deck you’ll get + audio feedback on system events, like low battery or charging + completed. Or why not create your own melodies that will play + while you’re buzzing around. If you’re tapped out on creativity + there are already a few melodies pre-programmed that you can use. + +config DECK_CPPM + bool "Support the CPPM deck (obsolete)" + default n + help + Combined PPM / PPM-Sum driver. + +config DECK_FLOW + bool "Support the Flow (v1 and v2) deck" + default y + select ESTIMATOR_KALMAN_ENABLE + select DECK_ZRANGER + select DECK_ZRANGER2 + help + The Flow deck gives the Crazyflie 2.X the ability to understand + when it’s moving in any direction. The VL53L1x ToF sensor measures + the distance to the ground with high precision and the PMW3901 + optical flow sensor measures movements in relation to the ground. + This creates a flying 3D robot that can be pre-programmed to fly + distances in any direction or act as an aid for beginners where + it creates a very stable flying platform. + +config DECK_GTGPS + bool "Support the GPS prototype deck (obsolete)" + default n + depends on !DEBUG_PRINT_ON_UART1 + help + GPS prototype deck, not active nor in production. + +config DECK_LEDRING + bool "Support the LED-ring deck" + default y + help + Light up the dark with custom patterns using the Crazyflie 2.1 + LED-ring expansion board. Featuring 12 strong RGB LEDs facing + downwards, you can create custom patterns in firmware that you + control from your computer. Two additional strong front facing + LEDs that can be switched on and off, act as headlights. + +config DECK_LEDRING_DEFAULT_EFFECT + int "Default light effect to use on the LED ring" + depends on DECK_LEDRING + default 6 + help + Index of the default light effect that the LED ring should use + after boot. Use zero to turn off the LEDs when booting. For more + information about the LED ring effects see the parameter documentation + on https://www.bitcraze.io/ + +config DECK_LEDRING_NBR_LEDS + int "Number of LEDs to use on the LED ring" + depends on DECK_LEDRING + default 12 + help + Number of LEDs to use on the LED ring. Values larger than 12 + require a customized LED ring deck. + +config DECK_LEDRING_DIMMER + int "Limit LED ring brightness" + depends on DECK_LEDRING + range 0 8 + default 0 + help + Use this setting to reduce the maximum brightness of the LED ring + when the LEDs and the motors would draw too much power if the LEDs + are operated at peak brightness. Zero means not to limit the + brightness of the LED ring. 1 means to shift the RGB values to the + right by 1 bit (i.e. dividing each RGB component by 2), 2 means to + shift by 2 bits (dividing by 4) and so on. + +config DECK_LIGHTHOUSE + bool "Support the Lighthouse positioning deck" + default y + depends on !DEBUG_PRINT_ON_UART1 + select ESTIMATOR_KALMAN_ENABLE + help + With Crazyflie and the Lighthouse positioning deck, high precision + autonomous flight becomes available for everyone. The Lighthouse + deck uses the HTC Vive base stations (aka Lighthouse V1) or + SteamVR Base Station 2.0 (aka Lighthouse V2) to achieve high + precision positioning. The deck has 4 receivers which gives the + full pose of the Crazyflie. + +config DECK_LIGHTHOUSE_AS_GROUNDTRUTH + bool "Use Lighthouse system as groundtruth" + depends on DECK_LIGHTHOUSE + default n + help + Use the lighthouse system as a groundtruth. This will also switch the + estimation method to the crossing beam position estimate and not send the + position to the kalman estimator and you can + read out the positioning from the loggin lighthouse.x.y.z + +config DECK_LOCO + bool "Support the Loco positioning deck" + default y + select ESTIMATOR_KALMAN_ENABLE + help + The Loco Positioning deck is a Tag in a Loco Positioning system + and measures the distances to Anchors. The distances are used to + estimate the absolute position of the Crazyflie 2.X in the room, + which can be used for autonomous flight. The position is calculated + on-board of the Crazyflie and there is no need for an external + computer for position estimation. + + config DECK_LOCO_NR_OF_ANCHORS + int "The number of anchors in use" + default 8 + depends on DECK_LOCO + help + The number of anchors in your Loco setup. See documentation on + https://www.bitcraze.io/ for more details. + +choice + prompt "Algorithm to use" + depends on DECK_LOCO + default DECK_LOCO_ALGORITHM_AUTO + + config DECK_LOCO_ALGORITHM_AUTO + bool "Let the system decide for you" + + config DECK_LOCO_ALGORITHM_TWR + bool "Use the Two Way Ranging algorithm" + + config DECK_LOCO_ALGORITHM_TDOA2 + bool "Use the Time Difference of Arrival (2) algorithm" + + config DECK_LOCO_ALGORITHM_TDOA3 + bool "Use the Time Difference of Arrival (3) algorithm" +endchoice + +config DECK_LOCO_FULL_TX_POWER + bool "Full TX power" + default n + +config DECK_LOCO_LONGER_RANGE + bool "Longer range mode" + depends on DECK_LOCO_ALGORITHM_TDOA3 + default n + help + Note that anchors need to be built with support for this as well + +config DECK_LOCO_TDMA + bool "Use Time Division Multiple Access" + depends on DECK_LOCO_ALGORITHM_TWR + default n + +config DECK_LOCO_TDMA_SLOTS + int "Number of slots bits for TDMA" + depends on DECK_LOCO_TDMA + default 1 + +config DECK_LOCO_TDMA_SLOT + int "TDMA slot" + depends on DECK_LOCO_TDMA + default 0 + +config DECK_LOCO_2D_POSITION + bool "If set we assume we are doing 2D positioning" + depends on DECK_LOCO && (DECK_LOCO_ALGORITHM_TDOA3 || DECK_LOCO_ALGORITHM_TDOA2) + default n + +# +# Kconfig does not support float, we use a string here and fix it up in the +# Makefile. +# +config DECK_LOCO_2D_POSITION_HEIGHT + string "The height (Z) that the tag will be located at" + depends on DECK_LOCO_2D_POSITION + default 1.2 + +config DECK_MULTIRANGER + bool "Support the Multi-ranger deck" + default y + help + The Multi-ranger deck gives the Crazyflie 2.X the ability to + detect objects around it. This is done by measuring the distance + to objects in the following 5 directions: front/back/left/right/up + with mm precision up to 4 meters. + +config DECK_OA + bool "Support the Obstacle avoidance deck (obsolete)" + default y + help + Codename: Obstacle Avoidance driver, obsolete deck. + +config DECK_USD + bool "Support the Micro SD card deck" + default y + help + The Micro SD card deck adds a Micro SD card reader to the Crazyflie + and makes it possible to read and write files to the SD-card. + The standard firmware has support for high speed logging (up to 1 kHz) + to the SD-card which can record data at rates that are higher than + what can be transfered via radio in real time. When the recoding + is finished the SD-card can be moved to a computer for data analyses. + It is also possible to access the file system from applications + (or any other firmware code) to implement usecases that requires + the use of files. + +config DECK_USD_USE_ALT_PINS_AND_SPI + bool "Use alternate SPI and alternate CS pin" + default n + depends on DECK_USD + help + Changes the DECK_GPIO_RX2 for CS and SPI3 for spi. + This requires hardware changes on the deck. + Check out for instructions on the micro SD card deck + product page on https://www.bitcraze.io/ + +config DECK_ZRANGER + bool "Support the Z-ranger deck V1 (discontinued)" + default n + help + The Z-ranger deck uses a laser sensor to measure the distance to + the ground. The deck enables the Crazyflie to automatically maintain + a constant distance to the objects under it when flying in automatic + mode. Over a floor it will stay on the same height, but will follow + the flow of a stair case up or down when going on adventures. + +config DECK_ZRANGER2 + bool "Support the Z-ranger deck V2" + default y + help + The Z-ranger deck V2 uses a laser sensor to measure the distance to + the ground. The deck enables the Crazyflie to automatically maintain + a constant distance to the objects under it when flying in automatic + mode. Over a floor it will stay on the same height, but will follow + the flow of a stair case up or down when going on adventures. diff --git a/src/deck/drivers/src/aideck-router.c b/src/deck/drivers/src/aideck-router.c new file mode 100644 index 0000000000..59f3d6973d --- /dev/null +++ b/src/deck/drivers/src/aideck-router.c @@ -0,0 +1,133 @@ +/* + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2021 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * aideck.c - Deck driver for the AIdeck + */ +#define DEBUG_MODULE "AIDECK-ROUTER" + +#include + +#include "FreeRTOS.h" +#include "config.h" +#include "debug.h" +#include "FreeRTOS.h" +#include "task.h" +#include "queue.h" +#include +#include +#include +#include "queue.h" +#include "stm32fxxx.h" +#include "system.h" + +#include "aideck.h" +#include "aideck-router.h" + +#define WIFI_SET_SSID_CMD 0x10 +#define WIFI_SET_KEY_CMD 0x11 + +#define WIFI_CONNECT_CMD 0x20 +#define WIFI_CONNECT_AS_AP 0x01 +#define WIFI_CONNECT_AS_STA 0x00 +#define WIFI_CONNECT_AS_LENGTH 2 + +#define WIFI_AP_CONNECTED_CMD 0x31 +#define WIFI_CLIENT_CONNECTED_CMD 0x32 + +static CPXPacket_t cpxRx; + +static void cxpRxTest(void *param) +{ + systemWaitStart(); + while (1) { + cpxReceivePacketBlocking(&cpxRx); + + DEBUG_PRINT("CPX RX: Message from [0x%02X] to function [0x%02X] (size=%u)\n", cpxRx.route.source, cpxRx.route.function, cpxRx.dataLength); + + if (cpxRx.route.source == CPX_T_ESP32) { + switch(cpxRx.route.function) { + case CPX_F_WIFI_CTRL: + if (cpxRx.data[0] == WIFI_AP_CONNECTED_CMD) { + DEBUG_PRINT("WiFi connected to ip: %u.%u.%u.%u\n", + cpxRx.data[1], + cpxRx.data[2], + cpxRx.data[3], + cpxRx.data[4]); + } + if (cpxRx.data[0] == WIFI_CLIENT_CONNECTED_CMD) { + DEBUG_PRINT("WiFi client connected\n"); + } + break; + default: + DEBUG_PRINT("Not handling function [0x%02X]\n", cpxRx.route.function); + } + } + } +} + +#ifndef CONFIG_DECK_AI_WIFI_NO_SETUP +static CPXPacket_t cpxTx; +static void setupWiFi() { +#ifdef CONFIG_DECK_AI_WIFI_SETUP_STA + DEBUG_PRINT("AI-deck will connect to WiFi\n"); +#endif + +#ifdef CONFIG_DECK_AI_WIFI_SETUP_AP + DEBUG_PRINT("AI-deck will become access point\n"); +#endif + + cpxInitRoute(CPX_T_STM32, CPX_T_ESP32, CPX_F_WIFI_CTRL, &cpxTx.route); + + cpxTx.data[0] = WIFI_SET_SSID_CMD; // Set SSID + memcpy(&cpxTx.data[1], CONFIG_DECK_AI_SSID, sizeof(CONFIG_DECK_AI_SSID)); + cpxTx.dataLength = sizeof(CONFIG_DECK_AI_SSID); + cpxSendPacketBlocking(&cpxTx); + + cpxTx.data[0] = WIFI_SET_KEY_CMD; // Set SSID + memcpy(&cpxTx.data[1], CONFIG_DECK_AI_PASSWORD, sizeof(CONFIG_DECK_AI_PASSWORD)); + cpxTx.dataLength = sizeof(CONFIG_DECK_AI_PASSWORD); + cpxSendPacketBlocking(&cpxTx); + + cpxTx.data[0] = WIFI_CONNECT_CMD; // Connect wifi +#ifdef CONFIG_DECK_AI_WIFI_SETUP_STA + cpxTx.data[1] = WIFI_CONNECT_AS_STA; +#endif +#ifdef CONFIG_DECK_AI_WIFI_SETUP_AP + cpxTx.data[1] = WIFI_CONNECT_AS_AP; +#endif + cpxTx.dataLength = WIFI_CONNECT_AS_LENGTH; + cpxSendPacketBlocking(&cpxTx); +} +#endif + +void aideckRouterInit(void) { + xTaskCreate(cxpRxTest, "CPX Router RX", configMINIMAL_STACK_SIZE, NULL, + AI_DECK_TASK_PRI, NULL); + +#ifdef CONFIG_DECK_AI_WIFI_NO_SETUP + DEBUG_PRINT("Not setting up WiFi\n"); +#else + setupWiFi(); +#endif + +} \ No newline at end of file diff --git a/src/deck/drivers/src/aideck.c b/src/deck/drivers/src/aideck.c index dd8a1e71e1..a7aad078da 100644 --- a/src/deck/drivers/src/aideck.c +++ b/src/deck/drivers/src/aideck.c @@ -54,49 +54,74 @@ #include "system.h" #include "aideck.h" +#include "aideck-router.h" static bool isInit = false; static bool reset = false; static uint8_t byte; -#define UART_TRANSPORT_HEADER_SIZE (2) - #define ESP_TX_QUEUE_LENGTH 4 #define ESP_RX_QUEUE_LENGTH 4 static xQueueHandle espTxQueue; static xQueueHandle espRxQueue; -// These structs are used when sending/receiving data -typedef struct -{ - uint8_t start; // Should be 0xFF - uint8_t length; // Length of data - uint8_t data[AIDECK_UART_TRANSPORT_MTU]; + +// Length of start + payloadLength +#define UART_HEADER_LENGTH 2 +#define UART_CRC_LENGTH 1 +#define UART_META_LENGTH (UART_HEADER_LENGTH + UART_CRC_LENGTH) + +typedef struct { + CPXTarget_t destination : 3; + CPXTarget_t source : 3; + bool lastPacket : 1; + bool reserved : 1; + CPXFunction_t function : 8; +} __attribute__((packed)) CPXRoutingPacked_t; + +#define CPX_ROUTING_PACKED_SIZE (sizeof(CPXRoutingPacked_t)) + +typedef struct { + CPXRoutingPacked_t route; + uint8_t data[AIDECK_UART_TRANSPORT_MTU - CPX_ROUTING_PACKED_SIZE]; +} __attribute__((packed)) uartTransportPayload_t; + +typedef struct { + uint8_t start; + uint8_t payloadLength; // Excluding start and crc + union { + uartTransportPayload_t routablePayload; + uint8_t payload[AIDECK_UART_TRANSPORT_MTU]; + }; + + uint8_t crcPlaceHolder; // Not actual position. CRC is added after the last byte of payload } __attribute__((packed)) uart_transport_packet_t; + +// Used when sending/receiving data on the UART static uart_transport_packet_t espTxp; +static CPXPacket_t cpxTxp; static uart_transport_packet_t espRxp; -// These structs are used before/after sending/receiving though the queues -// to the user -typedef struct -{ - uint8_t start; // Should be 0xFF - uint8_t length; // Length data from cpxDst - uint8_t cpxDst : 4; - uint8_t cpxSrc : 4; - uint8_t cpxFunc; - uint8_t data[AIDECK_UART_TRANSPORT_MTU - CPX_HEADER_SIZE]; -} __attribute__((packed)) uart_transport_with_routing_packet_t; - -static uart_transport_with_routing_packet_t cpxTxp; -static uart_transport_with_routing_packet_t cpxRxp; - static EventGroupHandle_t evGroup; #define ESP_CTS_EVENT (1 << 0) #define ESP_CTR_EVENT (1 << 1) #define ESP_TXQ_EVENT (1 << 2) +static void assemblePacket(const CPXPacket_t *packet, uart_transport_packet_t * txp); + +static uint8_t calcCrc(const uart_transport_packet_t* packet) { + const uint8_t* start = (const uint8_t*) packet; + const uint8_t* end = &packet->payload[packet->payloadLength]; + + uint8_t crc = 0; + for (const uint8_t* p = start; p < end; p++) { + crc ^= *p; + } + + return crc; +} + static void ESP_RX(void *param) { systemWaitStart(); @@ -109,19 +134,23 @@ static void ESP_RX(void *param) uart2GetDataWithTimeout(&espRxp.start, (TickType_t)portMAX_DELAY); } while (espRxp.start != 0xFF); - uart2GetDataWithTimeout(&espRxp.length, (TickType_t)portMAX_DELAY); + uart2GetDataWithTimeout(&espRxp.payloadLength, (TickType_t)portMAX_DELAY); - if (espRxp.length == 0) + if (espRxp.payloadLength == 0) { xEventGroupSetBits(evGroup, ESP_CTS_EVENT); } else { - for (int i = 0; i < espRxp.length; i++) + for (int i = 0; i < espRxp.payloadLength; i++) { - uart2GetDataWithTimeout(&espRxp.data[i], (TickType_t)portMAX_DELAY); + uart2GetDataWithTimeout(&espRxp.payload[i], (TickType_t)portMAX_DELAY); } + uint8_t crc; + uart2GetDataWithTimeout(&crc, (TickType_t)portMAX_DELAY); + ASSERT(crc == calcCrc(&espRxp)); + xQueueSend(espRxQueue, &espRxp, portMAX_DELAY); xEventGroupSetBits(evGroup, ESP_CTR_EVENT); } @@ -169,7 +198,9 @@ static void ESP_TX(void *param) if (uxQueueMessagesWaiting(espTxQueue) > 0) { // Dequeue and wait for either CTS or CTR - xQueueReceive(espTxQueue, &espTxp, 0); + xQueueReceive(espTxQueue, &cpxTxp, 0); + espTxp.start = 0xFF; + assemblePacket(&cpxTxp, &espTxp); do { evBits = xEventGroupWaitBits(evGroup, @@ -182,8 +213,7 @@ static void ESP_TX(void *param) uart2SendData(sizeof(ctr), (uint8_t *)&ctr); } } while ((evBits & ESP_CTS_EVENT) != ESP_CTS_EVENT); - espTxp.start = 0xFF; - uart2SendData((uint32_t) espTxp.length + UART_TRANSPORT_HEADER_SIZE, (uint8_t *)&espTxp); + uart2SendData((uint32_t) espTxp.payloadLength + UART_META_LENGTH, (uint8_t *)&espTxp); } } } @@ -212,60 +242,62 @@ static void Gap8Task(void *param) paramSetInt(paramGetVarId("aiDeck", "reset"), 0); } - uart1GetDataWithDefaultTimeout(&byte); + uart1GetDataWithTimeout(&byte, portMAX_DELAY); consolePutchar(byte); } } -uint32_t cpxReceivePacketBlocking(CPXPacket_t *packet) -{ - uint32_t size; - xQueueReceive(espRxQueue, &cpxRxp, portMAX_DELAY); - size = (uint32_t) cpxRxp.length - CPX_HEADER_SIZE; - packet->route.destination = cpxRxp.cpxDst; - packet->route.source = cpxRxp.cpxSrc; - packet->route.function = cpxRxp.cpxFunc; - memcpy(packet->data, cpxRxp.data, size); - return size; -} - -void cpxSendPacketBlocking(CPXPacket_t *packet, uint32_t size) -{ +static void assemblePacket(const CPXPacket_t *packet, uart_transport_packet_t * txp) { ASSERT((packet->route.destination >> 4) == 0); ASSERT((packet->route.source >> 4) == 0); ASSERT((packet->route.function >> 8) == 0); - ASSERT(size <= AIDECK_UART_TRANSPORT_MTU - CPX_HEADER_SIZE); + ASSERT(packet->dataLength <= AIDECK_UART_TRANSPORT_MTU - CPX_ROUTING_PACKED_SIZE); + + txp->payloadLength = packet->dataLength + CPX_ROUTING_PACKED_SIZE; + txp->routablePayload.route.destination = packet->route.destination; + txp->routablePayload.route.source = packet->route.source; + txp->routablePayload.route.lastPacket = packet->route.lastPacket; + txp->routablePayload.route.function = packet->route.function; + memcpy(txp->routablePayload.data, &packet->data, packet->dataLength); + txp->payload[txp->payloadLength] = calcCrc(txp); +} - cpxTxp.length = (uint8_t) size + CPX_HEADER_SIZE; - cpxTxp.cpxDst = packet->route.destination; - cpxTxp.cpxSrc = packet->route.source; - cpxTxp.cpxFunc = packet->route.function; - memcpy(cpxTxp.data, &packet->data, size); +void cpxReceivePacketBlocking(CPXPacket_t *packet) +{ + static uart_transport_packet_t cpxRxp; - xQueueSend(espTxQueue, &cpxTxp, (TickType_t)portMAX_DELAY); - xEventGroupSetBits(evGroup, ESP_TXQ_EVENT); + xQueueReceive(espRxQueue, &cpxRxp, portMAX_DELAY); + + packet->dataLength = (uint32_t) cpxRxp.payloadLength - CPX_ROUTING_PACKED_SIZE; + packet->route.destination = cpxRxp.routablePayload.route.destination; + packet->route.source = cpxRxp.routablePayload.route.source; + packet->route.function = cpxRxp.routablePayload.route.function; + packet->route.lastPacket = cpxRxp.routablePayload.route.lastPacket; + memcpy(&packet->data, cpxRxp.routablePayload.data, packet->dataLength); } -bool cpxSendPacket(CPXPacket_t *packet, uint32_t size, uint32_t timeoutInMS) +void cpxSendPacketBlocking(const CPXPacket_t *packet) { - ASSERT((packet->route.destination >> 4) == 0); - ASSERT((packet->route.source >> 4) == 0); - ASSERT((packet->route.function >> 8) == 0); - ASSERT(size <= AIDECK_UART_TRANSPORT_MTU - CPX_HEADER_SIZE); - - bool packageWasSent = false; - cpxTxp.length = (uint8_t) size + CPX_HEADER_SIZE; - cpxTxp.cpxDst = packet->route.destination; - cpxTxp.cpxSrc = packet->route.source; - cpxTxp.cpxFunc = packet->route.function; - memcpy(cpxTxp.data, &packet->data, size); + xQueueSend(espTxQueue, packet, portMAX_DELAY); + xEventGroupSetBits(evGroup, ESP_TXQ_EVENT); +} - if (xQueueSend(espTxQueue, &cpxTxp, M2T(timeoutInMS)) == pdTRUE) +bool cpxSendPacket(const CPXPacket_t *packet, uint32_t timeoutInMS) +{ + bool packetWasSent = false; + if (xQueueSend(espTxQueue, packet, M2T(timeoutInMS)) == pdTRUE) { xEventGroupSetBits(evGroup, ESP_TXQ_EVENT); - packageWasSent = true; + packetWasSent = true; } - return packageWasSent; + return packetWasSent; +} + +void cpxInitRoute(const CPXTarget_t source, const CPXTarget_t destination, const CPXFunction_t function, CPXRouting_t* route) { + route->source = source; + route->destination = destination; + route->function = function; + route->lastPacket = true; } static void aideckInit(DeckInfo *info) @@ -278,7 +310,7 @@ static void aideckInit(DeckInfo *info) xTaskCreate(Gap8Task, AI_DECK_GAP_TASK_NAME, AI_DECK_TASK_STACKSIZE, NULL, AI_DECK_TASK_PRI, NULL); - espTxQueue = xQueueCreate(ESP_TX_QUEUE_LENGTH, sizeof(uart_transport_packet_t)); + espTxQueue = xQueueCreate(ESP_TX_QUEUE_LENGTH, sizeof(CPXPacket_t)); espRxQueue = xQueueCreate(ESP_RX_QUEUE_LENGTH, sizeof(uart_transport_packet_t)); evGroup = xEventGroupCreate(); @@ -300,6 +332,8 @@ static void aideckInit(DeckInfo *info) digitalWrite(DECK_GPIO_IO4, HIGH); pinMode(DECK_GPIO_IO4, INPUT_PULLUP); + aideckRouterInit(); + isInit = true; } diff --git a/src/deck/drivers/src/bigquad.c b/src/deck/drivers/src/bigquad.c index 3a48843e69..f69618ae30 100644 --- a/src/deck/drivers/src/bigquad.c +++ b/src/deck/drivers/src/bigquad.c @@ -29,6 +29,7 @@ #include #include "stm32fxxx.h" +#include "autoconf.h" #include "config.h" #include "motors.h" #include "debug.h" @@ -47,12 +48,12 @@ #define BIGQUAD_BAT_CURR_PIN DECK_GPIO_SCK #define BIGQUAD_BAT_AMP_PER_VOLT 1.0f -#ifdef ENABLE_BQ_DECK +#ifdef CONFIG_DECK_BIGQUAD_ENABLE //Hardware configuration static bool isInit; -#ifdef BQ_DECK_ENABLE_OSD +#ifdef CONFIG_DECK_BIGQUAD_ENABLE_OSD static MspObject s_MspObject; static void osdTask(void *param) @@ -70,7 +71,7 @@ static void osdResponseCallback(uint8_t* pBuffer, uint32_t bufferLen) { uart1SendData(bufferLen, pBuffer); } -#endif // BQ_DECK_ENABLE_OSD +#endif // CONFIG_DECK_BIGQUAD_ENABLE_OSD static void bigquadInit(DeckInfo *info) @@ -82,12 +83,12 @@ static void bigquadInit(DeckInfo *info) DEBUG_PRINT("Switching to brushless.\n"); motorsInit(motorMapBigQuadDeck); extRxInit(); -#ifdef BQ_DECK_ENABLE_PM +#ifdef CONFIG_DECK_BIGQUAD_ENABLE_PM pmEnableExtBatteryVoltMeasuring(BIGQUAD_BAT_VOLT_PIN, BIGQUAD_BAT_VOLT_MULT); pmEnableExtBatteryCurrMeasuring(BIGQUAD_BAT_CURR_PIN, BIGQUAD_BAT_AMP_PER_VOLT); #endif -#ifdef BQ_DECK_ENABLE_OSD +#ifdef CONFIG_DECK_BIGQUAD_ENABLE_OSD uart1Init(115200); mspInit(&s_MspObject, osdResponseCallback); xTaskCreate(osdTask, BQ_OSD_TASK_NAME, @@ -131,4 +132,4 @@ PARAM_GROUP_START(deck) PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcBigQuad, &isInit) PARAM_GROUP_STOP(deck) -#endif // ENABLE_BQ_DECK +#endif // CONFIG_DECK_BIGQUAD_ENABLE diff --git a/src/deck/drivers/src/ledring12.c b/src/deck/drivers/src/ledring12.c index 29ec6c5312..e5590a8a8a 100644 --- a/src/deck/drivers/src/ledring12.c +++ b/src/deck/drivers/src/ledring12.c @@ -42,16 +42,11 @@ #include "log.h" #include "pulse_processor.h" #include "mem.h" +#include "autoconf.h" #define DEBUG_MODULE "LED" #include "debug.h" -#ifdef LED_RING_NBR_LEDS -#define NBR_LEDS LED_RING_NBR_LEDS -#else -#define NBR_LEDS 12 -#endif - #ifndef LEDRING_TIME_MEM_SIZE #define LEDRING_TIME_MEM_SIZE 10 #endif @@ -69,7 +64,7 @@ typedef struct timings { ledtiming timings[LEDRING_TIME_MEM_SIZE]; } ledtimings; -static uint8_t ledringmem[NBR_LEDS * 2]; +static uint8_t ledringmem[CONFIG_DECK_LEDRING_NBR_LEDS * 2]; static ledtimings ledringtimingsmem; static bool isInit = false; @@ -138,9 +133,7 @@ typedef void (*Ledring12Effect)(uint8_t buffer[][3], bool reset); dest[1] = ((uint16_t)G6 * 259 + 33) >> 6; \ dest[2] = ((uint16_t)B5 * 527 + 23) >> 6; -#ifndef LEDRING_DEFAULT_EFFECT -#define LEDRING_DEFAULT_EFFECT 6 -#endif +#define LEDRING_DEFAULT_EFFECT CONFIG_DECK_LEDRING_DEFAULT_EFFECT #define LEDRING_TIME_MEM_SEC 1000 / 25 @@ -167,7 +160,7 @@ static void blackEffect(uint8_t buffer[][3], bool reset) if (reset) { - for (i=0; i 12 -static const uint8_t whiteRing[NBR_LEDS][3] = {{32, 32, 32}, {8,8,8}, {2,2,2}, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - }; +#if CONFIG_DECK_LEDRING_NBR_LEDS > 12 +static const uint8_t whiteRing[CONFIG_DECK_LEDRING_NBR_LEDS][3] = { + {32, 32, 32}, {8,8,8}, {2,2,2}, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, +}; #else -static const uint8_t whiteRing[][3] = {{32, 32, 32}, {8,8,8}, {2,2,2}, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - }; +static const uint8_t whiteRing[][3] = { + {32, 32, 32}, {8,8,8}, {2,2,2}, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, +}; #endif -#if NBR_LEDS > 12 -static const uint8_t blueRing[NBR_LEDS][3] = {{64, 64, 255}, {32,32,64}, {8,8,16}, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - }; +#if CONFIG_DECK_LEDRING_NBR_LEDS > 12 +static const uint8_t blueRing[CONFIG_DECK_LEDRING_NBR_LEDS][3] = { + {64, 64, 255}, {32,32,64}, {8,8,16}, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, +}; #else -static const uint8_t blueRing[][3] = {{64, 64, 255}, {32,32,64}, {8,8,16}, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - BLACK, BLACK, BLACK, - }; +static const uint8_t blueRing[][3] = { + {64, 64, 255}, {32,32,64}, {8,8,16}, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, + BLACK, BLACK, BLACK, +}; #endif -// #if NBR_LEDS > 12 -// static const uint8_t greenRing[NBR_LEDS][3] = {{64, 255, 64}, {32,64,32}, {8,16,8}, -// BLACK, BLACK, BLACK, -// BLACK, BLACK, BLACK, -// BLACK, BLACK, BLACK, -// }; +// #if CONFIG_DECK_LEDRING_NBR_LEDS > 12 +// static const uint8_t greenRing[CONFIG_DECK_LEDRING_NBR_LEDS][3] = { +// {64, 255, 64}, {32,64,32}, {8,16,8}, +// BLACK, BLACK, BLACK, +// BLACK, BLACK, BLACK, +// BLACK, BLACK, BLACK, +// }; // #else -// static const uint8_t greenRing[][3] = {{64, 255, 64}, {32,64,32}, {8,16,8}, -// BLACK, BLACK, BLACK, -// BLACK, BLACK, BLACK, -// BLACK, BLACK, BLACK, -// }; +// static const uint8_t greenRing[][3] = { +// {64, 255, 64}, {32,64,32}, {8,16,8}, +// BLACK, BLACK, BLACK, +// BLACK, BLACK, BLACK, +// BLACK, BLACK, BLACK, +// }; // #endif -// #if NBR_LEDS > 12 -// static const uint8_t redRing[NBR_LEDS][3] = {{64, 0, 0}, {16,0,0}, {8,0,0}, -// {4,0,0}, {2,0,0}, {1,0,0}, -// BLACK, BLACK, BLACK, -// BLACK, BLACK, BLACK, -// }; +// #if CONFIG_DECK_LEDRING_NBR_LEDS > 12 +// static const uint8_t redRing[CONFIG_DECK_LEDRING_NBR_LEDS][3] = { +// {64, 0, 0}, {16,0,0}, {8,0,0}, +// {4,0,0}, {2,0,0}, {1,0,0}, +// BLACK, BLACK, BLACK, +// BLACK, BLACK, BLACK, +// }; // #else -// static const uint8_t redRing[][3] = {{64, 0, 0}, {16,0,0}, {8,0,0}, -// {4,0,0}, {2,0,0}, {1,0,0}, -// BLACK, BLACK, BLACK, -// BLACK, BLACK, BLACK, -// }; +// static const uint8_t redRing[][3] = { +// {64, 0, 0}, {16,0,0}, {8,0,0}, +// {4,0,0}, {2,0,0}, {1,0,0}, +// BLACK, BLACK, BLACK, +// BLACK, BLACK, BLACK, +// }; // #endif static void whiteSpinEffect(uint8_t buffer[][3], bool reset) @@ -239,16 +240,16 @@ static void whiteSpinEffect(uint8_t buffer[][3], bool reset) if (reset) { - for (i=0; i 12 -static const uint8_t colorRing[NBR_LEDS][3] = {{0,0,32}, {0,0,16}, {0,0,8}, - {0,0,4}, {16,16,16}, {8,8,8}, - {4,4,4},{32,0,0},{16,0,0}, - {8,0,0}, {4,0,0}, {2,0,0}, - }; +#if CONFIG_DECK_LEDRING_NBR_LEDS > 12 +static const uint8_t colorRing[CONFIG_DECK_LEDRING_NBR_LEDS][3] = { + {0,0,32}, {0,0,16}, {0,0,8}, + {0,0,4}, {16,16,16}, {8,8,8}, + {4,4,4}, {32,0,0},{16,0,0}, + {8,0,0}, {4,0,0}, {2,0,0}, +}; #else -static const uint8_t colorRing[][3] = {{0,0,32}, {0,0,16}, {0,0,8}, - {0,0,4}, {16,16,16}, {8,8,8}, - {4,4,4},{32,0,0},{16,0,0}, - {8,0,0}, {4,0,0}, {2,0,0}, - }; +static const uint8_t colorRing[][3] = { + {0,0,32}, {0,0,16}, {0,0,8}, + {0,0,4}, {16,16,16}, {8,8,8}, + {4,4,4},{32,0,0},{16,0,0}, + {8,0,0}, {4,0,0}, {2,0,0}, +}; #endif static void colorSpinEffect(uint8_t buffer[][3], bool reset) @@ -351,16 +354,16 @@ static void colorSpinEffect(uint8_t buffer[][3], bool reset) if (reset) { - for (i=0; i0; i--) { + COPY_COLOR(temp, buffer[(CONFIG_DECK_LEDRING_NBR_LEDS-1)]); + for (i=(CONFIG_DECK_LEDRING_NBR_LEDS-1); i>0; i--) { COPY_COLOR(buffer[i], buffer[i-1]); } COPY_COLOR(buffer[0], temp); } static void doubleSpinEffect(uint8_t buffer[][3], bool reset) { - static uint8_t sub1[NBR_LEDS][3]; - static uint8_t sub2[NBR_LEDS][3]; + static uint8_t sub1[CONFIG_DECK_LEDRING_NBR_LEDS][3]; + static uint8_t sub2[CONFIG_DECK_LEDRING_NBR_LEDS][3]; int i; static int step; @@ -395,7 +398,7 @@ static void doubleSpinEffect(uint8_t buffer[][3], bool reset) { //if ((step%3)) spinEffect2(sub2, false); //if (reset) spinEffect2(sub2, true); - for (i=0; i NBR_LEDS / 2) { - distance = NBR_LEDS - distance; + if (distance > CONFIG_DECK_LEDRING_NBR_LEDS / 2) { + distance = CONFIG_DECK_LEDRING_NBR_LEDS - distance; } int col = height - distance * (height / (width / 2)); @@ -550,7 +553,7 @@ static void brightnessEffect(uint8_t buffer[][3], bool reset) gyroY = DEADBAND(gyroY, 5); gyroZ = DEADBAND(gyroZ, 5); - for (i=0; i < NBR_LEDS; i++) + for (i=0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { buffer[i][0] = (uint8_t)(LIMIT(gyroZ)); buffer[i][1] = (uint8_t)(LIMIT(gyroY)); @@ -589,7 +592,7 @@ static void ledTestEffect(uint8_t buffer[][3], bool reset) if (brightness<1) brightness += 0.05f; else brightness = 1; - for (i=0; i> 8) & 0x0FF; currentBlue = (fadeColor >> 0) & 0x0FF; - for (int i = 0; i < NBR_LEDS; i++) + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { buffer[i][0] = currentRed; buffer[i][1] = currentGreen; @@ -764,7 +767,7 @@ static void rssiEffect(uint8_t buffer[][3], bool reset) rssi = logGetFloat(rssiId); uint8_t rssi_scaled = LIMIT(LINSCALE(badRssi, goodRssi, 0, 255, rssi)); - for (i = 0; i < NBR_LEDS; i++) { + for (i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { if (isConnected) { buffer[i][0] = 255 - rssi_scaled; // Red (bad) buffer[i][1] = rssi_scaled; // Green (good) @@ -786,7 +789,7 @@ static void lighthouseEffect(uint8_t buffer[][3], bool reset) { uint16_t validAngles = pulseProcessorAnglesQuality(); - for (int i = 0; i < NBR_LEDS; i++) { + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { buffer[i][0] = LIMIT(LINSCALE(0.0f, 255.0f, 100.0f, 0.0f, validAngles)); // Red (small validAngles) buffer[i][1] = LIMIT(LINSCALE(0.0f, 255.0f, 0.0f, 100.0f, validAngles)); // Green (large validAngles) buffer[i][2] = 0; @@ -824,7 +827,7 @@ static void locSrvStatus(uint8_t buffer[][3], bool reset) batteryEverLow = true; } - for (int i = 0; i < NBR_LEDS; i++) { + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { if (batteryEverLow && tic < 10) { buffer[i][0] = 0; buffer[i][1] = 0; @@ -848,14 +851,14 @@ static bool isTimeMemDone(ledtiming current) static int timeEffectI = 0; static uint64_t timeEffectTime = 0; -static uint8_t timeEffectPrevBuffer[NBR_LEDS][3]; +static uint8_t timeEffectPrevBuffer[CONFIG_DECK_LEDRING_NBR_LEDS][3]; static float timeEffectRotation = 0; static void timeMemEffect(uint8_t outputBuffer[][3], bool reset) { // Start timer when going to this if (reset) { - for (int i = 0; i < NBR_LEDS; i++) { + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { COPY_COLOR(timeEffectPrevBuffer[i], part_black); COPY_COLOR(outputBuffer[i], part_black); } @@ -879,7 +882,7 @@ static void timeMemEffect(uint8_t outputBuffer[][3], bool reset) RGB565_TO_RGB888(color, current.color) if (current.leds == 0) { - for (int i = 0; i < NBR_LEDS; i++) { + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { COPY_COLOR(timeEffectPrevBuffer[i], color); } } else { @@ -900,15 +903,15 @@ static void timeMemEffect(uint8_t outputBuffer[][3], bool reset) // Apply the current effect uint8_t color[3]; RGB565_TO_RGB888(color, current.color) - uint8_t currentBuffer[NBR_LEDS][3]; - for (int i = 0; i < NBR_LEDS; i++) { + uint8_t currentBuffer[CONFIG_DECK_LEDRING_NBR_LEDS][3]; + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { COPY_COLOR(currentBuffer[i], timeEffectPrevBuffer[i]); } if (current.fade) { float percent = 1.0 * (time - timeEffectTime) / (current.duration * LEDRING_TIME_MEM_SEC); if (current.leds == 0) - for (int i = 0; i < NBR_LEDS; i++) + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) for (int j = 0; j < 3; j++) currentBuffer[i][j] = (1.0f - percent) * timeEffectPrevBuffer[i][j] + percent * color[j]; else @@ -917,7 +920,7 @@ static void timeMemEffect(uint8_t outputBuffer[][3], bool reset) } else { if (current.leds == 0) { - for (int i = 0; i < NBR_LEDS; i++) { + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { COPY_COLOR(currentBuffer[i], color); } } else { @@ -930,16 +933,16 @@ static void timeMemEffect(uint8_t outputBuffer[][3], bool reset) rotate += 1.0f * (time - timeEffectTime) / (current.rotate * 1000); } - int shift = rotate * NBR_LEDS; - float percentShift = rotate * NBR_LEDS - shift; - shift = shift % NBR_LEDS; + int shift = rotate * CONFIG_DECK_LEDRING_NBR_LEDS; + float percentShift = rotate * CONFIG_DECK_LEDRING_NBR_LEDS - shift; + shift = shift % CONFIG_DECK_LEDRING_NBR_LEDS; // Output current leds - for (int i = 0; i < NBR_LEDS; i++) + for (int i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) for (int j = 0; j < 3; j++) - outputBuffer[(i+shift) % NBR_LEDS][j] = + outputBuffer[(i+shift) % CONFIG_DECK_LEDRING_NBR_LEDS][j] = percentShift * currentBuffer[i][j] + - (1-percentShift) * currentBuffer[(i+1) % NBR_LEDS][j]; + (1-percentShift) * currentBuffer[(i+1) % CONFIG_DECK_LEDRING_NBR_LEDS][j]; } /**************** Effect list ***************/ @@ -1003,7 +1006,7 @@ static void overrideWithLightSignal(uint8_t buffer[][3]) color = (diffMsec <= 100) ? 255 : 0; } - memset(buffer, color, NBR_LEDS * 3); + memset(buffer, color, CONFIG_DECK_LEDRING_NBR_LEDS * 3); } } @@ -1015,11 +1018,11 @@ static xTimerHandle timer; void ledring12Worker(void * data) { static int current_effect = 0; - static uint8_t buffer[NBR_LEDS][3]; + static uint8_t buffer[CONFIG_DECK_LEDRING_NBR_LEDS][3]; bool reset = true; if (/*!pmIsDischarging() ||*/ (effect > neffect)) { - ws2812Send(black, NBR_LEDS); + ws2812Send(black, CONFIG_DECK_LEDRING_NBR_LEDS); return; } @@ -1037,7 +1040,16 @@ void ledring12Worker(void * data) effectsFct[current_effect](buffer, reset); overrideWithLightSignal(buffer); - ws2812Send(buffer, NBR_LEDS); + + if (CONFIG_DECK_LEDRING_DIMMER) { + for (uint8_t i = 0; i < CONFIG_DECK_LEDRING_NBR_LEDS; i++) { + for (uint8_t j = 0; j < 3; j++) { + buffer[i][j] = buffer[i][j] >> CONFIG_DECK_LEDRING_DIMMER; + } + } + } + + ws2812Send(buffer, CONFIG_DECK_LEDRING_NBR_LEDS); } static void ledring12Timer(xTimerHandle timer) diff --git a/src/deck/drivers/src/locodeck.c b/src/deck/drivers/src/locodeck.c index 5f3988fa08..08c60a76ac 100644 --- a/src/deck/drivers/src/locodeck.c +++ b/src/deck/drivers/src/locodeck.c @@ -41,6 +41,7 @@ #include "task.h" #include "queue.h" +#include "autoconf.h" #include "deck.h" #include "system.h" #include "debug.h" @@ -90,11 +91,11 @@ static lpsAlgoOptions_t algoOptions = { // .userRequestedMode is the wanted algorithm, available as a parameter -#if LPS_TDOA_ENABLE +#if defined(CONFIG_DECK_LOCO_ALGORITHM_TDOA2) .userRequestedMode = lpsMode_TDoA2, -#elif LPS_TDOA3_ENABLE +#elif defined(CONFIG_DECK_LOCO_ALGORITHM_TDOA3) .userRequestedMode = lpsMode_TDoA3, -#elif defined(LPS_TWR_ENABLE) +#elif defined(CONFIG_DECK_LOCO_ALGORITHM_TWR) .userRequestedMode = lpsMode_TWR, #else .userRequestedMode = lpsMode_auto, @@ -116,9 +117,9 @@ struct { [lpsMode_TDoA3] = {.algorithm = &uwbTdoa3TagAlgorithm, .name="TDoA3"}, }; -#if LPS_TDOA_ENABLE +#if defined(CONFIG_DECK_LOCO_ALGORITHM_TDOA2) static uwbAlgorithm_t *algorithm = &uwbTdoa2TagAlgorithm; -#elif LPS_TDOA3_ENABLE +#elif defined(CONFIG_DECK_LOCO_ALGORITHM_TDOA3) static uwbAlgorithm_t *algorithm = &uwbTdoa3TagAlgorithm; #else static uwbAlgorithm_t *algorithm = &uwbTwrTagAlgorithm; @@ -515,7 +516,7 @@ static void dwm1000Init(DeckInfo *info) dwSetDefaults(dwm); - #ifdef LPS_LONGER_RANGE + #ifdef CONFIG_DECK_LOCO_LONGER_RANGE dwEnableMode(dwm, MODE_SHORTDATA_MID_ACCURACY); #else dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY); @@ -524,7 +525,7 @@ static void dwm1000Init(DeckInfo *info) dwSetChannel(dwm, CHANNEL_2); dwSetPreambleCode(dwm, PREAMBLE_CODE_64MHZ_9); - #ifdef LPS_FULL_TX_POWER + #ifdef CONFIG_DECK_LOCO_FULL_TX_POWER dwUseSmartPower(dwm, false); dwSetTxPower(dwm, 0x1F1F1F1Ful); #else diff --git a/src/deck/drivers/src/lpsTdoa2Tag.c b/src/deck/drivers/src/lpsTdoa2Tag.c index a49993f789..416506d6d5 100644 --- a/src/deck/drivers/src/lpsTdoa2Tag.c +++ b/src/deck/drivers/src/lpsTdoa2Tag.c @@ -25,10 +25,12 @@ #include +#include #include "FreeRTOS.h" #include "task.h" +#include "autoconf.h" #include "log.h" #include "param.h" #include "lpsTdoa2Tag.h" @@ -277,12 +279,10 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) { static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) { estimatorEnqueueTDOA(tdoaMeasurement); - #ifdef LPS_2D_POSITION_HEIGHT - // If LPS_2D_POSITION_HEIGHT is defined we assume that we are doing 2D positioning. - // LPS_2D_POSITION_HEIGHT contains the height (Z) that the tag will be located at + #ifdef CONFIG_DECK_LOCO_2D_POSITION heightMeasurement_t heightData; heightData.timestamp = xTaskGetTickCount(); - heightData.height = LPS_2D_POSITION_HEIGHT; + heightData.height = DECK_LOCO_2D_POSITION_HEIGHT; heightData.stdDev = 0.0001; estimatorEnqueueAbsoluteHeight(&heightData); #endif diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c index 51b0a1038e..fe39059bad 100644 --- a/src/deck/drivers/src/lpsTdoa3Tag.c +++ b/src/deck/drivers/src/lpsTdoa3Tag.c @@ -43,6 +43,7 @@ The implementation must handle */ #include +#include #include "FreeRTOS.h" #include "task.h" @@ -260,12 +261,10 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) { static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) { estimatorEnqueueTDOA(tdoaMeasurement); - #ifdef LPS_2D_POSITION_HEIGHT - // If LPS_2D_POSITION_HEIGHT is defined we assume that we are doing 2D positioning. - // LPS_2D_POSITION_HEIGHT contains the height (Z) that the tag will be located at + #ifdef CONFIG_DECK_LOCO_2D_POSITION heightMeasurement_t heightData; heightData.timestamp = xTaskGetTickCount(); - heightData.height = LPS_2D_POSITION_HEIGHT; + heightData.height = DECK_LOCO_2D_POSITION_HEIGHT; heightData.stdDev = 0.0001; estimatorEnqueueAbsoluteHeight(&heightData); #endif @@ -297,8 +296,8 @@ static void Initialize(dwDevice_t *dev) { uint32_t now_ms = T2M(xTaskGetTickCount()); tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmRandom); - #ifdef LPS_2D_POSITION_HEIGHT - DEBUG_PRINT("2D positioning enabled at %f m height\n", LPS_2D_POSITION_HEIGHT); + #ifdef CONFIG_DECK_LOCO_2D_POSITION + DEBUG_PRINT("2D positioning enabled at %f m height\n", DECK_LOCO_2D_POSITION_HEIGHT); #endif dwSetReceiveWaitTimeout(dev, TDOA3_RECEIVE_TIMEOUT); diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c index d99344dcc2..dade9b1ba9 100644 --- a/src/deck/drivers/src/lpsTwrTag.c +++ b/src/deck/drivers/src/lpsTwrTag.c @@ -455,7 +455,7 @@ static void updateTagTdmaSlot(lpsTwrAlgoOptions_t * options) if (options->tdmaSlot < 0) { uint64_t radioAddress = configblockGetRadioAddress(); int nslot = 1; - for (int i=0; itdmaSlot = radioAddress % nslot; diff --git a/src/deck/drivers/src/test/Kbuild b/src/deck/drivers/src/test/Kbuild new file mode 100644 index 0000000000..f675beca9c --- /dev/null +++ b/src/deck/drivers/src/test/Kbuild @@ -0,0 +1,3 @@ +obj-y += exptest.o +obj-y += exptestRR.o +obj-y += exptestBolt11.o diff --git a/src/deck/drivers/src/test/exptestBolt.c b/src/deck/drivers/src/test/exptestBolt11.c similarity index 98% rename from src/deck/drivers/src/test/exptestBolt.c rename to src/deck/drivers/src/test/exptestBolt11.c index 8c51f1374e..42d888ce84 100644 --- a/src/deck/drivers/src/test/exptestBolt.c +++ b/src/deck/drivers/src/test/exptestBolt11.c @@ -21,9 +21,9 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * exptestBolt.c - Testing of expansion port and motor connectors + * exptestBolt11.c - Testing of expansion port and motor connectors for Bolt 1.1 */ -#define DEBUG_MODULE "ET" +#define DEBUG_MODULE "ET_BOLT" #include @@ -76,7 +76,7 @@ #define ET_GPIO_PORT_M3 GPIOA #define ET_GPIO_PIN_M3 GPIO_Pin_15 #define ET_GPIO_PORT_M4 GPIOB -#define ET_GPIO_PIN_M4 GPIO_Pin_9 +#define ET_GPIO_PIN_M4 GPIO_Pin_10 #define ET_GPIO_PORT_M1_OR GPIOA #define ET_GPIO_PIN_M1_OR GPIO_Pin_0 diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c index 978a591f76..f143bb2a71 100644 --- a/src/deck/drivers/src/usddeck.c +++ b/src/deck/drivers/src/usddeck.c @@ -63,8 +63,10 @@ #include "mem.h" #include "eventtrigger.h" +#include "autoconf.h" + // Hardware defines -#ifdef USDDECK_USE_ALT_PINS_AND_SPI +#ifdef CONFIG_DECK_USD_USE_ALT_PINS_AND_SPI #include "deck_spi3.h" #define USD_CS_PIN DECK_GPIO_RX2 diff --git a/src/drivers/Kbuild b/src/drivers/Kbuild new file mode 100644 index 0000000000..8ef4b4840e --- /dev/null +++ b/src/drivers/Kbuild @@ -0,0 +1,3 @@ +obj-y += src/ +obj-y += bosch/src/ +obj-y += esp32/src/ diff --git a/src/drivers/bosch/src/Kbuild b/src/drivers/bosch/src/Kbuild new file mode 100644 index 0000000000..3ec1b34392 --- /dev/null +++ b/src/drivers/bosch/src/Kbuild @@ -0,0 +1,10 @@ +obj-y += bmi055_accel.o +obj-y += bmi055_gyro.o +obj-y += bmi088_accel.o +obj-y += bmi088_fifo.o +obj-y += bmi088_gyro.o +obj-y += bmi160.o +obj-y += bmm150.o +obj-y += bmp280.o +obj-y += bmp3.o +obj-y += bstdr_comm_support.o diff --git a/src/drivers/esp32/src/Kbuild b/src/drivers/esp32/src/Kbuild new file mode 100644 index 0000000000..377716b935 --- /dev/null +++ b/src/drivers/esp32/src/Kbuild @@ -0,0 +1,2 @@ +obj-y += esp_rom_bootloader.o +obj-y += esp_slip.o diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h index e161740699..51a86734ad 100644 --- a/src/drivers/interface/motors.h +++ b/src/drivers/interface/motors.h @@ -36,6 +36,7 @@ #include #include #include "config.h" +#include "autoconf.h" /* ST includes */ #include "stm32fxxx.h" @@ -62,9 +63,7 @@ // Not applied for brushless motor setup. #define ENABLE_THRUST_BAT_COMPENSATED -//#define ENABLE_ONESHOT125 - -#ifdef ENABLE_ONESHOT125 +#ifdef CONFIG_MOTORS_ESC_PROTOCOL_ONESHOT125 /** * *VARNING* Make sure the brushless driver is configured correctly as on the Crazyflie with normal * brushed motors connected they can turn on at full speed when it is powered on! @@ -80,7 +79,7 @@ #define MOTORS_BL_PWM_PERIOD MOTORS_BL_PWM_CNT_FOR_PERIOD #define MOTORS_BL_PWM_PRESCALE (uint16_t)(MOTORS_BL_PWM_PRESCALE_RAW - 1) #define MOTORS_BL_POLARITY TIM_OCPolarity_Low -#elif defined(ENABLE_ONESHOT42) +#elif defined(CONFIG_MOTORS_ESC_PROTOCOL_ONESHOT42) /** * *VARNING* Make sure the brushless driver is configured correctly as on the Crazyflie with normal * brushed motors connected they can turn on at full speed when it is powered on! @@ -98,7 +97,7 @@ #define MOTORS_BL_POLARITY TIM_OCPolarity_Low #else /** - * *VARNING* Make sure the brushless driver is configured correctly as on the Crazyflie with normal + * *WARNING* Make sure the brushless driver is configured correctly as on the Crazyflie with normal * brushed motors connected they can turn on at full speed when it is powered on! * * Generates a PWM wave (50 - 400 Hz update rate with 1-2 ms high pulse) using the timer. That way we can use the same @@ -231,6 +230,7 @@ extern const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS]; extern const MotorPerifDef* motorMapDefaltConBrushless[NBR_OF_MOTORS]; extern const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS]; extern const MotorPerifDef* motorMapBoltBrushless[NBR_OF_MOTORS]; +extern const MotorPerifDef* motorMapBolt11Brushless[NBR_OF_MOTORS]; /** * Test sound tones diff --git a/src/drivers/src/Kbuild b/src/drivers/src/Kbuild new file mode 100644 index 0000000000..18bc093636 --- /dev/null +++ b/src/drivers/src/Kbuild @@ -0,0 +1,27 @@ +obj-y += ak8963.o +obj-y += cppm.o +obj-y += eeprom.o +obj-y += exti.o +obj-y += fatfs_sd.o +obj-y += i2cdev.o +obj-y += i2c_drv.o +obj-y += led.o +obj-y += lh_bootloader.o +obj-y += lps25h.o +obj-y += maxsonar.o +obj-y += motors.o +obj-y += mpu6050.o +obj-y += mpu6500.o +obj-y += ms5611.o +obj-y += nvic.o +obj-y += pca9685.o +obj-y += piezo.o +obj-y += pmw3901.o +obj-y += swd.o +obj-y += uart1.o +obj-y += uart2.o +obj-y += uart_syslink.o +obj-y += vl53l0x.o +obj-y += vl53l1x.o +obj-y += watchdog.o +obj-y += ws2812_cf2.o diff --git a/src/drivers/src/Kconfig b/src/drivers/src/Kconfig new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/drivers/src/i2c_drv.c b/src/drivers/src/i2c_drv.c index 79bd7ed9ec..3d69c73244 100644 --- a/src/drivers/src/i2c_drv.c +++ b/src/drivers/src/i2c_drv.c @@ -49,6 +49,8 @@ #include "nvicconf.h" #include "sleepus.h" +#include "autoconf.h" + //DEBUG #ifdef I2CDRV_DEBUG_LOG_EVENTS #include "usec_time.h" @@ -169,7 +171,7 @@ static const I2cDef deckBusDef = .gpioAF = GPIO_AF_I2C1, .dmaPerif = RCC_AHB1Periph_DMA1, .dmaChannel = DMA_Channel_1, -#ifdef USDDECK_USE_ALT_PINS_AND_SPI +#ifdef CONFIG_DECK_USD_USE_ALT_PINS_AND_SPI .dmaRxStream = DMA1_Stream5, .dmaRxIRQ = DMA1_Stream5_IRQn, .dmaRxTCFlag = DMA_FLAG_TCIF5, @@ -645,7 +647,7 @@ void __attribute__((used)) I2C1_EV_IRQHandler(void) i2cdrvEventIsrHandler(&deckBus); } -#ifdef USDDECK_USE_ALT_PINS_AND_SPI +#ifdef CONFIG_DECK_USD_USE_ALT_PINS_AND_SPI void __attribute__((used)) DMA1_Stream5_IRQHandler(void) #else void __attribute__((used)) DMA1_Stream0_IRQHandler(void) diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 87122d270c..56c5631350 100644 --- a/src/drivers/src/motors.c +++ b/src/drivers/src/motors.c @@ -1,3 +1,4 @@ + /** * || ____ _ __ * +------+ / __ )(_) /_______________ _____ ___ @@ -53,7 +54,7 @@ void motorsPlayTone(uint16_t frequency, uint16_t duration_msec); void motorsPlayMelody(uint16_t *notes); void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio); -#include "motors_def_cf2.c" +#include "motors_def.c" const MotorPerifDef** motorMap; /* Current map configuration */ diff --git a/src/drivers/src/motors_def_cf2.c b/src/drivers/src/motors_def.c similarity index 81% rename from src/drivers/src/motors_def_cf2.c rename to src/drivers/src/motors_def.c index 73166e6655..6791038544 100644 --- a/src/drivers/src/motors_def_cf2.c +++ b/src/drivers/src/motors_def.c @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2011-2012 Bitcraze AB + * Copyright (C) 2022 Bitcraze AB * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -21,12 +21,11 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * motors.c - Motor driver + * motors_def.c - Mapping and configuration of motor outputs * - * This code mainly interfacing the PWM peripheral lib of ST. */ -// Connector M1, PA1, TIM2_CH2 -static const MotorPerifDef CONN_M1 = +// CF2.X connector M1, PA1, TIM2_CH2 +static const MotorPerifDef MOTORS_PA1_TIM2_CH2_BRUSHED = { .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -47,8 +46,8 @@ static const MotorPerifDef CONN_M1 = .preloadConfig = TIM_OC2PreloadConfig, }; -// Connector M2, PB11, TIM2_CH4 -static const MotorPerifDef CONN_M2 = +// CF2.X connector M2, PB11, TIM2_CH4 +static const MotorPerifDef MOTORS_PB11_TIM2_CH4_BRUSHED = { .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -69,8 +68,8 @@ static const MotorPerifDef CONN_M2 = .preloadConfig = TIM_OC4PreloadConfig, }; -// Connector M3, PA15, TIM2_CH1 -static const MotorPerifDef CONN_M3 = +// CF2.X connector M3, PA15, TIM2_CH1 +static const MotorPerifDef MOTORS_PA15_TIM2_CH1_BRUSHED = { .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -91,8 +90,8 @@ static const MotorPerifDef CONN_M3 = .preloadConfig = TIM_OC1PreloadConfig, }; -// Connector M4, PB9, TIM4_CH4 -static const MotorPerifDef CONN_M4 = +// CF2.X connector M4, PB9, TIM4_CH4 +static const MotorPerifDef MOTORS_PB9_TIM4_CH4_BRUSHED = { .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -113,8 +112,8 @@ static const MotorPerifDef CONN_M4 = .preloadConfig = TIM_OC4PreloadConfig, }; -// Connector M1, PA1, TIM2_CH2, Brushless config, inversed -static const MotorPerifDef CONN_M1_BL_INV = +// CF2.X connector M1, PA1, TIM2_CH2, Brushless config, inversed +static const MotorPerifDef MOTORS_PA1_TIM2_CH2_BRUSHLESS_INV_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -135,8 +134,8 @@ static const MotorPerifDef CONN_M1_BL_INV = .preloadConfig = TIM_OC2PreloadConfig, }; -// Connector M2, PB11, TIM2_CH4, Brushless config, inversed -static const MotorPerifDef CONN_M2_BL_INV = +// CF2.X connector M2, PB11, TIM2_CH4, Brushless config, inversed +static const MotorPerifDef MOTORS_PB11_TIM2_CH4_BRUSHLESS_INV_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -157,8 +156,8 @@ static const MotorPerifDef CONN_M2_BL_INV = .preloadConfig = TIM_OC4PreloadConfig, }; -// Connector M3, PA15, TIM2_CH1, Brushless config, inversed -static const MotorPerifDef CONN_M3_BL_INV = +// CF2.X connector M3, PA15, TIM2_CH1, Brushless config, inversed +static const MotorPerifDef MOTORS_PA15_TIM2_CH1_BRUSHLESS_INV_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -179,8 +178,8 @@ static const MotorPerifDef CONN_M3_BL_INV = .preloadConfig = TIM_OC1PreloadConfig, }; -// Connector M4, PB9, TIM4_CH4, Brushless config, inversed -static const MotorPerifDef CONN_M4_BL_INV = +// CF2.X connector M4, PB9, TIM4_CH4, Brushless config, inversed +static const MotorPerifDef MOTORS_PB9_TIM4_CH4_BRUSHLESS_INV_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -202,7 +201,7 @@ static const MotorPerifDef CONN_M4_BL_INV = }; // Bolt M1, PA1, TIM2_CH2, Brushless config -static const MotorPerifDef BOLT_M1_BL = +static const MotorPerifDef MOTORS_PA1_TIM2_CH2_BRUSHLESS_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -227,7 +226,7 @@ static const MotorPerifDef BOLT_M1_BL = }; // Bolt M2, PB11, TIM2_CH4, Brushless config -static const MotorPerifDef BOLT_M2_BL = +static const MotorPerifDef MOTORS_PB11_TIM2_CH4_BRUSHLESS_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -252,7 +251,7 @@ static const MotorPerifDef BOLT_M2_BL = }; // Bolt M3, PA15, TIM2_CH1, Brushless config -static const MotorPerifDef BOLT_M3_BL = +static const MotorPerifDef MOTORS_PA15_TIM2_CH1_BRUSHLESS_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -277,7 +276,7 @@ static const MotorPerifDef BOLT_M3_BL = }; // Bolt M4, PB9, TIM4_CH4, Brushless config -static const MotorPerifDef BOLT_M4_BL = +static const MotorPerifDef MOTORS_PB9_TIM4_CH4_BRUSHLESS_PP = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -301,8 +300,33 @@ static const MotorPerifDef BOLT_M4_BL = .preloadConfig = TIM_OC4PreloadConfig, }; +// Bolt 1.1 M4, PB10, TIM2_CH3, Brushless config +static const MotorPerifDef MOTORS_PB10_TIM2_CH3_BRUSHLESS_PP = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_10, + .gpioPinSource = GPIO_PinSource10, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM2, + .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOC, + .gpioPowerswitchPort = GPIOC, + .gpioPowerswitchPin = GPIO_Pin_15, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + // Deck TX2, PA2, TIM2_CH3 -static const MotorPerifDef DECK_TX2_TIM2 = +static const MotorPerifDef MOTORS_PA2_TIM2_CH3_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -324,7 +348,7 @@ static const MotorPerifDef DECK_TX2_TIM2 = }; // Deck TX2, PA2, TIM5_CH3 -static const MotorPerifDef DECK_TX2_TIM5 = +static const MotorPerifDef MOTORS_PA2_TIM5_CH3_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -346,7 +370,7 @@ static const MotorPerifDef DECK_TX2_TIM5 = }; // Deck RX2, PA3, TIM2_CH4 -static const MotorPerifDef DECK_RX2_TIM2 = +static const MotorPerifDef MOTORS_PA3_TIM2_CH4_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -368,7 +392,7 @@ static const MotorPerifDef DECK_RX2_TIM2 = }; // Deck RX2, PA3, TIM5_CH4 -static const MotorPerifDef DECK_RX2_TIM5 = +static const MotorPerifDef MOTORS_PA3_TIM5_CH4_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -390,7 +414,7 @@ static const MotorPerifDef DECK_RX2_TIM5 = }; // Deck IO1, PB8, TIM4_CH3 -static const MotorPerifDef DECK_IO1_TIM4 = +static const MotorPerifDef MOTORS_PB8_TIM4_CH3_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -412,7 +436,7 @@ static const MotorPerifDef DECK_IO1_TIM4 = }; // Deck IO2, PB5, TIM3_CH2 -static const MotorPerifDef DECK_IO2 = +static const MotorPerifDef MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -434,7 +458,7 @@ static const MotorPerifDef DECK_IO2 = }; // Deck IO3, PB4, TIM3_CH1 -static const MotorPerifDef DECK_IO3 = +static const MotorPerifDef MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -456,7 +480,7 @@ static const MotorPerifDef DECK_IO3 = }; // Deck SCK, PA5, TIM2_CH1 -static const MotorPerifDef DECK_SCK = +static const MotorPerifDef MOTORS_PA5_TIM2_CH1_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -478,7 +502,7 @@ static const MotorPerifDef DECK_SCK = }; // Deck MISO, PA6, TIM3_CH1 -static const MotorPerifDef DECK_MISO = +static const MotorPerifDef MOTORS_PA6_TIM3_CH1_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -500,7 +524,7 @@ static const MotorPerifDef DECK_MISO = }; // Deck MOSI, PA7, TIM14_CH1 -static const MotorPerifDef DECK_MOSI = +static const MotorPerifDef MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, @@ -523,14 +547,14 @@ static const MotorPerifDef DECK_MOSI = /** * Mapping for Tags that don't have motors. - * Actually same mapping as for CF2 but the pins are not connected. + * Actually same mapping as for CF2 but the pins are not physically connected. */ const MotorPerifDef* motorMapNoMotors[NBR_OF_MOTORS] = { - &CONN_M1, - &CONN_M2, - &CONN_M3, - &CONN_M4 + &MOTORS_PA1_TIM2_CH2_BRUSHED, + &MOTORS_PB11_TIM2_CH4_BRUSHED, + &MOTORS_PA15_TIM2_CH1_BRUSHED, + &MOTORS_PB9_TIM4_CH4_BRUSHED }; /** @@ -538,10 +562,10 @@ const MotorPerifDef* motorMapNoMotors[NBR_OF_MOTORS] = */ const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] = { - &CONN_M1, - &CONN_M2, - &CONN_M3, - &CONN_M4 + &MOTORS_PA1_TIM2_CH2_BRUSHED, + &MOTORS_PB11_TIM2_CH4_BRUSHED, + &MOTORS_PA15_TIM2_CH1_BRUSHED, + &MOTORS_PB9_TIM4_CH4_BRUSHED }; /** @@ -553,10 +577,10 @@ const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] = */ const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] = { - &DECK_TX2_TIM2, - &DECK_IO3, - &DECK_IO2, - &DECK_RX2_TIM2 + &MOTORS_PA2_TIM2_CH3_BRUSHLESS_OD, + &MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD, + &MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD, + &MOTORS_PA3_TIM2_CH4_BRUSHLESS_OD }; /** @@ -564,10 +588,10 @@ const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] = */ const MotorPerifDef* motorMapDefaltConBrushless[NBR_OF_MOTORS] = { - &CONN_M1_BL_INV, - &CONN_M2_BL_INV, - &CONN_M3_BL_INV, - &CONN_M4_BL_INV + &MOTORS_PA1_TIM2_CH2_BRUSHLESS_INV_PP, + &MOTORS_PB11_TIM2_CH4_BRUSHLESS_INV_PP, + &MOTORS_PA15_TIM2_CH1_BRUSHLESS_INV_PP, + &MOTORS_PB9_TIM4_CH4_BRUSHLESS_INV_PP }; /** @@ -575,9 +599,20 @@ const MotorPerifDef* motorMapDefaltConBrushless[NBR_OF_MOTORS] = */ const MotorPerifDef* motorMapBoltBrushless[NBR_OF_MOTORS] = { - &BOLT_M1_BL, - &BOLT_M2_BL, - &BOLT_M3_BL, - &BOLT_M4_BL + &MOTORS_PA1_TIM2_CH2_BRUSHLESS_PP, + &MOTORS_PB11_TIM2_CH4_BRUSHLESS_PP, + &MOTORS_PA15_TIM2_CH1_BRUSHLESS_PP, + &MOTORS_PB9_TIM4_CH4_BRUSHLESS_PP +}; + +/** + * Brushless motors mapped to the Bolt 1.1 PWM outputs. + */ +const MotorPerifDef* motorMapBolt11Brushless[NBR_OF_MOTORS] = +{ + &MOTORS_PA1_TIM2_CH2_BRUSHLESS_PP, + &MOTORS_PB11_TIM2_CH4_BRUSHLESS_PP, + &MOTORS_PA15_TIM2_CH1_BRUSHLESS_PP, + &MOTORS_PB10_TIM2_CH3_BRUSHLESS_PP }; diff --git a/src/drivers/src/ws2812_cf2.c b/src/drivers/src/ws2812_cf2.c index 34e5bd7b94..3320fb0372 100644 --- a/src/drivers/src/ws2812_cf2.c +++ b/src/drivers/src/ws2812_cf2.c @@ -35,6 +35,8 @@ #include "FreeRTOS.h" #include "semphr.h" +#include "autoconf.h" + //#define TIM1_CCR1_Address 0x40012C34 // physical memory address of Timer 3 CCR1 register static TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; @@ -215,6 +217,8 @@ void ws2812DmaIsr(void) DMA_Cmd(DMA1_Stream5, DISABLE); } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_HTIF5)) { DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_HTIF5); @@ -226,6 +230,7 @@ void ws2812DmaIsr(void) DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5); buffer = led_dma.end; } +#pragma GCC diagnostic pop for(i=0; (i bat671723HS25C[9]) + if (voltage > LiPoTypicalChargeCurve[9]) { return 9; } - while (voltage > bat671723HS25C[charge]) + while (voltage > LiPoTypicalChargeCurve[charge]) { charge++; } @@ -375,11 +387,11 @@ void pmTask(void *param) extBatteryCurrent = pmMeasureExtBatteryCurrent(); batteryLevel = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) * 10; - if (pmGetBatteryVoltage() > PM_BAT_LOW_VOLTAGE) + if (pmGetBatteryVoltage() > batteryLowVoltage) { batteryLowTimeStamp = tickCount; } - if (pmGetBatteryVoltage() > PM_BAT_CRITICAL_LOW_VOLTAGE) + if (pmGetBatteryVoltage() > batteryCriticalLowVoltage) { batteryCriticalLowTimeStamp = tickCount; } @@ -504,3 +516,19 @@ LOG_ADD_CORE(LOG_UINT8, batteryLevel, &batteryLevel) LOG_ADD(LOG_FLOAT, temp, &temp) #endif LOG_GROUP_STOP(pm) + +/** + * Power management parameters. + */ +PARAM_GROUP_START(pm) +/** + * @brief At what voltage power management will indicate low battery. + */ +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, lowVoltage, &batteryLowVoltage) +/** + * @brief At what voltage power management will indicate critical low battery. + */ +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, criticalLowVoltage, &batteryCriticalLowVoltage) + +PARAM_GROUP_STOP(pm) + diff --git a/src/hal/src/sensors.c b/src/hal/src/sensors.c index 62f07ac378..3ee74f4057 100644 --- a/src/hal/src/sensors.c +++ b/src/hal/src/sensors.c @@ -31,19 +31,21 @@ #include "platform.h" #include "debug.h" +#include "autoconf.h" + // https://gcc.gnu.org/onlinedocs/cpp/Stringizing.html #define xstr(s) str(s) #define str(s) #s -#if defined(SENSOR_INCLUDED_BMI088_BMP388) || defined(SENSOR_INCLUDED_BMI088_SPI_BMP388) +#if defined(CONFIG_SENSORS_BMI088_BMP388) || defined(CONFIG_SENSORS_BMI088_SPI) #include "sensors_bmi088_bmp388.h" #endif -#ifdef SENSOR_INCLUDED_MPU9250_LPS25H +#ifdef CONFIG_SENSORS_MPU9250_LPS25H #include "sensors_mpu9250_lps25h.h" #endif -#ifdef SENSOR_INCLUDED_BOSCH +#ifdef CONFIG_SENSORS_BOSCH #include "sensors_bosch.h" #endif @@ -70,7 +72,7 @@ static void nullFunction(void) {} #pragma GCC diagnostic pop static const sensorsImplementation_t sensorImplementations[SensorImplementation_COUNT] = { -#ifdef SENSOR_INCLUDED_BMI088_BMP388 +#ifdef CONFIG_SENSORS_BMI088_BMP388 { .implements = SensorImplementation_bmi088_bmp388, .init = sensorsBmi088Bmp388Init_I2C, @@ -87,7 +89,7 @@ static const sensorsImplementation_t sensorImplementations[SensorImplementation_ .dataAvailableCallback = sensorsBmi088Bmp388DataAvailableCallback, }, #endif -#ifdef SENSOR_INCLUDED_BMI088_SPI_BMP388 +#ifdef CONFIG_SENSORS_BMI088_SPI { .implements = SensorImplementation_bmi088_spi_bmp388, .init = sensorsBmi088Bmp388Init_SPI, @@ -104,7 +106,7 @@ static const sensorsImplementation_t sensorImplementations[SensorImplementation_ .dataAvailableCallback = sensorsBmi088Bmp388DataAvailableCallback, }, #endif -#ifdef SENSOR_INCLUDED_MPU9250_LPS25H +#ifdef CONFIG_SENSORS_MPU9250_LPS25H { .implements = SensorImplementation_mpu9250_lps25h, .init = sensorsMpu9250Lps25hInit, @@ -121,7 +123,7 @@ static const sensorsImplementation_t sensorImplementations[SensorImplementation_ .dataAvailableCallback = nullFunction, }, #endif -#ifdef SENSOR_INCLUDED_BOSCH +#ifdef CONFIG_SENSORS_BOSCH { .implements = SensorImplementation_bosch, .init = sensorsBoschInit, diff --git a/src/hal/src/sensors_bmi088_bmp388.c b/src/hal/src/sensors_bmi088_bmp388.c index 1cc6994335..8dd94242b0 100644 --- a/src/hal/src/sensors_bmi088_bmp388.c +++ b/src/hal/src/sensors_bmi088_bmp388.c @@ -514,7 +514,7 @@ static void sensorsDeviceInit(void) } else { -#ifndef SENSORS_IGNORE_BAROMETER_FAIL +#ifndef CONFIG_SENSORS_IGNORE_BAROMETER_FAIL DEBUG_PRINT("BMP388 I2C connection [FAIL]\n"); isInit = false; return; diff --git a/src/hal/src/sensors_mpu9250_lps25h.c b/src/hal/src/sensors_mpu9250_lps25h.c index 84a0c17862..35af9c449e 100644 --- a/src/hal/src/sensors_mpu9250_lps25h.c +++ b/src/hal/src/sensors_mpu9250_lps25h.c @@ -52,12 +52,6 @@ #include "static_mem.h" #include "estimator.h" -/** - * Enable 250Hz digital LPF mode. However does not work with - * multiple slave reading through MPU9250 (MAG and BARO), only single for some reason. - */ -//#define SENSORS_MPU6500_DLPF_256HZ - #define SENSORS_ENABLE_PRESSURE_LPS25H //#define GYRO_ADD_RAW_AND_VARIANCE_LOG_VALUES @@ -395,13 +389,6 @@ static void sensorsDeviceInit(void) // Set accelerometer digital low-pass bandwidth mpu6500SetAccelDLPF(MPU6500_ACCEL_DLPF_BW_41); -#if SENSORS_MPU6500_DLPF_256HZ - // 256Hz digital low-pass filter only works with little vibrations - // Set output rate (15): 8000 / (1 + 7) = 1000Hz - mpu6500SetRate(7); - // Set digital low-pass bandwidth - mpu6500SetDLPFMode(MPU6500_DLPF_BW_256); -#else // To low DLPF bandwidth might cause instability and decrease agility // but it works well for handling vibrations and unbalanced propellers // Set output rate (1): 1000 / (1 + 0) = 1000Hz @@ -414,7 +401,6 @@ static void sensorsDeviceInit(void) lpf2pInit(&gyroLpf[i], 1000, GYRO_LPF_CUTOFF_FREQ); lpf2pInit(&accLpf[i], 1000, ACCEL_LPF_CUTOFF_FREQ); } -#endif #ifdef SENSORS_ENABLE_MAG_AK8963 @@ -456,14 +442,7 @@ static void sensorsDeviceInit(void) static void sensorsSetupSlaveRead(void) { // Now begin to set up the slaves -#ifdef SENSORS_MPU6500_DLPF_256HZ - // As noted in registersheet 4.4: "Data should be sampled at or above sample rate; - // SMPLRT_DIV is only used for 1kHz internal sampling." Slowest update rate is then 500Hz. - mpu6500SetSlave4MasterDelay(15); // read slaves at 500Hz = (8000Hz / (1 + 15)) -#else mpu6500SetSlave4MasterDelay(9); // read slaves at 100Hz = (500Hz / (1 + 4)) -#endif - mpu6500SetI2CBypassEnabled(false); mpu6500SetWaitForExternalSensorEnabled(true); // the slave data isn't so important for the state estimation mpu6500SetInterruptMode(0); // active high diff --git a/src/hal/src/usb.c b/src/hal/src/usb.c index ec2ee5e956..a65d23c63f 100644 --- a/src/hal/src/usb.c +++ b/src/hal/src/usb.c @@ -48,6 +48,8 @@ #include "crtp.h" #include "static_mem.h" +#include "bootloader.h" + NO_DMA_CCM_SAFE_ZERO_INIT __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END ; @@ -177,6 +179,10 @@ static uint8_t usbd_cf_Setup(void *pdev , USB_SETUP_REQ *req) USB_RX_TX_PACKET_SIZE); rxStopped = false; } + } else if(command == 0x02){ + //restart system and transition to DFU bootloader mode + //enter bootloader specific to STM32f4xx + enter_bootloader(0, 0x00000000); } else { crtpSetLink(radiolinkGetLink()); } diff --git a/src/init/Kbuild b/src/init/Kbuild new file mode 100644 index 0000000000..5752cc983c --- /dev/null +++ b/src/init/Kbuild @@ -0,0 +1,2 @@ +obj-y += main.o +obj-y += startup_stm32f40xx.o diff --git a/src/init/main.c b/src/init/main.c index 83c2e3e264..1faf554a5f 100644 --- a/src/init/main.c +++ b/src/init/main.c @@ -42,8 +42,12 @@ /* ST includes */ #include "stm32fxxx.h" +#include "bootloader.h" + int main() { + check_enter_bootloader(); + //Initialize the platform. int err = platformInit(); if (err != 0) { diff --git a/src/init/startup_stm32f40xx.s b/src/init/startup_stm32f40xx.S similarity index 100% rename from src/init/startup_stm32f40xx.s rename to src/init/startup_stm32f40xx.S diff --git a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_cl.s b/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_cl.s deleted file mode 100644 index 0cb477c482..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_cl.s +++ /dev/null @@ -1,364 +0,0 @@ -;******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_cl.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x Connectivity line devices vector table for RVMDK -;* toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == Reset_Handler -;* - Set the vector table entries with the exceptions ISR address -;* - Branches to __main in the C library (which eventually -;* calls main()). -;* After Reset the CortexM3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;* <<< Use Configuration Wizard in Context Menu >>> -;******************************************************************************* -; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;******************************************************************************* - -; Amount of memory (in bytes) allocated for Stack -; Tailor this value to your application needs -; Stack Configuration -; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Stack_Size EQU 0x00000400 - - AREA STACK, NOINIT, READWRITE, ALIGN=3 -Stack_Mem SPACE Stack_Size -__initial_sp - - -; Heap Configuration -; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Heap_Size EQU 0x00000200 - - AREA HEAP, NOINIT, READWRITE, ALIGN=3 -__heap_base -Heap_Mem SPACE Heap_Size -__heap_limit - - PRESERVE8 - THUMB - - -; Vector Table Mapped to Address 0 at Reset - AREA RESET, DATA, READONLY - EXPORT __Vectors - EXPORT __Vectors_End - EXPORT __Vectors_Size - -__Vectors DCD __initial_sp ; Top of Stack - DCD Reset_Handler ; Reset Handler - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1 and ADC2 - DCD CAN1_TX_IRQHandler ; CAN1 TX - DCD CAN1_RX0_IRQHandler ; CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C1 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD USART3_IRQHandler ; USART3 - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC alarm through EXTI line - DCD OTG_FS_WKUP_IRQHandler ; USB OTG FS Wakeup through EXTI line - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD TIM5_IRQHandler ; TIM5 - DCD SPI3_IRQHandler ; SPI3 - DCD UART4_IRQHandler ; UART4 - DCD UART5_IRQHandler ; UART5 - DCD TIM6_IRQHandler ; TIM6 - DCD TIM7_IRQHandler ; TIM7 - DCD DMA2_Channel1_IRQHandler ; DMA2 Channel1 - DCD DMA2_Channel2_IRQHandler ; DMA2 Channel2 - DCD DMA2_Channel3_IRQHandler ; DMA2 Channel3 - DCD DMA2_Channel4_IRQHandler ; DMA2 Channel4 - DCD DMA2_Channel5_IRQHandler ; DMA2 Channel5 - DCD ETH_IRQHandler ; Ethernet - DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line - DCD CAN2_TX_IRQHandler ; CAN2 TX - DCD CAN2_RX0_IRQHandler ; CAN2 RX0 - DCD CAN2_RX1_IRQHandler ; CAN2 RX1 - DCD CAN2_SCE_IRQHandler ; CAN2 SCE - DCD OTG_FS_IRQHandler ; USB OTG FS -__Vectors_End - -__Vectors_Size EQU __Vectors_End - __Vectors - - AREA |.text|, CODE, READONLY - -; Reset handler routine -Reset_Handler PROC - EXPORT Reset_Handler [WEAK] - IMPORT __main - LDR R0, =__main - BX R0 - ENDP - -; Dummy Exception Handlers (infinite loops which can be modified) - -NMI_Handler PROC - EXPORT NMI_Handler [WEAK] - B . - ENDP -HardFault_Handler\ - PROC - EXPORT HardFault_Handler [WEAK] - B . - ENDP -MemManage_Handler\ - PROC - EXPORT MemManage_Handler [WEAK] - B . - ENDP -BusFault_Handler\ - PROC - EXPORT BusFault_Handler [WEAK] - B . - ENDP -UsageFault_Handler\ - PROC - EXPORT UsageFault_Handler [WEAK] - B . - ENDP -SVC_Handler PROC - EXPORT SVC_Handler [WEAK] - B . - ENDP -DebugMon_Handler\ - PROC - EXPORT DebugMon_Handler [WEAK] - B . - ENDP -PendSV_Handler PROC - EXPORT PendSV_Handler [WEAK] - B . - ENDP -SysTick_Handler PROC - EXPORT SysTick_Handler [WEAK] - B . - ENDP - -Default_Handler PROC - - EXPORT WWDG_IRQHandler [WEAK] - EXPORT PVD_IRQHandler [WEAK] - EXPORT TAMPER_IRQHandler [WEAK] - EXPORT RTC_IRQHandler [WEAK] - EXPORT FLASH_IRQHandler [WEAK] - EXPORT RCC_IRQHandler [WEAK] - EXPORT EXTI0_IRQHandler [WEAK] - EXPORT EXTI1_IRQHandler [WEAK] - EXPORT EXTI2_IRQHandler [WEAK] - EXPORT EXTI3_IRQHandler [WEAK] - EXPORT EXTI4_IRQHandler [WEAK] - EXPORT DMA1_Channel1_IRQHandler [WEAK] - EXPORT DMA1_Channel2_IRQHandler [WEAK] - EXPORT DMA1_Channel3_IRQHandler [WEAK] - EXPORT DMA1_Channel4_IRQHandler [WEAK] - EXPORT DMA1_Channel5_IRQHandler [WEAK] - EXPORT DMA1_Channel6_IRQHandler [WEAK] - EXPORT DMA1_Channel7_IRQHandler [WEAK] - EXPORT ADC1_2_IRQHandler [WEAK] - EXPORT CAN1_TX_IRQHandler [WEAK] - EXPORT CAN1_RX0_IRQHandler [WEAK] - EXPORT CAN1_RX1_IRQHandler [WEAK] - EXPORT CAN1_SCE_IRQHandler [WEAK] - EXPORT EXTI9_5_IRQHandler [WEAK] - EXPORT TIM1_BRK_IRQHandler [WEAK] - EXPORT TIM1_UP_IRQHandler [WEAK] - EXPORT TIM1_TRG_COM_IRQHandler [WEAK] - EXPORT TIM1_CC_IRQHandler [WEAK] - EXPORT TIM2_IRQHandler [WEAK] - EXPORT TIM3_IRQHandler [WEAK] - EXPORT TIM4_IRQHandler [WEAK] - EXPORT I2C1_EV_IRQHandler [WEAK] - EXPORT I2C1_ER_IRQHandler [WEAK] - EXPORT I2C2_EV_IRQHandler [WEAK] - EXPORT I2C2_ER_IRQHandler [WEAK] - EXPORT SPI1_IRQHandler [WEAK] - EXPORT SPI2_IRQHandler [WEAK] - EXPORT USART1_IRQHandler [WEAK] - EXPORT USART2_IRQHandler [WEAK] - EXPORT USART3_IRQHandler [WEAK] - EXPORT EXTI15_10_IRQHandler [WEAK] - EXPORT RTCAlarm_IRQHandler [WEAK] - EXPORT OTG_FS_WKUP_IRQHandler [WEAK] - EXPORT TIM5_IRQHandler [WEAK] - EXPORT SPI3_IRQHandler [WEAK] - EXPORT UART4_IRQHandler [WEAK] - EXPORT UART5_IRQHandler [WEAK] - EXPORT TIM6_IRQHandler [WEAK] - EXPORT TIM7_IRQHandler [WEAK] - EXPORT DMA2_Channel1_IRQHandler [WEAK] - EXPORT DMA2_Channel2_IRQHandler [WEAK] - EXPORT DMA2_Channel3_IRQHandler [WEAK] - EXPORT DMA2_Channel4_IRQHandler [WEAK] - EXPORT DMA2_Channel5_IRQHandler [WEAK] - EXPORT ETH_IRQHandler [WEAK] - EXPORT ETH_WKUP_IRQHandler [WEAK] - EXPORT CAN2_TX_IRQHandler [WEAK] - EXPORT CAN2_RX0_IRQHandler [WEAK] - EXPORT CAN2_RX1_IRQHandler [WEAK] - EXPORT CAN2_SCE_IRQHandler [WEAK] - EXPORT OTG_FS_IRQHandler [WEAK] - -WWDG_IRQHandler -PVD_IRQHandler -TAMPER_IRQHandler -RTC_IRQHandler -FLASH_IRQHandler -RCC_IRQHandler -EXTI0_IRQHandler -EXTI1_IRQHandler -EXTI2_IRQHandler -EXTI3_IRQHandler -EXTI4_IRQHandler -DMA1_Channel1_IRQHandler -DMA1_Channel2_IRQHandler -DMA1_Channel3_IRQHandler -DMA1_Channel4_IRQHandler -DMA1_Channel5_IRQHandler -DMA1_Channel6_IRQHandler -DMA1_Channel7_IRQHandler -ADC1_2_IRQHandler -CAN1_TX_IRQHandler -CAN1_RX0_IRQHandler -CAN1_RX1_IRQHandler -CAN1_SCE_IRQHandler -EXTI9_5_IRQHandler -TIM1_BRK_IRQHandler -TIM1_UP_IRQHandler -TIM1_TRG_COM_IRQHandler -TIM1_CC_IRQHandler -TIM2_IRQHandler -TIM3_IRQHandler -TIM4_IRQHandler -I2C1_EV_IRQHandler -I2C1_ER_IRQHandler -I2C2_EV_IRQHandler -I2C2_ER_IRQHandler -SPI1_IRQHandler -SPI2_IRQHandler -USART1_IRQHandler -USART2_IRQHandler -USART3_IRQHandler -EXTI15_10_IRQHandler -RTCAlarm_IRQHandler -OTG_FS_WKUP_IRQHandler -TIM5_IRQHandler -SPI3_IRQHandler -UART4_IRQHandler -UART5_IRQHandler -TIM6_IRQHandler -TIM7_IRQHandler -DMA2_Channel1_IRQHandler -DMA2_Channel2_IRQHandler -DMA2_Channel3_IRQHandler -DMA2_Channel4_IRQHandler -DMA2_Channel5_IRQHandler -ETH_IRQHandler -ETH_WKUP_IRQHandler -CAN2_TX_IRQHandler -CAN2_RX0_IRQHandler -CAN2_RX1_IRQHandler -CAN2_SCE_IRQHandler -OTG_FS_IRQHandler - - B . - - ENDP - - ALIGN - -;******************************************************************************* -; User Stack and Heap initialization -;******************************************************************************* - IF :DEF:__MICROLIB - - EXPORT __initial_sp - EXPORT __heap_base - EXPORT __heap_limit - - ELSE - - IMPORT __use_two_region_memory - EXPORT __user_initial_stackheap - -__user_initial_stackheap - - LDR R0, = Heap_Mem - LDR R1, =(Stack_Mem + Stack_Size) - LDR R2, = (Heap_Mem + Heap_Size) - LDR R3, = Stack_Mem - BX LR - - ALIGN - - ENDIF - - END - -;******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE***** diff --git a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_hd.s b/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_hd.s deleted file mode 100644 index 16b1dd4784..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_hd.s +++ /dev/null @@ -1,370 +0,0 @@ -;******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_hd.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x High Density Devices vector table for RVMDK -;* toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == Reset_Handler -;* - Set the vector table entries with the exceptions ISR address -;* - Configure external SRAM mounted on STM3210E-EVAL board -;* to be used as data memory (optional, to be enabled by user) -;* - Branches to __main in the C library (which eventually -;* calls main()). -;* After Reset the CortexM3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;* <<< Use Configuration Wizard in Context Menu >>> -;******************************************************************************* -; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;******************************************************************************* - -; Amount of memory (in bytes) allocated for Stack -; Tailor this value to your application needs -; Stack Configuration -; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Stack_Size EQU 0x00000400 - - AREA STACK, NOINIT, READWRITE, ALIGN=3 -Stack_Mem SPACE Stack_Size -__initial_sp - -__initial_spTop EQU 0x20000400 ; stack used for SystemInit_ExtMemCtl - ; always internal RAM used - -; Heap Configuration -; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Heap_Size EQU 0x00000200 - - AREA HEAP, NOINIT, READWRITE, ALIGN=3 -__heap_base -Heap_Mem SPACE Heap_Size -__heap_limit - - PRESERVE8 - THUMB - - -; Vector Table Mapped to Address 0 at Reset - AREA RESET, DATA, READONLY - EXPORT __Vectors - EXPORT __Vectors_End - EXPORT __Vectors_Size - -__Vectors DCD __initial_spTop ; Top of Stack - DCD Reset_Handler ; Reset Handler - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1 & ADC2 - DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX - DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C2 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD USART3_IRQHandler ; USART3 - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line - DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend - DCD TIM8_BRK_IRQHandler ; TIM8 Break - DCD TIM8_UP_IRQHandler ; TIM8 Update - DCD TIM8_TRG_COM_IRQHandler ; TIM8 Trigger and Commutation - DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare - DCD ADC3_IRQHandler ; ADC3 - DCD FSMC_IRQHandler ; FSMC - DCD SDIO_IRQHandler ; SDIO - DCD TIM5_IRQHandler ; TIM5 - DCD SPI3_IRQHandler ; SPI3 - DCD UART4_IRQHandler ; UART4 - DCD UART5_IRQHandler ; UART5 - DCD TIM6_IRQHandler ; TIM6 - DCD TIM7_IRQHandler ; TIM7 - DCD DMA2_Channel1_IRQHandler ; DMA2 Channel1 - DCD DMA2_Channel2_IRQHandler ; DMA2 Channel2 - DCD DMA2_Channel3_IRQHandler ; DMA2 Channel3 - DCD DMA2_Channel4_5_IRQHandler ; DMA2 Channel4 & Channel5 -__Vectors_End - -__Vectors_Size EQU __Vectors_End - __Vectors - - AREA |.text|, CODE, READONLY - -; Dummy SystemInit_ExtMemCtl function -SystemInit_ExtMemCtl PROC - EXPORT SystemInit_ExtMemCtl [WEAK] - BX LR - ENDP - -; Reset handler routine -Reset_Handler PROC - EXPORT Reset_Handler [WEAK] - IMPORT __main - - LDR R0, = SystemInit_ExtMemCtl ; initialize external memory controller - BLX R0 - - LDR R1, = __initial_sp ; restore original stack pointer - MSR MSP, R1 - - LDR R0, =__main - BX R0 - ENDP - -; Dummy Exception Handlers (infinite loops which can be modified) - -NMI_Handler PROC - EXPORT NMI_Handler [WEAK] - B . - ENDP -HardFault_Handler\ - PROC - EXPORT HardFault_Handler [WEAK] - B . - ENDP -MemManage_Handler\ - PROC - EXPORT MemManage_Handler [WEAK] - B . - ENDP -BusFault_Handler\ - PROC - EXPORT BusFault_Handler [WEAK] - B . - ENDP -UsageFault_Handler\ - PROC - EXPORT UsageFault_Handler [WEAK] - B . - ENDP -SVC_Handler PROC - EXPORT SVC_Handler [WEAK] - B . - ENDP -DebugMon_Handler\ - PROC - EXPORT DebugMon_Handler [WEAK] - B . - ENDP -PendSV_Handler PROC - EXPORT PendSV_Handler [WEAK] - B . - ENDP -SysTick_Handler PROC - EXPORT SysTick_Handler [WEAK] - B . - ENDP - -Default_Handler PROC - - EXPORT WWDG_IRQHandler [WEAK] - EXPORT PVD_IRQHandler [WEAK] - EXPORT TAMPER_IRQHandler [WEAK] - EXPORT RTC_IRQHandler [WEAK] - EXPORT FLASH_IRQHandler [WEAK] - EXPORT RCC_IRQHandler [WEAK] - EXPORT EXTI0_IRQHandler [WEAK] - EXPORT EXTI1_IRQHandler [WEAK] - EXPORT EXTI2_IRQHandler [WEAK] - EXPORT EXTI3_IRQHandler [WEAK] - EXPORT EXTI4_IRQHandler [WEAK] - EXPORT DMA1_Channel1_IRQHandler [WEAK] - EXPORT DMA1_Channel2_IRQHandler [WEAK] - EXPORT DMA1_Channel3_IRQHandler [WEAK] - EXPORT DMA1_Channel4_IRQHandler [WEAK] - EXPORT DMA1_Channel5_IRQHandler [WEAK] - EXPORT DMA1_Channel6_IRQHandler [WEAK] - EXPORT DMA1_Channel7_IRQHandler [WEAK] - EXPORT ADC1_2_IRQHandler [WEAK] - EXPORT USB_HP_CAN1_TX_IRQHandler [WEAK] - EXPORT USB_LP_CAN1_RX0_IRQHandler [WEAK] - EXPORT CAN1_RX1_IRQHandler [WEAK] - EXPORT CAN1_SCE_IRQHandler [WEAK] - EXPORT EXTI9_5_IRQHandler [WEAK] - EXPORT TIM1_BRK_IRQHandler [WEAK] - EXPORT TIM1_UP_IRQHandler [WEAK] - EXPORT TIM1_TRG_COM_IRQHandler [WEAK] - EXPORT TIM1_CC_IRQHandler [WEAK] - EXPORT TIM2_IRQHandler [WEAK] - EXPORT TIM3_IRQHandler [WEAK] - EXPORT TIM4_IRQHandler [WEAK] - EXPORT I2C1_EV_IRQHandler [WEAK] - EXPORT I2C1_ER_IRQHandler [WEAK] - EXPORT I2C2_EV_IRQHandler [WEAK] - EXPORT I2C2_ER_IRQHandler [WEAK] - EXPORT SPI1_IRQHandler [WEAK] - EXPORT SPI2_IRQHandler [WEAK] - EXPORT USART1_IRQHandler [WEAK] - EXPORT USART2_IRQHandler [WEAK] - EXPORT USART3_IRQHandler [WEAK] - EXPORT EXTI15_10_IRQHandler [WEAK] - EXPORT RTCAlarm_IRQHandler [WEAK] - EXPORT USBWakeUp_IRQHandler [WEAK] - EXPORT TIM8_BRK_IRQHandler [WEAK] - EXPORT TIM8_UP_IRQHandler [WEAK] - EXPORT TIM8_TRG_COM_IRQHandler [WEAK] - EXPORT TIM8_CC_IRQHandler [WEAK] - EXPORT ADC3_IRQHandler [WEAK] - EXPORT FSMC_IRQHandler [WEAK] - EXPORT SDIO_IRQHandler [WEAK] - EXPORT TIM5_IRQHandler [WEAK] - EXPORT SPI3_IRQHandler [WEAK] - EXPORT UART4_IRQHandler [WEAK] - EXPORT UART5_IRQHandler [WEAK] - EXPORT TIM6_IRQHandler [WEAK] - EXPORT TIM7_IRQHandler [WEAK] - EXPORT DMA2_Channel1_IRQHandler [WEAK] - EXPORT DMA2_Channel2_IRQHandler [WEAK] - EXPORT DMA2_Channel3_IRQHandler [WEAK] - EXPORT DMA2_Channel4_5_IRQHandler [WEAK] - -WWDG_IRQHandler -PVD_IRQHandler -TAMPER_IRQHandler -RTC_IRQHandler -FLASH_IRQHandler -RCC_IRQHandler -EXTI0_IRQHandler -EXTI1_IRQHandler -EXTI2_IRQHandler -EXTI3_IRQHandler -EXTI4_IRQHandler -DMA1_Channel1_IRQHandler -DMA1_Channel2_IRQHandler -DMA1_Channel3_IRQHandler -DMA1_Channel4_IRQHandler -DMA1_Channel5_IRQHandler -DMA1_Channel6_IRQHandler -DMA1_Channel7_IRQHandler -ADC1_2_IRQHandler -USB_HP_CAN1_TX_IRQHandler -USB_LP_CAN1_RX0_IRQHandler -CAN1_RX1_IRQHandler -CAN1_SCE_IRQHandler -EXTI9_5_IRQHandler -TIM1_BRK_IRQHandler -TIM1_UP_IRQHandler -TIM1_TRG_COM_IRQHandler -TIM1_CC_IRQHandler -TIM2_IRQHandler -TIM3_IRQHandler -TIM4_IRQHandler -I2C1_EV_IRQHandler -I2C1_ER_IRQHandler -I2C2_EV_IRQHandler -I2C2_ER_IRQHandler -SPI1_IRQHandler -SPI2_IRQHandler -USART1_IRQHandler -USART2_IRQHandler -USART3_IRQHandler -EXTI15_10_IRQHandler -RTCAlarm_IRQHandler -USBWakeUp_IRQHandler -TIM8_BRK_IRQHandler -TIM8_UP_IRQHandler -TIM8_TRG_COM_IRQHandler -TIM8_CC_IRQHandler -ADC3_IRQHandler -FSMC_IRQHandler -SDIO_IRQHandler -TIM5_IRQHandler -SPI3_IRQHandler -UART4_IRQHandler -UART5_IRQHandler -TIM6_IRQHandler -TIM7_IRQHandler -DMA2_Channel1_IRQHandler -DMA2_Channel2_IRQHandler -DMA2_Channel3_IRQHandler -DMA2_Channel4_5_IRQHandler - B . - - ENDP - - ALIGN - -;******************************************************************************* -; User Stack and Heap initialization -;******************************************************************************* - IF :DEF:__MICROLIB - - EXPORT __initial_sp - EXPORT __heap_base - EXPORT __heap_limit - - ELSE - - IMPORT __use_two_region_memory - EXPORT __user_initial_stackheap - -__user_initial_stackheap - - LDR R0, = Heap_Mem - LDR R1, =(Stack_Mem + Stack_Size) - LDR R2, = (Heap_Mem + Heap_Size) - LDR R3, = Stack_Mem - BX LR - - ALIGN - - ENDIF - - END - -;******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE***** diff --git a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_ld.s b/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_ld.s deleted file mode 100644 index 7742056442..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_ld.s +++ /dev/null @@ -1,293 +0,0 @@ -;******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_ld.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x Low Density Devices vector table for RVMDK -;* toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == Reset_Handler -;* - Set the vector table entries with the exceptions ISR address -;* - Branches to __main in the C library (which eventually -;* calls main()). -;* After Reset the CortexM3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;* <<< Use Configuration Wizard in Context Menu >>> -;******************************************************************************* -; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;******************************************************************************* - -; Amount of memory (in bytes) allocated for Stack -; Tailor this value to your application needs -; Stack Configuration -; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Stack_Size EQU 0x00000400 - - AREA STACK, NOINIT, READWRITE, ALIGN=3 -Stack_Mem SPACE Stack_Size -__initial_sp - - -; Heap Configuration -; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Heap_Size EQU 0x00000200 - - AREA HEAP, NOINIT, READWRITE, ALIGN=3 -__heap_base -Heap_Mem SPACE Heap_Size -__heap_limit - - PRESERVE8 - THUMB - - -; Vector Table Mapped to Address 0 at Reset - AREA RESET, DATA, READONLY - EXPORT __Vectors - EXPORT __Vectors_End - EXPORT __Vectors_Size - -__Vectors DCD __initial_sp ; Top of Stack - DCD Reset_Handler ; Reset Handler - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1_2 - DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX - DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD 0 ; Reserved - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SPI1_IRQHandler ; SPI1 - DCD 0 ; Reserved - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD 0 ; Reserved - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line - DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend -__Vectors_End - -__Vectors_Size EQU __Vectors_End - __Vectors - - AREA |.text|, CODE, READONLY - -; Reset handler routine -Reset_Handler PROC - EXPORT Reset_Handler [WEAK] - IMPORT __main - LDR R0, =__main - BX R0 - ENDP - -; Dummy Exception Handlers (infinite loops which can be modified) - -NMI_Handler PROC - EXPORT NMI_Handler [WEAK] - B . - ENDP -HardFault_Handler\ - PROC - EXPORT HardFault_Handler [WEAK] - B . - ENDP -MemManage_Handler\ - PROC - EXPORT MemManage_Handler [WEAK] - B . - ENDP -BusFault_Handler\ - PROC - EXPORT BusFault_Handler [WEAK] - B . - ENDP -UsageFault_Handler\ - PROC - EXPORT UsageFault_Handler [WEAK] - B . - ENDP -SVC_Handler PROC - EXPORT SVC_Handler [WEAK] - B . - ENDP -DebugMon_Handler\ - PROC - EXPORT DebugMon_Handler [WEAK] - B . - ENDP -PendSV_Handler PROC - EXPORT PendSV_Handler [WEAK] - B . - ENDP -SysTick_Handler PROC - EXPORT SysTick_Handler [WEAK] - B . - ENDP - -Default_Handler PROC - - EXPORT WWDG_IRQHandler [WEAK] - EXPORT PVD_IRQHandler [WEAK] - EXPORT TAMPER_IRQHandler [WEAK] - EXPORT RTC_IRQHandler [WEAK] - EXPORT FLASH_IRQHandler [WEAK] - EXPORT RCC_IRQHandler [WEAK] - EXPORT EXTI0_IRQHandler [WEAK] - EXPORT EXTI1_IRQHandler [WEAK] - EXPORT EXTI2_IRQHandler [WEAK] - EXPORT EXTI3_IRQHandler [WEAK] - EXPORT EXTI4_IRQHandler [WEAK] - EXPORT DMA1_Channel1_IRQHandler [WEAK] - EXPORT DMA1_Channel2_IRQHandler [WEAK] - EXPORT DMA1_Channel3_IRQHandler [WEAK] - EXPORT DMA1_Channel4_IRQHandler [WEAK] - EXPORT DMA1_Channel5_IRQHandler [WEAK] - EXPORT DMA1_Channel6_IRQHandler [WEAK] - EXPORT DMA1_Channel7_IRQHandler [WEAK] - EXPORT ADC1_2_IRQHandler [WEAK] - EXPORT USB_HP_CAN1_TX_IRQHandler [WEAK] - EXPORT USB_LP_CAN1_RX0_IRQHandler [WEAK] - EXPORT CAN1_RX1_IRQHandler [WEAK] - EXPORT CAN1_SCE_IRQHandler [WEAK] - EXPORT EXTI9_5_IRQHandler [WEAK] - EXPORT TIM1_BRK_IRQHandler [WEAK] - EXPORT TIM1_UP_IRQHandler [WEAK] - EXPORT TIM1_TRG_COM_IRQHandler [WEAK] - EXPORT TIM1_CC_IRQHandler [WEAK] - EXPORT TIM2_IRQHandler [WEAK] - EXPORT TIM3_IRQHandler [WEAK] - EXPORT I2C1_EV_IRQHandler [WEAK] - EXPORT I2C1_ER_IRQHandler [WEAK] - EXPORT SPI1_IRQHandler [WEAK] - EXPORT USART1_IRQHandler [WEAK] - EXPORT USART2_IRQHandler [WEAK] - EXPORT EXTI15_10_IRQHandler [WEAK] - EXPORT RTCAlarm_IRQHandler [WEAK] - EXPORT USBWakeUp_IRQHandler [WEAK] - -WWDG_IRQHandler -PVD_IRQHandler -TAMPER_IRQHandler -RTC_IRQHandler -FLASH_IRQHandler -RCC_IRQHandler -EXTI0_IRQHandler -EXTI1_IRQHandler -EXTI2_IRQHandler -EXTI3_IRQHandler -EXTI4_IRQHandler -DMA1_Channel1_IRQHandler -DMA1_Channel2_IRQHandler -DMA1_Channel3_IRQHandler -DMA1_Channel4_IRQHandler -DMA1_Channel5_IRQHandler -DMA1_Channel6_IRQHandler -DMA1_Channel7_IRQHandler -ADC1_2_IRQHandler -USB_HP_CAN1_TX_IRQHandler -USB_LP_CAN1_RX0_IRQHandler -CAN1_RX1_IRQHandler -CAN1_SCE_IRQHandler -EXTI9_5_IRQHandler -TIM1_BRK_IRQHandler -TIM1_UP_IRQHandler -TIM1_TRG_COM_IRQHandler -TIM1_CC_IRQHandler -TIM2_IRQHandler -TIM3_IRQHandler -I2C1_EV_IRQHandler -I2C1_ER_IRQHandler -SPI1_IRQHandler -USART1_IRQHandler -USART2_IRQHandler -EXTI15_10_IRQHandler -RTCAlarm_IRQHandler -USBWakeUp_IRQHandler - - B . - - ENDP - - ALIGN - -;******************************************************************************* -; User Stack and Heap initialization -;******************************************************************************* - IF :DEF:__MICROLIB - - EXPORT __initial_sp - EXPORT __heap_base - EXPORT __heap_limit - - ELSE - - IMPORT __use_two_region_memory - EXPORT __user_initial_stackheap - -__user_initial_stackheap - - LDR R0, = Heap_Mem - LDR R1, =(Stack_Mem + Stack_Size) - LDR R2, = (Heap_Mem + Heap_Size) - LDR R3, = Stack_Mem - BX LR - - ALIGN - - ENDIF - - END - -;******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE***** diff --git a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_md.s b/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_md.s deleted file mode 100644 index 4380a01613..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/arm/startup_stm32f10x_md.s +++ /dev/null @@ -1,303 +0,0 @@ -;******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_md.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x Medium Density Devices vector table for RVMDK -;* toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == Reset_Handler -;* - Set the vector table entries with the exceptions ISR address -;* - Branches to __main in the C library (which eventually -;* calls main()). -;* After Reset the CortexM3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;* <<< Use Configuration Wizard in Context Menu >>> -;******************************************************************************* -; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;******************************************************************************* - -; Amount of memory (in bytes) allocated for Stack -; Tailor this value to your application needs -; Stack Configuration -; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Stack_Size EQU 0x00000400 - - AREA STACK, NOINIT, READWRITE, ALIGN=3 -Stack_Mem SPACE Stack_Size -__initial_sp - - -; Heap Configuration -; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> -; - -Heap_Size EQU 0x00000200 - - AREA HEAP, NOINIT, READWRITE, ALIGN=3 -__heap_base -Heap_Mem SPACE Heap_Size -__heap_limit - - PRESERVE8 - THUMB - - -; Vector Table Mapped to Address 0 at Reset - AREA RESET, DATA, READONLY - EXPORT __Vectors - EXPORT __Vectors_End - EXPORT __Vectors_Size - -__Vectors DCD __initial_sp ; Top of Stack - DCD Reset_Handler ; Reset Handler - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1_2 - DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX - DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C2 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD USART3_IRQHandler ; USART3 - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line - DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend -__Vectors_End - -__Vectors_Size EQU __Vectors_End - __Vectors - - AREA |.text|, CODE, READONLY - -; Reset handler routine -Reset_Handler PROC - EXPORT Reset_Handler [WEAK] - IMPORT __main - LDR R0, =__main - BX R0 - ENDP - -; Dummy Exception Handlers (infinite loops which can be modified) - -NMI_Handler PROC - EXPORT NMI_Handler [WEAK] - B . - ENDP -HardFault_Handler\ - PROC - EXPORT HardFault_Handler [WEAK] - B . - ENDP -MemManage_Handler\ - PROC - EXPORT MemManage_Handler [WEAK] - B . - ENDP -BusFault_Handler\ - PROC - EXPORT BusFault_Handler [WEAK] - B . - ENDP -UsageFault_Handler\ - PROC - EXPORT UsageFault_Handler [WEAK] - B . - ENDP -SVC_Handler PROC - EXPORT SVC_Handler [WEAK] - B . - ENDP -DebugMon_Handler\ - PROC - EXPORT DebugMon_Handler [WEAK] - B . - ENDP -PendSV_Handler PROC - EXPORT PendSV_Handler [WEAK] - B . - ENDP -SysTick_Handler PROC - EXPORT SysTick_Handler [WEAK] - B . - ENDP - -Default_Handler PROC - - EXPORT WWDG_IRQHandler [WEAK] - EXPORT PVD_IRQHandler [WEAK] - EXPORT TAMPER_IRQHandler [WEAK] - EXPORT RTC_IRQHandler [WEAK] - EXPORT FLASH_IRQHandler [WEAK] - EXPORT RCC_IRQHandler [WEAK] - EXPORT EXTI0_IRQHandler [WEAK] - EXPORT EXTI1_IRQHandler [WEAK] - EXPORT EXTI2_IRQHandler [WEAK] - EXPORT EXTI3_IRQHandler [WEAK] - EXPORT EXTI4_IRQHandler [WEAK] - EXPORT DMA1_Channel1_IRQHandler [WEAK] - EXPORT DMA1_Channel2_IRQHandler [WEAK] - EXPORT DMA1_Channel3_IRQHandler [WEAK] - EXPORT DMA1_Channel4_IRQHandler [WEAK] - EXPORT DMA1_Channel5_IRQHandler [WEAK] - EXPORT DMA1_Channel6_IRQHandler [WEAK] - EXPORT DMA1_Channel7_IRQHandler [WEAK] - EXPORT ADC1_2_IRQHandler [WEAK] - EXPORT USB_HP_CAN1_TX_IRQHandler [WEAK] - EXPORT USB_LP_CAN1_RX0_IRQHandler [WEAK] - EXPORT CAN1_RX1_IRQHandler [WEAK] - EXPORT CAN1_SCE_IRQHandler [WEAK] - EXPORT EXTI9_5_IRQHandler [WEAK] - EXPORT TIM1_BRK_IRQHandler [WEAK] - EXPORT TIM1_UP_IRQHandler [WEAK] - EXPORT TIM1_TRG_COM_IRQHandler [WEAK] - EXPORT TIM1_CC_IRQHandler [WEAK] - EXPORT TIM2_IRQHandler [WEAK] - EXPORT TIM3_IRQHandler [WEAK] - EXPORT TIM4_IRQHandler [WEAK] - EXPORT I2C1_EV_IRQHandler [WEAK] - EXPORT I2C1_ER_IRQHandler [WEAK] - EXPORT I2C2_EV_IRQHandler [WEAK] - EXPORT I2C2_ER_IRQHandler [WEAK] - EXPORT SPI1_IRQHandler [WEAK] - EXPORT SPI2_IRQHandler [WEAK] - EXPORT USART1_IRQHandler [WEAK] - EXPORT USART2_IRQHandler [WEAK] - EXPORT USART3_IRQHandler [WEAK] - EXPORT EXTI15_10_IRQHandler [WEAK] - EXPORT RTCAlarm_IRQHandler [WEAK] - EXPORT USBWakeUp_IRQHandler [WEAK] - -WWDG_IRQHandler -PVD_IRQHandler -TAMPER_IRQHandler -RTC_IRQHandler -FLASH_IRQHandler -RCC_IRQHandler -EXTI0_IRQHandler -EXTI1_IRQHandler -EXTI2_IRQHandler -EXTI3_IRQHandler -EXTI4_IRQHandler -DMA1_Channel1_IRQHandler -DMA1_Channel2_IRQHandler -DMA1_Channel3_IRQHandler -DMA1_Channel4_IRQHandler -DMA1_Channel5_IRQHandler -DMA1_Channel6_IRQHandler -DMA1_Channel7_IRQHandler -ADC1_2_IRQHandler -USB_HP_CAN1_TX_IRQHandler -USB_LP_CAN1_RX0_IRQHandler -CAN1_RX1_IRQHandler -CAN1_SCE_IRQHandler -EXTI9_5_IRQHandler -TIM1_BRK_IRQHandler -TIM1_UP_IRQHandler -TIM1_TRG_COM_IRQHandler -TIM1_CC_IRQHandler -TIM2_IRQHandler -TIM3_IRQHandler -TIM4_IRQHandler -I2C1_EV_IRQHandler -I2C1_ER_IRQHandler -I2C2_EV_IRQHandler -I2C2_ER_IRQHandler -SPI1_IRQHandler -SPI2_IRQHandler -USART1_IRQHandler -USART2_IRQHandler -USART3_IRQHandler -EXTI15_10_IRQHandler -RTCAlarm_IRQHandler -USBWakeUp_IRQHandler - - B . - - ENDP - - ALIGN - -;******************************************************************************* -; User Stack and Heap initialization -;******************************************************************************* - IF :DEF:__MICROLIB - - EXPORT __initial_sp - EXPORT __heap_base - EXPORT __heap_limit - - ELSE - - IMPORT __use_two_region_memory - EXPORT __user_initial_stackheap - -__user_initial_stackheap - - LDR R0, = Heap_Mem - LDR R1, =(Stack_Mem + Stack_Size) - LDR R2, = (Heap_Mem + Heap_Size) - LDR R3, = Stack_Mem - BX LR - - ALIGN - - ENDIF - - END - -;******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE***** diff --git a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_cl.s b/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_cl.s deleted file mode 100644 index ee9b9ebd4e..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_cl.s +++ /dev/null @@ -1,464 +0,0 @@ -/** - ****************************************************************************** - * @file startup_stm32f10x_cl.s - * @author MCD Application Team - * @version V3.1.2 - * @date 09/28/2009 - * @brief STM32F10x Connectivity line Devices vector table for RIDE7 toolchain. - * This module performs: - * - Set the initial SP - * - Set the initial PC == Reset_Handler, - * - Set the vector table entries with the exceptions ISR - * address. - * - Branches to main in the C library (which eventually - * calls main()). - * After Reset the Cortex-M3 processor is in Thread mode, - * priority is Privileged, and the Stack is set to Main. - ******************************************************************************* - * @copy - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2009 STMicroelectronics

- */ - - .syntax unified - .cpu cortex-m3 - .fpu softvfp - .thumb - -.global g_pfnVectors -.global Default_Handler - -/* start address for the initialization values of the .data section. -defined in linker script */ -.word _sidata -/* start address for the .data section. defined in linker script */ -.word _sdata -/* end address for the .data section. defined in linker script */ -.word _edata -/* start address for the .bss section. defined in linker script */ -.word _sbss -/* end address for the .bss section. defined in linker script */ -.word _ebss - -.equ BootRAM, 0xF1E0F85F -/** - * @brief This is the code that gets called when the processor first - * starts execution following a reset event. Only the absolutely - * necessary set is performed, after which the application - * supplied main() routine is called. - * @param None - * @retval : None -*/ - - .section .text.Reset_Handler - .weak Reset_Handler - .type Reset_Handler, %function -Reset_Handler: - -/* Copy the data segment initializers from flash to SRAM */ - movs r1, #0 - b LoopCopyDataInit - -CopyDataInit: - ldr r3, =_sidata - ldr r3, [r3, r1] - str r3, [r0, r1] - adds r1, r1, #4 - -LoopCopyDataInit: - ldr r0, =_sdata - ldr r3, =_edata - adds r2, r0, r1 - cmp r2, r3 - bcc CopyDataInit - ldr r2, =_sbss - b LoopFillZerobss - -/* Zero fill the bss segment. */ -FillZerobss: - movs r3, #0 - str r3, [r2], #4 - -LoopFillZerobss: - ldr r3, = _ebss - cmp r2, r3 - bcc FillZerobss -/* Call the application's entry point.*/ - bl main - bx lr -.size Reset_Handler, .-Reset_Handler - -/** - * @brief This is the code that gets called when the processor receives an - * unexpected interrupt. This simply enters an infinite loop, preserving - * the system state for examination by a debugger. - * - * @param None - * @retval : None -*/ - .section .text.Default_Handler,"ax",%progbits -Default_Handler: -Infinite_Loop: - b Infinite_Loop - .size Default_Handler, .-Default_Handler - -/****************************************************************************** -* -* The minimal vector table for a Cortex M3. Note that the proper constructs -* must be placed on this to ensure that it ends up at physical address -* 0x0000.0000. -* -******************************************************************************/ - .section .isr_vector,"a",%progbits - .type g_pfnVectors, %object - .size g_pfnVectors, .-g_pfnVectors - - -g_pfnVectors: - .word _estack - .word Reset_Handler - .word NMI_Handler - .word HardFault_Handler - .word MemManage_Handler - .word BusFault_Handler - .word UsageFault_Handler - .word 0 - .word 0 - .word 0 - .word 0 - .word SVC_Handler - .word DebugMon_Handler - .word 0 - .word PendSV_Handler - .word SysTick_Handler - .word WWDG_IRQHandler - .word PVD_IRQHandler - .word TAMPER_IRQHandler - .word RTC_IRQHandler - .word FLASH_IRQHandler - .word RCC_IRQHandler - .word EXTI0_IRQHandler - .word EXTI1_IRQHandler - .word EXTI2_IRQHandler - .word EXTI3_IRQHandler - .word EXTI4_IRQHandler - .word DMA1_Channel1_IRQHandler - .word DMA1_Channel2_IRQHandler - .word DMA1_Channel3_IRQHandler - .word DMA1_Channel4_IRQHandler - .word DMA1_Channel5_IRQHandler - .word DMA1_Channel6_IRQHandler - .word DMA1_Channel7_IRQHandler - .word ADC1_2_IRQHandler - .word CAN1_TX_IRQHandler - .word CAN1_RX0_IRQHandler - .word CAN1_RX1_IRQHandler - .word CAN1_SCE_IRQHandler - .word EXTI9_5_IRQHandler - .word TIM1_BRK_IRQHandler - .word TIM1_UP_IRQHandler - .word TIM1_TRG_COM_IRQHandler - .word TIM1_CC_IRQHandler - .word TIM2_IRQHandler - .word TIM3_IRQHandler - .word TIM4_IRQHandler - .word I2C1_EV_IRQHandler - .word I2C1_ER_IRQHandler - .word I2C2_EV_IRQHandler - .word I2C2_ER_IRQHandler - .word SPI1_IRQHandler - .word SPI2_IRQHandler - .word USART1_IRQHandler - .word USART2_IRQHandler - .word USART3_IRQHandler - .word EXTI15_10_IRQHandler - .word RTCAlarm_IRQHandler - .word OTG_FS_WKUP_IRQHandler - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word TIM5_IRQHandler - .word SPI3_IRQHandler - .word UART4_IRQHandler - .word UART5_IRQHandler - .word TIM6_IRQHandler - .word TIM7_IRQHandler - .word DMA2_Channel1_IRQHandler - .word DMA2_Channel2_IRQHandler - .word DMA2_Channel3_IRQHandler - .word DMA2_Channel4_IRQHandler - .word DMA2_Channel5_IRQHandler - .word ETH_IRQHandler - .word ETH_WKUP_IRQHandler - .word CAN2_TX_IRQHandler - .word CAN2_RX0_IRQHandler - .word CAN2_RX1_IRQHandler - .word CAN2_SCE_IRQHandler - .word OTG_FS_IRQHandler - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word BootRAM /* @0x1E0. This is for boot in RAM mode for - STM32F10x Connectivity line Devices. */ - -/******************************************************************************* -* -* Provide weak aliases for each Exception handler to the Default_Handler. -* As they are weak aliases, any function with the same name will override -* this definition. -* -*******************************************************************************/ - .weak NMI_Handler - .thumb_set NMI_Handler,Default_Handler - - .weak HardFault_Handler - .thumb_set HardFault_Handler,Default_Handler - - .weak MemManage_Handler - .thumb_set MemManage_Handler,Default_Handler - - .weak BusFault_Handler - .thumb_set BusFault_Handler,Default_Handler - - .weak UsageFault_Handler - .thumb_set UsageFault_Handler,Default_Handler - - .weak SVC_Handler - .thumb_set SVC_Handler,Default_Handler - - .weak DebugMon_Handler - .thumb_set DebugMon_Handler,Default_Handler - - .weak PendSV_Handler - .thumb_set PendSV_Handler,Default_Handler - - .weak SysTick_Handler - .thumb_set SysTick_Handler,Default_Handler - - .weak WWDG_IRQHandler - .thumb_set WWDG_IRQHandler,Default_Handler - - .weak PVD_IRQHandler - .thumb_set PVD_IRQHandler,Default_Handler - - .weak TAMPER_IRQHandler - .thumb_set TAMPER_IRQHandler,Default_Handler - - .weak RTC_IRQHandler - .thumb_set RTC_IRQHandler,Default_Handler - - .weak FLASH_IRQHandler - .thumb_set FLASH_IRQHandler,Default_Handler - - .weak RCC_IRQHandler - .thumb_set RCC_IRQHandler,Default_Handler - - .weak EXTI0_IRQHandler - .thumb_set EXTI0_IRQHandler,Default_Handler - - .weak EXTI1_IRQHandler - .thumb_set EXTI1_IRQHandler,Default_Handler - - .weak EXTI2_IRQHandler - .thumb_set EXTI2_IRQHandler,Default_Handler - - .weak EXTI3_IRQHandler - .thumb_set EXTI3_IRQHandler,Default_Handler - - .weak EXTI4_IRQHandler - .thumb_set EXTI4_IRQHandler,Default_Handler - - .weak DMA1_Channel1_IRQHandler - .thumb_set DMA1_Channel1_IRQHandler,Default_Handler - - .weak DMA1_Channel2_IRQHandler - .thumb_set DMA1_Channel2_IRQHandler,Default_Handler - - .weak DMA1_Channel3_IRQHandler - .thumb_set DMA1_Channel3_IRQHandler,Default_Handler - - .weak DMA1_Channel4_IRQHandler - .thumb_set DMA1_Channel4_IRQHandler,Default_Handler - - .weak DMA1_Channel5_IRQHandler - .thumb_set DMA1_Channel5_IRQHandler,Default_Handler - - .weak DMA1_Channel6_IRQHandler - .thumb_set DMA1_Channel6_IRQHandler,Default_Handler - - .weak DMA1_Channel7_IRQHandler - .thumb_set DMA1_Channel7_IRQHandler,Default_Handler - - .weak ADC1_2_IRQHandler - .thumb_set ADC1_2_IRQHandler,Default_Handler - - .weak CAN1_TX_IRQHandler - .thumb_set CAN1_TX_IRQHandler,Default_Handler - - .weak CAN1_RX0_IRQHandler - .thumb_set CAN1_RX0_IRQHandler,Default_Handler - - .weak CAN1_RX1_IRQHandler - .thumb_set CAN1_RX1_IRQHandler,Default_Handler - - .weak CAN1_SCE_IRQHandler - .thumb_set CAN1_SCE_IRQHandler,Default_Handler - - .weak EXTI9_5_IRQHandler - .thumb_set EXTI9_5_IRQHandler,Default_Handler - - .weak TIM1_BRK_IRQHandler - .thumb_set TIM1_BRK_IRQHandler,Default_Handler - - .weak TIM1_UP_IRQHandler - .thumb_set TIM1_UP_IRQHandler,Default_Handler - - .weak TIM1_TRG_COM_IRQHandler - .thumb_set TIM1_TRG_COM_IRQHandler,Default_Handler - - .weak TIM1_CC_IRQHandler - .thumb_set TIM1_CC_IRQHandler,Default_Handler - - .weak TIM2_IRQHandler - .thumb_set TIM2_IRQHandler,Default_Handler - - .weak TIM3_IRQHandler - .thumb_set TIM3_IRQHandler,Default_Handler - - .weak TIM4_IRQHandler - .thumb_set TIM4_IRQHandler,Default_Handler - - .weak I2C1_EV_IRQHandler - .thumb_set I2C1_EV_IRQHandler,Default_Handler - - .weak I2C1_ER_IRQHandler - .thumb_set I2C1_ER_IRQHandler,Default_Handler - - .weak I2C2_EV_IRQHandler - .thumb_set I2C2_EV_IRQHandler,Default_Handler - - .weak I2C2_ER_IRQHandler - .thumb_set I2C2_ER_IRQHandler,Default_Handler - - .weak SPI1_IRQHandler - .thumb_set SPI1_IRQHandler,Default_Handler - - .weak SPI2_IRQHandler - .thumb_set SPI2_IRQHandler,Default_Handler - - .weak USART1_IRQHandler - .thumb_set USART1_IRQHandler,Default_Handler - - .weak USART2_IRQHandler - .thumb_set USART2_IRQHandler,Default_Handler - - .weak USART3_IRQHandler - .thumb_set USART3_IRQHandler,Default_Handler - - .weak EXTI15_10_IRQHandler - .thumb_set EXTI15_10_IRQHandler,Default_Handler - - .weak RTCAlarm_IRQHandler - .thumb_set RTCAlarm_IRQHandler,Default_Handler - - .weak OTG_FS_WKUP_IRQHandler - .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler - - .weak TIM5_IRQHandler - .thumb_set TIM5_IRQHandler,Default_Handler - - .weak SPI3_IRQHandler - .thumb_set SPI3_IRQHandler,Default_Handler - - .weak UART4_IRQHandler - .thumb_set UART4_IRQHandler,Default_Handler - - .weak UART5_IRQHandler - .thumb_set UART5_IRQHandler,Default_Handler - - .weak TIM6_IRQHandler - .thumb_set TIM6_IRQHandler,Default_Handler - - .weak TIM7_IRQHandler - .thumb_set TIM7_IRQHandler,Default_Handler - - .weak DMA2_Channel1_IRQHandler - .thumb_set DMA2_Channel1_IRQHandler,Default_Handler - - .weak DMA2_Channel2_IRQHandler - .thumb_set DMA2_Channel2_IRQHandler,Default_Handler - - .weak DMA2_Channel3_IRQHandler - .thumb_set DMA2_Channel3_IRQHandler,Default_Handler - - .weak DMA2_Channel4_IRQHandler - .thumb_set DMA2_Channel4_IRQHandler,Default_Handler - - .weak DMA2_Channel5_IRQHandler - .thumb_set DMA2_Channel5_IRQHandler,Default_Handler - - .weak ETH_IRQHandler - .thumb_set ETH_IRQHandler,Default_Handler - - .weak ETH_WKUP_IRQHandler - .thumb_set ETH_WKUP_IRQHandler,Default_Handler - - .weak CAN2_TX_IRQHandler - .thumb_set CAN2_TX_IRQHandler,Default_Handler - - .weak CAN2_RX0_IRQHandler - .thumb_set CAN2_RX0_IRQHandler,Default_Handler - - .weak CAN2_RX1_IRQHandler - .thumb_set CAN2_RX1_IRQHandler,Default_Handler - - .weak CAN2_SCE_IRQHandler - .thumb_set CAN2_SCE_IRQHandler,Default_Handler - - .weak OTG_FS_IRQHandler - .thumb_set OTG_FS_IRQHandler ,Default_Handler - diff --git a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_hd.s b/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_hd.s deleted file mode 100644 index 7abcbcde3e..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_hd.s +++ /dev/null @@ -1,483 +0,0 @@ -/** - ****************************************************************************** - * @file startup_stm32f10x_hd.s - * @author MCD Application Team - * @version V3.1.2 - * @date 09/28/2009 - * @brief STM32F10x High Density Devices vector table for RIDE7 toolchain. - * This module performs: - * - Set the initial SP - * - Set the initial PC == Reset_Handler, - * - Set the vector table entries with the exceptions ISR address, - * - Configure external SRAM mounted on STM3210E-EVAL board - * to be used as data memory (optional, to be enabled by user) - * - Branches to main in the C library (which eventually - * calls main()). - * After Reset the Cortex-M3 processor is in Thread mode, - * priority is Privileged, and the Stack is set to Main. - ****************************************************************************** - * @copy - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2009 STMicroelectronics

- */ - - .syntax unified - .cpu cortex-m3 - .fpu softvfp - .thumb - -.global g_pfnVectors -.global SystemInit_ExtMemCtl_Dummy -.global Default_Handler - -/* start address for the initialization values of the .data section. -defined in linker script */ -.word _sidata -/* start address for the .data section. defined in linker script */ -.word _sdata -/* end address for the .data section. defined in linker script */ -.word _edata -/* start address for the .bss section. defined in linker script */ -.word _sbss -/* end address for the .bss section. defined in linker script */ -.word _ebss -/* stack used for SystemInit_ExtMemCtl; always internal RAM used */ - -.equ Initial_spTop, 0x20000400 -.equ BootRAM, 0xF1E0F85F -/** - * @brief This is the code that gets called when the processor first - * starts execution following a reset event. Only the absolutely - * necessary set is performed, after which the application - * supplied main() routine is called. - * @param None - * @retval : None -*/ - - .section .text.Reset_Handler - .weak Reset_Handler - .type Reset_Handler, %function -Reset_Handler: - -/* FSMC Bank1 NOR/SRAM3 is used for the STM3210E-EVAL, if another Bank is - required, then adjust the Register Addresses */ - bl SystemInit_ExtMemCtl -/* restore original stack pointer */ - LDR r0, =_estack - MSR msp, r0 -/* Copy the data segment initializers from flash to SRAM */ - movs r1, #0 - b LoopCopyDataInit - -CopyDataInit: - ldr r3, =_sidata - ldr r3, [r3, r1] - str r3, [r0, r1] - adds r1, r1, #4 - -LoopCopyDataInit: - ldr r0, =_sdata - ldr r3, =_edata - adds r2, r0, r1 - cmp r2, r3 - bcc CopyDataInit - ldr r2, =_sbss - b LoopFillZerobss -/* Zero fill the bss segment. */ -FillZerobss: - movs r3, #0 - str r3, [r2], #4 - -LoopFillZerobss: - ldr r3, = _ebss - cmp r2, r3 - bcc FillZerobss -/* Call the application's entry point.*/ - bl main - bx lr -.size Reset_Handler, .-Reset_Handler - -/** - * @brief Dummy SystemInit_ExtMemCtl function - * @param None - * @retval : None -*/ - .section .text.SystemInit_ExtMemCtl_Dummy,"ax",%progbits -SystemInit_ExtMemCtl_Dummy: - bx lr - .size SystemInit_ExtMemCtl_Dummy, .-SystemInit_ExtMemCtl_Dummy - -/** - * @brief This is the code that gets called when the processor receives an - * unexpected interrupt. This simply enters an infinite loop, preserving - * the system state for examination by a debugger. - * - * @param None - * @retval : None -*/ - .section .text.Default_Handler,"ax",%progbits -Default_Handler: -Infinite_Loop: - b Infinite_Loop - .size Default_Handler, .-Default_Handler -/****************************************************************************** -* -* The minimal vector table for a Cortex M3. Note that the proper constructs -* must be placed on this to ensure that it ends up at physical address -* 0x0000.0000. -* -******************************************************************************/ - .section .isr_vector,"a",%progbits - .type g_pfnVectors, %object - .size g_pfnVectors, .-g_pfnVectors - - -g_pfnVectors: - .word Initial_spTop - .word Reset_Handler - .word NMI_Handler - .word HardFault_Handler - .word MemManage_Handler - .word BusFault_Handler - .word UsageFault_Handler - .word 0 - .word 0 - .word 0 - .word 0 - .word SVC_Handler - .word DebugMon_Handler - .word 0 - .word PendSV_Handler - .word SysTick_Handler - .word WWDG_IRQHandler - .word PVD_IRQHandler - .word TAMPER_IRQHandler - .word RTC_IRQHandler - .word FLASH_IRQHandler - .word RCC_IRQHandler - .word EXTI0_IRQHandler - .word EXTI1_IRQHandler - .word EXTI2_IRQHandler - .word EXTI3_IRQHandler - .word EXTI4_IRQHandler - .word DMA1_Channel1_IRQHandler - .word DMA1_Channel2_IRQHandler - .word DMA1_Channel3_IRQHandler - .word DMA1_Channel4_IRQHandler - .word DMA1_Channel5_IRQHandler - .word DMA1_Channel6_IRQHandler - .word DMA1_Channel7_IRQHandler - .word ADC1_2_IRQHandler - .word USB_HP_CAN1_TX_IRQHandler - .word USB_LP_CAN1_RX0_IRQHandler - .word CAN1_RX1_IRQHandler - .word CAN1_SCE_IRQHandler - .word EXTI9_5_IRQHandler - .word TIM1_BRK_IRQHandler - .word TIM1_UP_IRQHandler - .word TIM1_TRG_COM_IRQHandler - .word TIM1_CC_IRQHandler - .word TIM2_IRQHandler - .word TIM3_IRQHandler - .word TIM4_IRQHandler - .word I2C1_EV_IRQHandler - .word I2C1_ER_IRQHandler - .word I2C2_EV_IRQHandler - .word I2C2_ER_IRQHandler - .word SPI1_IRQHandler - .word SPI2_IRQHandler - .word USART1_IRQHandler - .word USART2_IRQHandler - .word USART3_IRQHandler - .word EXTI15_10_IRQHandler - .word RTCAlarm_IRQHandler - .word USBWakeUp_IRQHandler - .word TIM8_BRK_IRQHandler - .word TIM8_UP_IRQHandler - .word TIM8_TRG_COM_IRQHandler - .word TIM8_CC_IRQHandler - .word ADC3_IRQHandler - .word FSMC_IRQHandler - .word SDIO_IRQHandler - .word TIM5_IRQHandler - .word SPI3_IRQHandler - .word UART4_IRQHandler - .word UART5_IRQHandler - .word TIM6_IRQHandler - .word TIM7_IRQHandler - .word DMA2_Channel1_IRQHandler - .word DMA2_Channel2_IRQHandler - .word DMA2_Channel3_IRQHandler - .word DMA2_Channel4_5_IRQHandler - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word BootRAM /* @0x1E0. This is for boot in RAM mode for - STM32F10x High Density devices. */ - -/******************************************************************************* -* -* Provide weak aliases for each Exception handler to the Default_Handler. -* As they are weak aliases, any function with the same name will override -* this definition. -* -*******************************************************************************/ - - .weak NMI_Handler - .thumb_set NMI_Handler,Default_Handler - - .weak HardFault_Handler - .thumb_set HardFault_Handler,Default_Handler - - .weak MemManage_Handler - .thumb_set MemManage_Handler,Default_Handler - - .weak BusFault_Handler - .thumb_set BusFault_Handler,Default_Handler - - .weak UsageFault_Handler - .thumb_set UsageFault_Handler,Default_Handler - - .weak SVC_Handler - .thumb_set SVC_Handler,Default_Handler - - .weak DebugMon_Handler - .thumb_set DebugMon_Handler,Default_Handler - - .weak PendSV_Handler - .thumb_set PendSV_Handler,Default_Handler - - .weak SysTick_Handler - .thumb_set SysTick_Handler,Default_Handler - - .weak WWDG_IRQHandler - .thumb_set WWDG_IRQHandler,Default_Handler - - .weak PVD_IRQHandler - .thumb_set PVD_IRQHandler,Default_Handler - - .weak TAMPER_IRQHandler - .thumb_set TAMPER_IRQHandler,Default_Handler - - .weak RTC_IRQHandler - .thumb_set RTC_IRQHandler,Default_Handler - - .weak FLASH_IRQHandler - .thumb_set FLASH_IRQHandler,Default_Handler - - .weak RCC_IRQHandler - .thumb_set RCC_IRQHandler,Default_Handler - - .weak EXTI0_IRQHandler - .thumb_set EXTI0_IRQHandler,Default_Handler - - .weak EXTI1_IRQHandler - .thumb_set EXTI1_IRQHandler,Default_Handler - - .weak EXTI2_IRQHandler - .thumb_set EXTI2_IRQHandler,Default_Handler - - .weak EXTI3_IRQHandler - .thumb_set EXTI3_IRQHandler,Default_Handler - - .weak EXTI4_IRQHandler - .thumb_set EXTI4_IRQHandler,Default_Handler - - .weak DMA1_Channel1_IRQHandler - .thumb_set DMA1_Channel1_IRQHandler,Default_Handler - - .weak DMA1_Channel2_IRQHandler - .thumb_set DMA1_Channel2_IRQHandler,Default_Handler - - .weak DMA1_Channel3_IRQHandler - .thumb_set DMA1_Channel3_IRQHandler,Default_Handler - - .weak DMA1_Channel4_IRQHandler - .thumb_set DMA1_Channel4_IRQHandler,Default_Handler - - .weak DMA1_Channel5_IRQHandler - .thumb_set DMA1_Channel5_IRQHandler,Default_Handler - - .weak DMA1_Channel6_IRQHandler - .thumb_set DMA1_Channel6_IRQHandler,Default_Handler - - .weak DMA1_Channel7_IRQHandler - .thumb_set DMA1_Channel7_IRQHandler,Default_Handler - - .weak ADC1_2_IRQHandler - .thumb_set ADC1_2_IRQHandler,Default_Handler - - .weak USB_HP_CAN1_TX_IRQHandler - .thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler - - .weak USB_LP_CAN1_RX0_IRQHandler - .thumb_set USB_LP_CAN1_RX0_IRQHandler,Default_Handler - - .weak CAN1_RX1_IRQHandler - .thumb_set CAN1_RX1_IRQHandler,Default_Handler - - .weak CAN1_SCE_IRQHandler - .thumb_set CAN1_SCE_IRQHandler,Default_Handler - - .weak EXTI9_5_IRQHandler - .thumb_set EXTI9_5_IRQHandler,Default_Handler - - .weak TIM1_BRK_IRQHandler - .thumb_set TIM1_BRK_IRQHandler,Default_Handler - - .weak TIM1_UP_IRQHandler - .thumb_set TIM1_UP_IRQHandler,Default_Handler - - .weak TIM1_TRG_COM_IRQHandler - .thumb_set TIM1_TRG_COM_IRQHandler,Default_Handler - - .weak TIM1_CC_IRQHandler - .thumb_set TIM1_CC_IRQHandler,Default_Handler - - .weak TIM2_IRQHandler - .thumb_set TIM2_IRQHandler,Default_Handler - - .weak TIM3_IRQHandler - .thumb_set TIM3_IRQHandler,Default_Handler - - .weak TIM4_IRQHandler - .thumb_set TIM4_IRQHandler,Default_Handler - - .weak I2C1_EV_IRQHandler - .thumb_set I2C1_EV_IRQHandler,Default_Handler - - .weak I2C1_ER_IRQHandler - .thumb_set I2C1_ER_IRQHandler,Default_Handler - - .weak I2C2_EV_IRQHandler - .thumb_set I2C2_EV_IRQHandler,Default_Handler - - .weak I2C2_ER_IRQHandler - .thumb_set I2C2_ER_IRQHandler,Default_Handler - - .weak SPI1_IRQHandler - .thumb_set SPI1_IRQHandler,Default_Handler - - .weak SPI2_IRQHandler - .thumb_set SPI2_IRQHandler,Default_Handler - - .weak USART1_IRQHandler - .thumb_set USART1_IRQHandler,Default_Handler - - .weak USART2_IRQHandler - .thumb_set USART2_IRQHandler,Default_Handler - - .weak USART3_IRQHandler - .thumb_set USART3_IRQHandler,Default_Handler - - .weak EXTI15_10_IRQHandler - .thumb_set EXTI15_10_IRQHandler,Default_Handler - - .weak RTCAlarm_IRQHandler - .thumb_set RTCAlarm_IRQHandler,Default_Handler - - .weak USBWakeUp_IRQHandler - .thumb_set USBWakeUp_IRQHandler,Default_Handler - - .weak TIM8_BRK_IRQHandler - .thumb_set TIM8_BRK_IRQHandler,Default_Handler - - .weak TIM8_UP_IRQHandler - .thumb_set TIM8_UP_IRQHandler,Default_Handler - - .weak TIM8_TRG_COM_IRQHandler - .thumb_set TIM8_TRG_COM_IRQHandler,Default_Handler - - .weak TIM8_CC_IRQHandler - .thumb_set TIM8_CC_IRQHandler,Default_Handler - - .weak ADC3_IRQHandler - .thumb_set ADC3_IRQHandler,Default_Handler - - .weak FSMC_IRQHandler - .thumb_set FSMC_IRQHandler,Default_Handler - - .weak SDIO_IRQHandler - .thumb_set SDIO_IRQHandler,Default_Handler - - .weak TIM5_IRQHandler - .thumb_set TIM5_IRQHandler,Default_Handler - - .weak SPI3_IRQHandler - .thumb_set SPI3_IRQHandler,Default_Handler - - .weak UART4_IRQHandler - .thumb_set UART4_IRQHandler,Default_Handler - - .weak UART5_IRQHandler - .thumb_set UART5_IRQHandler,Default_Handler - - .weak TIM6_IRQHandler - .thumb_set TIM6_IRQHandler,Default_Handler - - .weak TIM7_IRQHandler - .thumb_set TIM7_IRQHandler,Default_Handler - - .weak DMA2_Channel1_IRQHandler - .thumb_set DMA2_Channel1_IRQHandler,Default_Handler - - .weak DMA2_Channel2_IRQHandler - .thumb_set DMA2_Channel2_IRQHandler,Default_Handler - - .weak DMA2_Channel3_IRQHandler - .thumb_set DMA2_Channel3_IRQHandler,Default_Handler - - .weak DMA2_Channel4_5_IRQHandler - .thumb_set DMA2_Channel4_5_IRQHandler,Default_Handler - - .weak SystemInit_ExtMemCtl - .thumb_set SystemInit_ExtMemCtl,SystemInit_ExtMemCtl_Dummy - diff --git a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_ld.s b/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_ld.s deleted file mode 100644 index 1ad1bb46d9..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_ld.s +++ /dev/null @@ -1,339 +0,0 @@ -/** - ****************************************************************************** - * @file startup_stm32f10x_ld.s - * @author MCD Application Team - * @version V3.1.2 - * @date 09/28/2009 - * @brief STM32F10x Low Density Devices vector table for RIDE7 toolchain. - * This module performs: - * - Set the initial SP - * - Set the initial PC == Reset_Handler, - * - Set the vector table entries with the exceptions ISR address. - * - Branches to main in the C library (which eventually - * calls main()). - * After Reset the Cortex-M3 processor is in Thread mode, - * priority is Privileged, and the Stack is set to Main. - ****************************************************************************** - * @copy - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2009 STMicroelectronics

- */ - - .syntax unified - .cpu cortex-m3 - .fpu softvfp - .thumb - -.global g_pfnVectors -.global Default_Handler - -/* start address for the initialization values of the .data section. -defined in linker script */ -.word _sidata -/* start address for the .data section. defined in linker script */ -.word _sdata -/* end address for the .data section. defined in linker script */ -.word _edata -/* start address for the .bss section. defined in linker script */ -.word _sbss -/* end address for the .bss section. defined in linker script */ -.word _ebss - -.equ BootRAM, 0xF108F85F -/** - * @brief This is the code that gets called when the processor first - * starts execution following a reset event. Only the absolutely - * necessary set is performed, after which the application - * supplied main() routine is called. - * @param None - * @retval : None -*/ - - .section .text.Reset_Handler - .weak Reset_Handler - .type Reset_Handler, %function -Reset_Handler: - -/* Copy the data segment initializers from flash to SRAM */ - movs r1, #0 - b LoopCopyDataInit - -CopyDataInit: - ldr r3, =_sidata - ldr r3, [r3, r1] - str r3, [r0, r1] - adds r1, r1, #4 - -LoopCopyDataInit: - ldr r0, =_sdata - ldr r3, =_edata - adds r2, r0, r1 - cmp r2, r3 - bcc CopyDataInit - ldr r2, =_sbss - b LoopFillZerobss -/* Zero fill the bss segment. */ -FillZerobss: - movs r3, #0 - str r3, [r2], #4 - -LoopFillZerobss: - ldr r3, = _ebss - cmp r2, r3 - bcc FillZerobss -/* Call the application's entry point.*/ - bl main - bx lr -.size Reset_Handler, .-Reset_Handler - -/** - * @brief This is the code that gets called when the processor receives an - * unexpected interrupt. This simply enters an infinite loop, preserving - * the system state for examination by a debugger. - * - * @param None - * @retval : None -*/ - .section .text.Default_Handler,"ax",%progbits -Default_Handler: -Infinite_Loop: - b Infinite_Loop - .size Default_Handler, .-Default_Handler -/****************************************************************************** -* -* The minimal vector table for a Cortex M3. Note that the proper constructs -* must be placed on this to ensure that it ends up at physical address -* 0x0000.0000. -* -******************************************************************************/ - .section .isr_vector,"a",%progbits - .type g_pfnVectors, %object - .size g_pfnVectors, .-g_pfnVectors - - -g_pfnVectors: - .word _estack - .word Reset_Handler - .word NMI_Handler - .word HardFault_Handler - .word MemManage_Handler - .word BusFault_Handler - .word UsageFault_Handler - .word 0 - .word 0 - .word 0 - .word 0 - .word SVC_Handler - .word DebugMon_Handler - .word 0 - .word PendSV_Handler - .word SysTick_Handler - .word WWDG_IRQHandler - .word PVD_IRQHandler - .word TAMPER_IRQHandler - .word RTC_IRQHandler - .word FLASH_IRQHandler - .word RCC_IRQHandler - .word EXTI0_IRQHandler - .word EXTI1_IRQHandler - .word EXTI2_IRQHandler - .word EXTI3_IRQHandler - .word EXTI4_IRQHandler - .word DMA1_Channel1_IRQHandler - .word DMA1_Channel2_IRQHandler - .word DMA1_Channel3_IRQHandler - .word DMA1_Channel4_IRQHandler - .word DMA1_Channel5_IRQHandler - .word DMA1_Channel6_IRQHandler - .word DMA1_Channel7_IRQHandler - .word ADC1_2_IRQHandler - .word USB_HP_CAN1_TX_IRQHandler - .word USB_LP_CAN1_RX0_IRQHandler - .word CAN1_RX1_IRQHandler - .word CAN1_SCE_IRQHandler - .word EXTI9_5_IRQHandler - .word TIM1_BRK_IRQHandler - .word TIM1_UP_IRQHandler - .word TIM1_TRG_COM_IRQHandler - .word TIM1_CC_IRQHandler - .word TIM2_IRQHandler - .word TIM3_IRQHandler - 0 - .word I2C1_EV_IRQHandler - .word I2C1_ER_IRQHandler - 0 - 0 - .word SPI1_IRQHandler - 0 - .word USART1_IRQHandler - .word USART2_IRQHandler - 0 - .word EXTI15_10_IRQHandler - .word RTCAlarm_IRQHandler - .word USBWakeUp_IRQHandler - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word BootRAM /* @0x108. This is for boot in RAM mode for - STM32F10x Low Density devices.*/ - -/******************************************************************************* -* -* Provide weak aliases for each Exception handler to the Default_Handler. -* As they are weak aliases, any function with the same name will override -* this definition. -* -*******************************************************************************/ - - .weak NMI_Handler - .thumb_set NMI_Handler,Default_Handler - - .weak HardFault_Handler - .thumb_set HardFault_Handler,Default_Handler - - .weak MemManage_Handler - .thumb_set MemManage_Handler,Default_Handler - - .weak BusFault_Handler - .thumb_set BusFault_Handler,Default_Handler - - .weak UsageFault_Handler - .thumb_set UsageFault_Handler,Default_Handler - - .weak SVC_Handler - .thumb_set SVC_Handler,Default_Handler - - .weak DebugMon_Handler - .thumb_set DebugMon_Handler,Default_Handler - - .weak PendSV_Handler - .thumb_set PendSV_Handler,Default_Handler - - .weak SysTick_Handler - .thumb_set SysTick_Handler,Default_Handler - - .weak WWDG_IRQHandler - .thumb_set WWDG_IRQHandler,Default_Handler - - .weak PVD_IRQHandler - .thumb_set PVD_IRQHandler,Default_Handler - - .weak TAMPER_IRQHandler - .thumb_set TAMPER_IRQHandler,Default_Handler - - .weak RTC_IRQHandler - .thumb_set RTC_IRQHandler,Default_Handler - - .weak FLASH_IRQHandler - .thumb_set FLASH_IRQHandler,Default_Handler - - .weak RCC_IRQHandler - .thumb_set RCC_IRQHandler,Default_Handler - - .weak EXTI0_IRQHandler - .thumb_set EXTI0_IRQHandler,Default_Handler - - .weak EXTI1_IRQHandler - .thumb_set EXTI1_IRQHandler,Default_Handler - - .weak EXTI2_IRQHandler - .thumb_set EXTI2_IRQHandler,Default_Handler - - .weak EXTI3_IRQHandler - .thumb_set EXTI3_IRQHandler,Default_Handler - - .weak EXTI4_IRQHandler - .thumb_set EXTI4_IRQHandler,Default_Handler - - .weak DMA1_Channel1_IRQHandler - .thumb_set DMA1_Channel1_IRQHandler,Default_Handler - - .weak DMA1_Channel2_IRQHandler - .thumb_set DMA1_Channel2_IRQHandler,Default_Handler - - .weak DMA1_Channel3_IRQHandler - .thumb_set DMA1_Channel3_IRQHandler,Default_Handler - - .weak DMA1_Channel4_IRQHandler - .thumb_set DMA1_Channel4_IRQHandler,Default_Handler - - .weak DMA1_Channel5_IRQHandler - .thumb_set DMA1_Channel5_IRQHandler,Default_Handler - - .weak DMA1_Channel6_IRQHandler - .thumb_set DMA1_Channel6_IRQHandler,Default_Handler - - .weak DMA1_Channel7_IRQHandler - .thumb_set DMA1_Channel7_IRQHandler,Default_Handler - - .weak ADC1_2_IRQHandler - .thumb_set ADC1_2_IRQHandler,Default_Handler - - .weak USB_HP_CAN1_TX_IRQHandler - .thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler - - .weak USB_LP_CAN1_RX0_IRQHandler - .thumb_set USB_LP_CAN1_RX0_IRQHandler,Default_Handler - - .weak CAN1_RX1_IRQHandler - .thumb_set CAN1_RX1_IRQHandler,Default_Handler - - .weak CAN1_SCE_IRQHandler - .thumb_set CAN1_SCE_IRQHandler,Default_Handler - - .weak EXTI9_5_IRQHandler - .thumb_set EXTI9_5_IRQHandler,Default_Handler - - .weak TIM1_BRK_IRQHandler - .thumb_set TIM1_BRK_IRQHandler,Default_Handler - - .weak TIM1_UP_IRQHandler - .thumb_set TIM1_UP_IRQHandler,Default_Handler - - .weak TIM1_TRG_COM_IRQHandler - .thumb_set TIM1_TRG_COM_IRQHandler,Default_Handler - - .weak TIM1_CC_IRQHandler - .thumb_set TIM1_CC_IRQHandler,Default_Handler - - .weak TIM2_IRQHandler - .thumb_set TIM2_IRQHandler,Default_Handler - - .weak TIM3_IRQHandler - .thumb_set TIM3_IRQHandler,Default_Handler - - .weak I2C1_EV_IRQHandler - .thumb_set I2C1_EV_IRQHandler,Default_Handler - - .weak I2C1_ER_IRQHandler - .thumb_set I2C1_ER_IRQHandler,Default_Handler - - .weak SPI1_IRQHandler - .thumb_set SPI1_IRQHandler,Default_Handler - - .weak USART1_IRQHandler - .thumb_set USART1_IRQHandler,Default_Handler - - .weak USART2_IRQHandler - .thumb_set USART2_IRQHandler,Default_Handler - - .weak EXTI15_10_IRQHandler - .thumb_set EXTI15_10_IRQHandler,Default_Handler - - .weak RTCAlarm_IRQHandler - .thumb_set RTCAlarm_IRQHandler,Default_Handler - - .weak USBWakeUp_IRQHandler - .thumb_set USBWakeUp_IRQHandler,Default_Handler - diff --git a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_md.s b/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_md.s deleted file mode 100644 index 28c8e26226..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/gcc/startup_stm32f10x_md.s +++ /dev/null @@ -1,371 +0,0 @@ -/** - ****************************************************************************** - * @file startup_stm32f10x_md.s - * @author MCD Application Team - * @version V3.1.2 - * @date 09/28/2009 - * @brief STM32F10x Medium Density Devices vector table for RIDE7 toolchain. - * This module performs: - * - Set the initial SP - * - Set the initial PC == Reset_Handler, - * - Set the vector table entries with the exceptions ISR address - * - Branches to main in the C library (which eventually - * calls main()). - * After Reset the Cortex-M3 processor is in Thread mode, - * priority is Privileged, and the Stack is set to Main. - ******************************************************************************* - * @copy - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2009 STMicroelectronics

- */ - - .syntax unified - .cpu cortex-m3 - .fpu softvfp - .thumb - -#include "stm32f10x.h" - -.global g_pfnVectors -.global Default_Handler - -/* start address for the initialization values of the .data section. -defined in linker script */ -.word _sidata -/* start address for the .data section. defined in linker script */ -.word _sdata -/* end address for the .data section. defined in linker script */ -.word _edata -/* start address for the .bss section. defined in linker script */ -.word _sbss -/* end address for the .bss section. defined in linker script */ -.word _ebss - -.equ BootRAM, 0xF108F85F -/** - * @brief This is the code that gets called when the processor first - * starts execution following a reset event. Only the absolutely - * necessary set is performed, after which the application - * supplied main() routine is called. - * @param None - * @retval : None -*/ - - .section .text.Reset_Handler - .weak Reset_Handler - .type Reset_Handler, %function -Reset_Handler: -/* Power the AFIO block - mov r1, #0x01 - movw r0, #(0x1000+0x18) - movt r0, #(0x4000+0x2) - str r1, [r0]*/ - -/* Disable the JTAG NJRST pin - mov r1, #0x01000000 - mov r0, #(0x0000+0x04) - movt r0, #(0x4000+0x1) - str r1, [r0]*/ - -/* Copy the data segment initializers from flash to SRAM */ - movs r1, #0 - b LoopCopyDataInit - -CopyDataInit: - ldr r3, =_sidata - ldr r3, [r3, r1] - str r3, [r0, r1] - adds r1, r1, #4 - -LoopCopyDataInit: - ldr r0, =_sdata - ldr r3, =_edata - adds r2, r0, r1 - cmp r2, r3 - bcc CopyDataInit - ldr r2, =_sbss - b LoopFillZerobss -/* Zero fill the bss segment. */ -FillZerobss: - movs r3, #0 - str r3, [r2], #4 - -LoopFillZerobss: - ldr r3, = _ebss - cmp r2, r3 - bcc FillZerobss - -/* Call the clock system intitialization function.*/ - bl SystemInit -/* Call the application's entry point.*/ - bl main - bx lr -.size Reset_Handler, .-Reset_Handler - -/** - * @brief This is the code that gets called when the processor receives an - * unexpected interrupt. This simply enters an infinite loop, preserving - * the system state for examination by a debugger. - * - * @param None - * @retval : None -*/ - .section .text.Default_Handler,"ax",%progbits -Default_Handler: -Infinite_Loop: - b Infinite_Loop - .size Default_Handler, .-Default_Handler -/****************************************************************************** -* -* The minimal vector table for a Cortex M3. Note that the proper constructs -* must be placed on this to ensure that it ends up at physical address -* 0x0000.0000. -* -******************************************************************************/ - .section .isr_vector,"a",%progbits - .type g_pfnVectors, %object - .size g_pfnVectors, .-g_pfnVectors - - -g_pfnVectors: - .word _estack - .word Reset_Handler - .word NMI_Handler - .word HardFault_Handler - .word MemManage_Handler - .word BusFault_Handler - .word UsageFault_Handler - .word 0 - .word 0 - .word 0 - .word 0 - .word SVC_Handler - .word DebugMon_Handler - .word 0 - .word PendSV_Handler - .word SysTick_Handler - .word WWDG_IRQHandler - .word PVD_IRQHandler - .word TAMPER_IRQHandler - .word RTC_IRQHandler - .word FLASH_IRQHandler - .word RCC_IRQHandler - .word EXTI0_IRQHandler - .word EXTI1_IRQHandler - .word EXTI2_IRQHandler - .word EXTI3_IRQHandler - .word EXTI4_IRQHandler - .word DMA1_Channel1_IRQHandler - .word DMA1_Channel2_IRQHandler - .word DMA1_Channel3_IRQHandler - .word DMA1_Channel4_IRQHandler - .word DMA1_Channel5_IRQHandler - .word DMA1_Channel6_IRQHandler - .word DMA1_Channel7_IRQHandler - .word ADC1_2_IRQHandler - .word USB_HP_CAN1_TX_IRQHandler - .word USB_LP_CAN1_RX0_IRQHandler - .word CAN1_RX1_IRQHandler - .word CAN1_SCE_IRQHandler - .word EXTI9_5_IRQHandler - .word TIM1_BRK_IRQHandler - .word TIM1_UP_IRQHandler - .word TIM1_TRG_COM_IRQHandler - .word TIM1_CC_IRQHandler - .word TIM2_IRQHandler - .word TIM3_IRQHandler - .word TIM4_IRQHandler - .word I2C1_EV_IRQHandler - .word I2C1_ER_IRQHandler - .word I2C2_EV_IRQHandler - .word I2C2_ER_IRQHandler - .word SPI1_IRQHandler - .word SPI2_IRQHandler - .word USART1_IRQHandler - .word USART2_IRQHandler - .word USART3_IRQHandler - .word EXTI15_10_IRQHandler - .word RTCAlarm_IRQHandler - .word USBWakeUp_IRQHandler - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word 0 - .word BootRAM /* @0x108. This is for boot in RAM mode for - STM32F10x Medium Density devices. */ - -/******************************************************************************* -* -* Provide weak aliases for each Exception handler to the Default_Handler. -* As they are weak aliases, any function with the same name will override -* this definition. -* -*******************************************************************************/ - - .weak NMI_Handler - .thumb_set NMI_Handler,Default_Handler - - .weak HardFault_Handler - .thumb_set HardFault_Handler,Default_Handler - - .weak MemManage_Handler - .thumb_set MemManage_Handler,Default_Handler - - .weak BusFault_Handler - .thumb_set BusFault_Handler,Default_Handler - - .weak UsageFault_Handler - .thumb_set UsageFault_Handler,Default_Handler - - .weak SVC_Handler - .thumb_set SVC_Handler,Default_Handler - - .weak DebugMon_Handler - .thumb_set DebugMon_Handler,Default_Handler - - .weak PendSV_Handler - .thumb_set PendSV_Handler,Default_Handler - - .weak SysTick_Handler - .thumb_set SysTick_Handler,Default_Handler - - .weak WWDG_IRQHandler - .thumb_set WWDG_IRQHandler,Default_Handler - - .weak PVD_IRQHandler - .thumb_set PVD_IRQHandler,Default_Handler - - .weak TAMPER_IRQHandler - .thumb_set TAMPER_IRQHandler,Default_Handler - - .weak RTC_IRQHandler - .thumb_set RTC_IRQHandler,Default_Handler - - .weak FLASH_IRQHandler - .thumb_set FLASH_IRQHandler,Default_Handler - - .weak RCC_IRQHandler - .thumb_set RCC_IRQHandler,Default_Handler - - .weak EXTI0_IRQHandler - .thumb_set EXTI0_IRQHandler,Default_Handler - - .weak EXTI1_IRQHandler - .thumb_set EXTI1_IRQHandler,Default_Handler - - .weak EXTI2_IRQHandler - .thumb_set EXTI2_IRQHandler,Default_Handler - - .weak EXTI3_IRQHandler - .thumb_set EXTI3_IRQHandler,Default_Handler - - .weak EXTI4_IRQHandler - .thumb_set EXTI4_IRQHandler,Default_Handler - - .weak DMA1_Channel1_IRQHandler - .thumb_set DMA1_Channel1_IRQHandler,Default_Handler - - .weak DMA1_Channel2_IRQHandler - .thumb_set DMA1_Channel2_IRQHandler,Default_Handler - - .weak DMA1_Channel3_IRQHandler - .thumb_set DMA1_Channel3_IRQHandler,Default_Handler - - .weak DMA1_Channel4_IRQHandler - .thumb_set DMA1_Channel4_IRQHandler,Default_Handler - - .weak DMA1_Channel5_IRQHandler - .thumb_set DMA1_Channel5_IRQHandler,Default_Handler - - .weak DMA1_Channel6_IRQHandler - .thumb_set DMA1_Channel6_IRQHandler,Default_Handler - - .weak DMA1_Channel7_IRQHandler - .thumb_set DMA1_Channel7_IRQHandler,Default_Handler - - .weak ADC1_2_IRQHandler - .thumb_set ADC1_2_IRQHandler,Default_Handler - - .weak USB_HP_CAN1_TX_IRQHandler - .thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler - - .weak USB_LP_CAN1_RX0_IRQHandler - .thumb_set USB_LP_CAN1_RX0_IRQHandler,Default_Handler - - .weak CAN1_RX1_IRQHandler - .thumb_set CAN1_RX1_IRQHandler,Default_Handler - - .weak CAN1_SCE_IRQHandler - .thumb_set CAN1_SCE_IRQHandler,Default_Handler - - .weak EXTI9_5_IRQHandler - .thumb_set EXTI9_5_IRQHandler,Default_Handler - - .weak TIM1_BRK_IRQHandler - .thumb_set TIM1_BRK_IRQHandler,Default_Handler - - .weak TIM1_UP_IRQHandler - .thumb_set TIM1_UP_IRQHandler,Default_Handler - - .weak TIM1_TRG_COM_IRQHandler - .thumb_set TIM1_TRG_COM_IRQHandler,Default_Handler - - .weak TIM1_CC_IRQHandler - .thumb_set TIM1_CC_IRQHandler,Default_Handler - - .weak TIM2_IRQHandler - .thumb_set TIM2_IRQHandler,Default_Handler - - .weak TIM3_IRQHandler - .thumb_set TIM3_IRQHandler,Default_Handler - - .weak TIM4_IRQHandler - .thumb_set TIM4_IRQHandler,Default_Handler - - .weak I2C1_EV_IRQHandler - .thumb_set I2C1_EV_IRQHandler,Default_Handler - - .weak I2C1_ER_IRQHandler - .thumb_set I2C1_ER_IRQHandler,Default_Handler - - .weak I2C2_EV_IRQHandler - .thumb_set I2C2_EV_IRQHandler,Default_Handler - - .weak I2C2_ER_IRQHandler - .thumb_set I2C2_ER_IRQHandler,Default_Handler - - .weak SPI1_IRQHandler - .thumb_set SPI1_IRQHandler,Default_Handler - - .weak SPI2_IRQHandler - .thumb_set SPI2_IRQHandler,Default_Handler - - .weak USART1_IRQHandler - .thumb_set USART1_IRQHandler,Default_Handler - - .weak USART2_IRQHandler - .thumb_set USART2_IRQHandler,Default_Handler - - .weak USART3_IRQHandler - .thumb_set USART3_IRQHandler,Default_Handler - - .weak EXTI15_10_IRQHandler - .thumb_set EXTI15_10_IRQHandler,Default_Handler - - .weak RTCAlarm_IRQHandler - .thumb_set RTCAlarm_IRQHandler,Default_Handler - - .weak USBWakeUp_IRQHandler - .thumb_set USBWakeUp_IRQHandler,Default_Handler - - diff --git a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_cl.s b/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_cl.s deleted file mode 100644 index 4594ebc527..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_cl.s +++ /dev/null @@ -1,498 +0,0 @@ -;/******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_cl.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x Connectivity line devices vector table for -;* EWARM5.x toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == __iar_program_start, -;* - Set the vector table entries with the exceptions ISR -;* address. -;* After Reset the Cortex-M3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;******************************************************************************** -;* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -;* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -;* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -;* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -;* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -;* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;*******************************************************************************/ -; -; -; The modules in this file are included in the libraries, and may be replaced -; by any user-defined modules that define the PUBLIC symbol _program_start or -; a user defined start symbol. -; To override the cstartup defined in the library, simply add your modified -; version to the workbench project. -; -; The vector table is normally located at address 0. -; When debugging in RAM, it can be located in RAM, aligned to at least 2^6. -; The name "__vector_table" has special meaning for C-SPY: -; it is where the SP start value is found, and the NVIC vector -; table register (VTOR) is initialized to this address if != 0. -; -; Cortex-M version -; - - MODULE ?cstartup - - ;; Forward declaration of sections. - SECTION CSTACK:DATA:NOROOT(3) - - SECTION .intvec:CODE:NOROOT(2) - - EXTERN __iar_program_start - PUBLIC __vector_table - - DATA -__vector_table - DCD sfe(CSTACK) - DCD __iar_program_start - - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1 and ADC2 - DCD CAN1_TX_IRQHandler ; CAN1 TX - DCD CAN1_RX0_IRQHandler ; CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C1 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD USART3_IRQHandler ; USART3 - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC alarm through EXTI line - DCD OTG_FS_WKUP_IRQHandler ; USB OTG FS Wakeup through EXTI line - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD TIM5_IRQHandler ; TIM5 - DCD SPI3_IRQHandler ; SPI3 - DCD UART4_IRQHandler ; UART4 - DCD UART5_IRQHandler ; UART5 - DCD TIM6_IRQHandler ; TIM6 - DCD TIM7_IRQHandler ; TIM7 - DCD DMA2_Channel1_IRQHandler ; DMA2 Channel1 - DCD DMA2_Channel2_IRQHandler ; DMA2 Channel2 - DCD DMA2_Channel3_IRQHandler ; DMA2 Channel3 - DCD DMA2_Channel4_IRQHandler ; DMA2 Channel4 - DCD DMA2_Channel5_IRQHandler ; DMA2 Channel5 - DCD ETH_IRQHandler ; Ethernet - DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line - DCD CAN2_TX_IRQHandler ; CAN2 TX - DCD CAN2_RX0_IRQHandler ; CAN2 RX0 - DCD CAN2_RX1_IRQHandler ; CAN2 RX1 - DCD CAN2_SCE_IRQHandler ; CAN2 SCE - DCD OTG_FS_IRQHandler ; USB OTG FS - -;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -;; -;; Default interrupt handlers. -;; - THUMB - - PUBWEAK NMI_Handler - SECTION .text:CODE:REORDER(1) -NMI_Handler - B NMI_Handler - - PUBWEAK HardFault_Handler - SECTION .text:CODE:REORDER(1) -HardFault_Handler - B HardFault_Handler - - PUBWEAK MemManage_Handler - SECTION .text:CODE:REORDER(1) -MemManage_Handler - B MemManage_Handler - - PUBWEAK BusFault_Handler - SECTION .text:CODE:REORDER(1) -BusFault_Handler - B BusFault_Handler - - PUBWEAK UsageFault_Handler - SECTION .text:CODE:REORDER(1) -UsageFault_Handler - B UsageFault_Handler - - PUBWEAK SVC_Handler - SECTION .text:CODE:REORDER(1) -SVC_Handler - B SVC_Handler - - PUBWEAK DebugMon_Handler - SECTION .text:CODE:REORDER(1) -DebugMon_Handler - B DebugMon_Handler - - PUBWEAK PendSV_Handler - SECTION .text:CODE:REORDER(1) -PendSV_Handler - B PendSV_Handler - - PUBWEAK SysTick_Handler - SECTION .text:CODE:REORDER(1) -SysTick_Handler - B SysTick_Handler - - PUBWEAK WWDG_IRQHandler - SECTION .text:CODE:REORDER(1) -WWDG_IRQHandler - B WWDG_IRQHandler - - PUBWEAK PVD_IRQHandler - SECTION .text:CODE:REORDER(1) -PVD_IRQHandler - B PVD_IRQHandler - - PUBWEAK TAMPER_IRQHandler - SECTION .text:CODE:REORDER(1) -TAMPER_IRQHandler - B TAMPER_IRQHandler - - PUBWEAK RTC_IRQHandler - SECTION .text:CODE:REORDER(1) -RTC_IRQHandler - B RTC_IRQHandler - - PUBWEAK FLASH_IRQHandler - SECTION .text:CODE:REORDER(1) -FLASH_IRQHandler - B FLASH_IRQHandler - - PUBWEAK RCC_IRQHandler - SECTION .text:CODE:REORDER(1) -RCC_IRQHandler - B RCC_IRQHandler - - PUBWEAK EXTI0_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI0_IRQHandler - B EXTI0_IRQHandler - - PUBWEAK EXTI1_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI1_IRQHandler - B EXTI1_IRQHandler - - PUBWEAK EXTI2_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI2_IRQHandler - B EXTI2_IRQHandler - - PUBWEAK EXTI3_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI3_IRQHandler - B EXTI3_IRQHandler - - - PUBWEAK EXTI4_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI4_IRQHandler - B EXTI4_IRQHandler - - PUBWEAK DMA1_Channel1_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel1_IRQHandler - B DMA1_Channel1_IRQHandler - - PUBWEAK DMA1_Channel2_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel2_IRQHandler - B DMA1_Channel2_IRQHandler - - PUBWEAK DMA1_Channel3_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel3_IRQHandler - B DMA1_Channel3_IRQHandler - - PUBWEAK DMA1_Channel4_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel4_IRQHandler - B DMA1_Channel4_IRQHandler - - PUBWEAK DMA1_Channel5_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel5_IRQHandler - B DMA1_Channel5_IRQHandler - - PUBWEAK DMA1_Channel6_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel6_IRQHandler - B DMA1_Channel6_IRQHandler - - PUBWEAK DMA1_Channel7_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel7_IRQHandler - B DMA1_Channel7_IRQHandler - - PUBWEAK ADC1_2_IRQHandler - SECTION .text:CODE:REORDER(1) -ADC1_2_IRQHandler - B ADC1_2_IRQHandler - - PUBWEAK CAN1_TX_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_TX_IRQHandler - B CAN1_TX_IRQHandler - - PUBWEAK CAN1_RX0_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_RX0_IRQHandler - B CAN1_RX0_IRQHandler - - PUBWEAK CAN1_RX1_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_RX1_IRQHandler - B CAN1_RX1_IRQHandler - - PUBWEAK CAN1_SCE_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_SCE_IRQHandler - B CAN1_SCE_IRQHandler - - PUBWEAK EXTI9_5_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI9_5_IRQHandler - B EXTI9_5_IRQHandler - - PUBWEAK TIM1_BRK_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_BRK_IRQHandler - B TIM1_BRK_IRQHandler - - PUBWEAK TIM1_UP_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_UP_IRQHandler - B TIM1_UP_IRQHandler - - PUBWEAK TIM1_TRG_COM_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_TRG_COM_IRQHandler - B TIM1_TRG_COM_IRQHandler - - PUBWEAK TIM1_CC_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_CC_IRQHandler - B TIM1_CC_IRQHandler - - PUBWEAK TIM2_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM2_IRQHandler - B TIM2_IRQHandler - - PUBWEAK TIM3_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM3_IRQHandler - B TIM3_IRQHandler - - PUBWEAK TIM4_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM4_IRQHandler - B TIM4_IRQHandler - - PUBWEAK I2C1_EV_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_EV_IRQHandler - B I2C1_EV_IRQHandler - - PUBWEAK I2C1_ER_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_ER_IRQHandler - B I2C1_ER_IRQHandler - - PUBWEAK I2C2_EV_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C2_EV_IRQHandler - B I2C2_EV_IRQHandler - - PUBWEAK I2C2_ER_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C2_ER_IRQHandler - B I2C2_ER_IRQHandler - - PUBWEAK SPI1_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI1_IRQHandler - B SPI1_IRQHandler - - PUBWEAK SPI2_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI2_IRQHandler - B SPI2_IRQHandler - - PUBWEAK USART1_IRQHandler - SECTION .text:CODE:REORDER(1) -USART1_IRQHandler - B USART1_IRQHandler - - PUBWEAK USART2_IRQHandler - SECTION .text:CODE:REORDER(1) -USART2_IRQHandler - B USART2_IRQHandler - - PUBWEAK USART3_IRQHandler - SECTION .text:CODE:REORDER(1) -USART3_IRQHandler - B USART3_IRQHandler - - PUBWEAK EXTI15_10_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI15_10_IRQHandler - B EXTI15_10_IRQHandler - - PUBWEAK RTCAlarm_IRQHandler - SECTION .text:CODE:REORDER(1) -RTCAlarm_IRQHandler - B RTCAlarm_IRQHandler - - PUBWEAK OTG_FS_WKUP_IRQHandler - SECTION .text:CODE:REORDER(1) -OTG_FS_WKUP_IRQHandler - B OTG_FS_WKUP_IRQHandler - - PUBWEAK TIM5_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM5_IRQHandler - B TIM5_IRQHandler - - PUBWEAK SPI3_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI3_IRQHandler - B SPI3_IRQHandler - - PUBWEAK UART4_IRQHandler - SECTION .text:CODE:REORDER(1) -UART4_IRQHandler - B UART4_IRQHandler - - PUBWEAK UART5_IRQHandler - SECTION .text:CODE:REORDER(1) -UART5_IRQHandler - B UART5_IRQHandler - - PUBWEAK TIM6_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM6_IRQHandler - B TIM6_IRQHandler - - PUBWEAK TIM7_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM7_IRQHandler - B TIM7_IRQHandler - - PUBWEAK DMA2_Channel1_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel1_IRQHandler - B DMA2_Channel1_IRQHandler - - PUBWEAK DMA2_Channel2_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel2_IRQHandler - B DMA2_Channel2_IRQHandler - - PUBWEAK DMA2_Channel3_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel3_IRQHandler - B DMA2_Channel3_IRQHandler - - PUBWEAK DMA2_Channel4_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel4_IRQHandler - B DMA2_Channel4_IRQHandler - - PUBWEAK DMA2_Channel5_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel5_IRQHandler - B DMA2_Channel5_IRQHandler - - PUBWEAK ETH_IRQHandler - SECTION .text:CODE:REORDER(1) -ETH_IRQHandler - B ETH_IRQHandler - - PUBWEAK ETH_WKUP_IRQHandler - SECTION .text:CODE:REORDER(1) -ETH_WKUP_IRQHandler - B ETH_WKUP_IRQHandler - - PUBWEAK CAN2_TX_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN2_TX_IRQHandler - B CAN2_TX_IRQHandler - - PUBWEAK CAN2_RX0_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN2_RX0_IRQHandler - B CAN2_RX0_IRQHandler - - PUBWEAK CAN2_RX1_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN2_RX1_IRQHandler - B CAN2_RX1_IRQHandler - - PUBWEAK CAN2_SCE_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN2_SCE_IRQHandler - B CAN2_SCE_IRQHandler - - PUBWEAK OTG_FS_IRQHandler - SECTION .text:CODE:REORDER(1) -OTG_FS_IRQHandler - B OTG_FS_IRQHandler - - END -/******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/ diff --git a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_hd.s b/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_hd.s deleted file mode 100644 index 92cf6fb6fc..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_hd.s +++ /dev/null @@ -1,510 +0,0 @@ -;/******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_hd.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x High Density Devices vector table for EWARM5.x -;* toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == __iar_program_start, -;* - Set the vector table entries with the exceptions ISR address, -;* - Configure external SRAM mounted on STM3210E-EVAL board -;* to be used as data memory (optional, to be enabled by user) -;* After Reset the Cortex-M3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;******************************************************************************** -;* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -;* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -;* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -;* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -;* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -;* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;*******************************************************************************/ -; -; -; The modules in this file are included in the libraries, and may be replaced -; by any user-defined modules that define the PUBLIC symbol _program_start or -; a user defined start symbol. -; To override the cstartup defined in the library, simply add your modified -; version to the workbench project. -; -; The vector table is normally located at address 0. -; When debugging in RAM, it can be located in RAM, aligned to at least 2^6. -; The name "__vector_table" has special meaning for C-SPY: -; it is where the SP start value is found, and the NVIC vector -; table register (VTOR) is initialized to this address if != 0. -; -; Cortex-M version -; - - MODULE ?cstartup - - ;; ICODE is the same segment as cstartup. By placing __low_level_init - ;; in the same segment, we make sure it can be reached with BL. */ - - SECTION CSTACK:DATA:NOROOT(3) - SECTION .icode:CODE:NOROOT(2) - PUBLIC __low_level_init - - PUBWEAK SystemInit_ExtMemCtl - SECTION .text:CODE:REORDER(2) - THUMB -SystemInit_ExtMemCtl - BX LR - -__low_level_init: - - ;; Initialize hardware. - LDR R0, = SystemInit_ExtMemCtl ; initialize external memory controller - MOV R11, LR - BLX R0 - LDR R1, =sfe(CSTACK) ; restore original stack pointer - MSR MSP, R1 - MOV R0,#1 - ;; Return with BX to be independent of mode of caller - BX R11 - - ;; Forward declaration of sections. - SECTION .intvec:CODE:NOROOT(2) - - EXTERN __iar_program_start - PUBLIC __vector_table - - DATA -__intial_sp EQU 0x20000400 -__vector_table - DCD __intial_sp - DCD __iar_program_start - - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1 & ADC2 - DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX - DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C2 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD USART3_IRQHandler ; USART3 - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line - DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend - DCD TIM8_BRK_IRQHandler ; TIM8 Break - DCD TIM8_UP_IRQHandler ; TIM8 Update - DCD TIM8_TRG_COM_IRQHandler ; TIM8 Trigger and Commutation - DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare - DCD ADC3_IRQHandler ; ADC3 - DCD FSMC_IRQHandler ; FSMC - DCD SDIO_IRQHandler ; SDIO - DCD TIM5_IRQHandler ; TIM5 - DCD SPI3_IRQHandler ; SPI3 - DCD UART4_IRQHandler ; UART4 - DCD UART5_IRQHandler ; UART5 - DCD TIM6_IRQHandler ; TIM6 - DCD TIM7_IRQHandler ; TIM7 - DCD DMA2_Channel1_IRQHandler ; DMA2 Channel1 - DCD DMA2_Channel2_IRQHandler ; DMA2 Channel2 - DCD DMA2_Channel3_IRQHandler ; DMA2 Channel3 - DCD DMA2_Channel4_5_IRQHandler ; DMA2 Channel4 & Channel5 -;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -;; -;; Default interrupt handlers. -;; - THUMB - - PUBWEAK NMI_Handler - SECTION .text:CODE:REORDER(1) -NMI_Handler - B NMI_Handler - - PUBWEAK HardFault_Handler - SECTION .text:CODE:REORDER(1) -HardFault_Handler - B HardFault_Handler - - PUBWEAK MemManage_Handler - SECTION .text:CODE:REORDER(1) -MemManage_Handler - B MemManage_Handler - - PUBWEAK BusFault_Handler - SECTION .text:CODE:REORDER(1) -BusFault_Handler - B BusFault_Handler - - PUBWEAK UsageFault_Handler - SECTION .text:CODE:REORDER(1) -UsageFault_Handler - B UsageFault_Handler - - PUBWEAK SVC_Handler - SECTION .text:CODE:REORDER(1) -SVC_Handler - B SVC_Handler - - PUBWEAK DebugMon_Handler - SECTION .text:CODE:REORDER(1) -DebugMon_Handler - B DebugMon_Handler - - PUBWEAK PendSV_Handler - SECTION .text:CODE:REORDER(1) -PendSV_Handler - B PendSV_Handler - - PUBWEAK SysTick_Handler - SECTION .text:CODE:REORDER(1) -SysTick_Handler - B SysTick_Handler - - PUBWEAK WWDG_IRQHandler - SECTION .text:CODE:REORDER(1) -WWDG_IRQHandler - B WWDG_IRQHandler - - PUBWEAK PVD_IRQHandler - SECTION .text:CODE:REORDER(1) -PVD_IRQHandler - B PVD_IRQHandler - - PUBWEAK TAMPER_IRQHandler - SECTION .text:CODE:REORDER(1) -TAMPER_IRQHandler - B TAMPER_IRQHandler - - PUBWEAK RTC_IRQHandler - SECTION .text:CODE:REORDER(1) -RTC_IRQHandler - B RTC_IRQHandler - - PUBWEAK FLASH_IRQHandler - SECTION .text:CODE:REORDER(1) -FLASH_IRQHandler - B FLASH_IRQHandler - - PUBWEAK RCC_IRQHandler - SECTION .text:CODE:REORDER(1) -RCC_IRQHandler - B RCC_IRQHandler - - PUBWEAK EXTI0_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI0_IRQHandler - B EXTI0_IRQHandler - - PUBWEAK EXTI1_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI1_IRQHandler - B EXTI1_IRQHandler - - PUBWEAK EXTI2_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI2_IRQHandler - B EXTI2_IRQHandler - - PUBWEAK EXTI3_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI3_IRQHandler - B EXTI3_IRQHandler - - PUBWEAK EXTI4_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI4_IRQHandler - B EXTI4_IRQHandler - - PUBWEAK DMA1_Channel1_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel1_IRQHandler - B DMA1_Channel1_IRQHandler - - PUBWEAK DMA1_Channel2_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel2_IRQHandler - B DMA1_Channel2_IRQHandler - - PUBWEAK DMA1_Channel3_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel3_IRQHandler - B DMA1_Channel3_IRQHandler - - PUBWEAK DMA1_Channel4_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel4_IRQHandler - B DMA1_Channel4_IRQHandler - - PUBWEAK DMA1_Channel5_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel5_IRQHandler - B DMA1_Channel5_IRQHandler - - PUBWEAK DMA1_Channel6_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel6_IRQHandler - B DMA1_Channel6_IRQHandler - - PUBWEAK DMA1_Channel7_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel7_IRQHandler - B DMA1_Channel7_IRQHandler - - PUBWEAK ADC1_2_IRQHandler - SECTION .text:CODE:REORDER(1) -ADC1_2_IRQHandler - B ADC1_2_IRQHandler - - PUBWEAK USB_HP_CAN1_TX_IRQHandler - SECTION .text:CODE:REORDER(1) -USB_HP_CAN1_TX_IRQHandler - B USB_HP_CAN1_TX_IRQHandler - - PUBWEAK USB_LP_CAN1_RX0_IRQHandler - SECTION .text:CODE:REORDER(1) -USB_LP_CAN1_RX0_IRQHandler - B USB_LP_CAN1_RX0_IRQHandler - - PUBWEAK CAN1_RX1_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_RX1_IRQHandler - B CAN1_RX1_IRQHandler - - PUBWEAK CAN1_SCE_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_SCE_IRQHandler - B CAN1_SCE_IRQHandler - - PUBWEAK EXTI9_5_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI9_5_IRQHandler - B EXTI9_5_IRQHandler - - PUBWEAK TIM1_BRK_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_BRK_IRQHandler - B TIM1_BRK_IRQHandler - - PUBWEAK TIM1_UP_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_UP_IRQHandler - B TIM1_UP_IRQHandler - - PUBWEAK TIM1_TRG_COM_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_TRG_COM_IRQHandler - B TIM1_TRG_COM_IRQHandler - - PUBWEAK TIM1_CC_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_CC_IRQHandler - B TIM1_CC_IRQHandler - - PUBWEAK TIM2_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM2_IRQHandler - B TIM2_IRQHandler - - PUBWEAK TIM3_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM3_IRQHandler - B TIM3_IRQHandler - - PUBWEAK TIM4_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM4_IRQHandler - B TIM4_IRQHandler - - PUBWEAK I2C1_EV_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_EV_IRQHandler - B I2C1_EV_IRQHandler - - PUBWEAK I2C1_ER_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_ER_IRQHandler - B I2C1_ER_IRQHandler - - PUBWEAK I2C2_EV_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C2_EV_IRQHandler - B I2C2_EV_IRQHandler - - PUBWEAK I2C2_ER_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C2_ER_IRQHandler - B I2C2_ER_IRQHandler - - PUBWEAK SPI1_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI1_IRQHandler - B SPI1_IRQHandler - - PUBWEAK SPI2_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI2_IRQHandler - B SPI2_IRQHandler - - PUBWEAK USART1_IRQHandler - SECTION .text:CODE:REORDER(1) -USART1_IRQHandler - B USART1_IRQHandler - - PUBWEAK USART2_IRQHandler - SECTION .text:CODE:REORDER(1) -USART2_IRQHandler - B USART2_IRQHandler - - PUBWEAK USART3_IRQHandler - SECTION .text:CODE:REORDER(1) -USART3_IRQHandler - B USART3_IRQHandler - - PUBWEAK EXTI15_10_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI15_10_IRQHandler - B EXTI15_10_IRQHandler - - PUBWEAK RTCAlarm_IRQHandler - SECTION .text:CODE:REORDER(1) -RTCAlarm_IRQHandler - B RTCAlarm_IRQHandler - - PUBWEAK USBWakeUp_IRQHandler - SECTION .text:CODE:REORDER(1) -USBWakeUp_IRQHandler - B USBWakeUp_IRQHandler - - PUBWEAK TIM8_BRK_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM8_BRK_IRQHandler - B TIM8_BRK_IRQHandler - - PUBWEAK TIM8_UP_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM8_UP_IRQHandler - B TIM8_UP_IRQHandler - - PUBWEAK TIM8_TRG_COM_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM8_TRG_COM_IRQHandler - B TIM8_TRG_COM_IRQHandler - - PUBWEAK TIM8_CC_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM8_CC_IRQHandler - B TIM8_CC_IRQHandler - - PUBWEAK ADC3_IRQHandler - SECTION .text:CODE:REORDER(1) -ADC3_IRQHandler - B ADC3_IRQHandler - - PUBWEAK FSMC_IRQHandler - SECTION .text:CODE:REORDER(1) -FSMC_IRQHandler - B FSMC_IRQHandler - - PUBWEAK SDIO_IRQHandler - SECTION .text:CODE:REORDER(1) -SDIO_IRQHandler - B SDIO_IRQHandler - - PUBWEAK TIM5_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM5_IRQHandler - B TIM5_IRQHandler - - PUBWEAK SPI3_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI3_IRQHandler - B SPI3_IRQHandler - - PUBWEAK UART4_IRQHandler - SECTION .text:CODE:REORDER(1) -UART4_IRQHandler - B UART4_IRQHandler - - PUBWEAK UART5_IRQHandler - SECTION .text:CODE:REORDER(1) -UART5_IRQHandler - B UART5_IRQHandler - - PUBWEAK TIM6_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM6_IRQHandler - B TIM6_IRQHandler - - PUBWEAK TIM7_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM7_IRQHandler - B TIM7_IRQHandler - - PUBWEAK DMA2_Channel1_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel1_IRQHandler - B DMA2_Channel1_IRQHandler - - PUBWEAK DMA2_Channel2_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel2_IRQHandler - B DMA2_Channel2_IRQHandler - - PUBWEAK DMA2_Channel3_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel3_IRQHandler - B DMA2_Channel3_IRQHandler - - PUBWEAK DMA2_Channel4_5_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA2_Channel4_5_IRQHandler - B DMA2_Channel4_5_IRQHandler - - - END - -/******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/ diff --git a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_ld.s b/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_ld.s deleted file mode 100644 index e3da4a7be2..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_ld.s +++ /dev/null @@ -1,357 +0,0 @@ -;/******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_ld.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x Low Density Devices vector table for EWARM5.x -;* toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == __iar_program_start, -;* - Set the vector table entries with the exceptions ISR -;* address. -;* After Reset the Cortex-M3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;******************************************************************************** -;* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -;* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -;* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -;* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -;* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -;* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;*******************************************************************************/ -; -; -; The modules in this file are included in the libraries, and may be replaced -; by any user-defined modules that define the PUBLIC symbol _program_start or -; a user defined start symbol. -; To override the cstartup defined in the library, simply add your modified -; version to the workbench project. -; -; The vector table is normally located at address 0. -; When debugging in RAM, it can be located in RAM, aligned to at least 2^6. -; The name "__vector_table" has special meaning for C-SPY: -; it is where the SP start value is found, and the NVIC vector -; table register (VTOR) is initialized to this address if != 0. -; -; Cortex-M version -; - - MODULE ?cstartup - - ;; Forward declaration of sections. - SECTION CSTACK:DATA:NOROOT(3) - - SECTION .intvec:CODE:NOROOT(2) - - EXTERN __iar_program_start - PUBLIC __vector_table - - DATA -__vector_table - DCD sfe(CSTACK) - DCD __iar_program_start - - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1 & ADC2 - DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX - DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD 0 ; Reserved - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SPI1_IRQHandler ; SPI1 - DCD 0 ; Reserved - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD 0 ; Reserved - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line - DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend - -;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -;; -;; Default interrupt handlers. -;; - THUMB - - PUBWEAK NMI_Handler - SECTION .text:CODE:REORDER(1) -NMI_Handler - B NMI_Handler - - PUBWEAK HardFault_Handler - SECTION .text:CODE:REORDER(1) -HardFault_Handler - B HardFault_Handler - - PUBWEAK MemManage_Handler - SECTION .text:CODE:REORDER(1) -MemManage_Handler - B MemManage_Handler - - PUBWEAK BusFault_Handler - SECTION .text:CODE:REORDER(1) -BusFault_Handler - B BusFault_Handler - - PUBWEAK UsageFault_Handler - SECTION .text:CODE:REORDER(1) -UsageFault_Handler - B UsageFault_Handler - - PUBWEAK SVC_Handler - SECTION .text:CODE:REORDER(1) -SVC_Handler - B SVC_Handler - - PUBWEAK DebugMon_Handler - SECTION .text:CODE:REORDER(1) -DebugMon_Handler - B DebugMon_Handler - - PUBWEAK PendSV_Handler - SECTION .text:CODE:REORDER(1) -PendSV_Handler - B PendSV_Handler - - PUBWEAK SysTick_Handler - SECTION .text:CODE:REORDER(1) -SysTick_Handler - B SysTick_Handler - - PUBWEAK WWDG_IRQHandler - SECTION .text:CODE:REORDER(1) -WWDG_IRQHandler - B WWDG_IRQHandler - - PUBWEAK PVD_IRQHandler - SECTION .text:CODE:REORDER(1) -PVD_IRQHandler - B PVD_IRQHandler - - PUBWEAK TAMPER_IRQHandler - SECTION .text:CODE:REORDER(1) -TAMPER_IRQHandler - B TAMPER_IRQHandler - - PUBWEAK RTC_IRQHandler - SECTION .text:CODE:REORDER(1) -RTC_IRQHandler - B RTC_IRQHandler - - PUBWEAK FLASH_IRQHandler - SECTION .text:CODE:REORDER(1) -FLASH_IRQHandler - B FLASH_IRQHandler - - PUBWEAK RCC_IRQHandler - SECTION .text:CODE:REORDER(1) -RCC_IRQHandler - B RCC_IRQHandler - - PUBWEAK EXTI0_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI0_IRQHandler - B EXTI0_IRQHandler - - PUBWEAK EXTI1_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI1_IRQHandler - B EXTI1_IRQHandler - - PUBWEAK EXTI2_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI2_IRQHandler - B EXTI2_IRQHandler - - PUBWEAK EXTI3_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI3_IRQHandler - B EXTI3_IRQHandler - - PUBWEAK EXTI4_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI4_IRQHandler - B EXTI4_IRQHandler - - PUBWEAK DMA1_Channel1_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel1_IRQHandler - B DMA1_Channel1_IRQHandler - - PUBWEAK DMA1_Channel2_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel2_IRQHandler - B DMA1_Channel2_IRQHandler - - PUBWEAK DMA1_Channel3_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel3_IRQHandler - B DMA1_Channel3_IRQHandler - - PUBWEAK DMA1_Channel4_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel4_IRQHandler - B DMA1_Channel4_IRQHandler - - PUBWEAK DMA1_Channel5_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel5_IRQHandler - B DMA1_Channel5_IRQHandler - - PUBWEAK DMA1_Channel6_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel6_IRQHandler - B DMA1_Channel6_IRQHandler - - PUBWEAK DMA1_Channel7_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel7_IRQHandler - B DMA1_Channel7_IRQHandler - - PUBWEAK ADC1_2_IRQHandler - SECTION .text:CODE:REORDER(1) -ADC1_2_IRQHandler - B ADC1_2_IRQHandler - - PUBWEAK USB_HP_CAN1_TX_IRQHandler - SECTION .text:CODE:REORDER(1) -USB_HP_CAN1_TX_IRQHandler - B USB_HP_CAN1_TX_IRQHandler - - PUBWEAK USB_LP_CAN1_RX0_IRQHandler - SECTION .text:CODE:REORDER(1) -USB_LP_CAN1_RX0_IRQHandler - B USB_LP_CAN1_RX0_IRQHandler - - PUBWEAK CAN1_RX1_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_RX1_IRQHandler - B CAN1_RX1_IRQHandler - - PUBWEAK CAN1_SCE_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_SCE_IRQHandler - B CAN1_SCE_IRQHandler - - PUBWEAK EXTI9_5_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI9_5_IRQHandler - B EXTI9_5_IRQHandler - - PUBWEAK TIM1_BRK_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_BRK_IRQHandler - B TIM1_BRK_IRQHandler - - PUBWEAK TIM1_UP_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_UP_IRQHandler - B TIM1_UP_IRQHandler - - PUBWEAK TIM1_TRG_COM_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_TRG_COM_IRQHandler - B TIM1_TRG_COM_IRQHandler - - PUBWEAK TIM1_CC_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_CC_IRQHandler - B TIM1_CC_IRQHandler - - PUBWEAK TIM2_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM2_IRQHandler - B TIM2_IRQHandler - - PUBWEAK TIM3_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM3_IRQHandler - B TIM3_IRQHandler - - PUBWEAK I2C1_EV_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_EV_IRQHandler - B I2C1_EV_IRQHandler - - PUBWEAK I2C1_ER_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_ER_IRQHandler - B I2C1_ER_IRQHandler - - PUBWEAK SPI1_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI1_IRQHandler - B SPI1_IRQHandler - - PUBWEAK USART1_IRQHandler - SECTION .text:CODE:REORDER(1) -USART1_IRQHandler - B USART1_IRQHandler - - PUBWEAK USART2_IRQHandler - SECTION .text:CODE:REORDER(1) -USART2_IRQHandler - B USART2_IRQHandler - - PUBWEAK EXTI15_10_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI15_10_IRQHandler - B EXTI15_10_IRQHandler - - PUBWEAK RTCAlarm_IRQHandler - SECTION .text:CODE:REORDER(1) -RTCAlarm_IRQHandler - B RTCAlarm_IRQHandler - - PUBWEAK USBWakeUp_IRQHandler - SECTION .text:CODE:REORDER(1) -USBWakeUp_IRQHandler - B USBWakeUp_IRQHandler - - END -/******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/ diff --git a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_md.s b/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_md.s deleted file mode 100644 index 32a28a1c9e..0000000000 --- a/src/lib/CMSIS/Core/CM3/startup/iar/startup_stm32f10x_md.s +++ /dev/null @@ -1,382 +0,0 @@ -;/******************** (C) COPYRIGHT 2009 STMicroelectronics ******************** -;* File Name : startup_stm32f10x_md.s -;* Author : MCD Application Team -;* Version : V3.1.2 -;* Date : 09/28/2009 -;* Description : STM32F10x Medium Density Devices vector table for -;* EWARM5.x toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == __iar_program_start, -;* - Set the vector table entries with the exceptions ISR -;* address. -;* After Reset the Cortex-M3 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;******************************************************************************** -;* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -;* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -;* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -;* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -;* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -;* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -;*******************************************************************************/ -; -; -; The modules in this file are included in the libraries, and may be replaced -; by any user-defined modules that define the PUBLIC symbol _program_start or -; a user defined start symbol. -; To override the cstartup defined in the library, simply add your modified -; version to the workbench project. -; -; The vector table is normally located at address 0. -; When debugging in RAM, it can be located in RAM, aligned to at least 2^6. -; The name "__vector_table" has special meaning for C-SPY: -; it is where the SP start value is found, and the NVIC vector -; table register (VTOR) is initialized to this address if != 0. -; -; Cortex-M version -; - - MODULE ?cstartup - - ;; Forward declaration of sections. - SECTION CSTACK:DATA:NOROOT(3) - - SECTION .intvec:CODE:NOROOT(2) - - EXTERN __iar_program_start - PUBLIC __vector_table - - DATA -__vector_table - DCD sfe(CSTACK) - DCD __iar_program_start - - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window Watchdog - DCD PVD_IRQHandler ; PVD through EXTI Line detect - DCD TAMPER_IRQHandler ; Tamper - DCD RTC_IRQHandler ; RTC - DCD FLASH_IRQHandler ; Flash - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line 0 - DCD EXTI1_IRQHandler ; EXTI Line 1 - DCD EXTI2_IRQHandler ; EXTI Line 2 - DCD EXTI3_IRQHandler ; EXTI Line 3 - DCD EXTI4_IRQHandler ; EXTI Line 4 - DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1 - DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2 - DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3 - DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4 - DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5 - DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6 - DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7 - DCD ADC1_2_IRQHandler ; ADC1 & ADC2 - DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX - DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0 - DCD CAN1_RX1_IRQHandler ; CAN1 RX1 - DCD CAN1_SCE_IRQHandler ; CAN1 SCE - DCD EXTI9_5_IRQHandler ; EXTI Line 9..5 - DCD TIM1_BRK_IRQHandler ; TIM1 Break - DCD TIM1_UP_IRQHandler ; TIM1 Update - DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C2 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD USART3_IRQHandler ; USART3 - DCD EXTI15_10_IRQHandler ; EXTI Line 15..10 - DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line - DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend - -;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -;; -;; Default interrupt handlers. -;; - THUMB - - PUBWEAK NMI_Handler - SECTION .text:CODE:REORDER(1) -NMI_Handler - B NMI_Handler - - PUBWEAK HardFault_Handler - SECTION .text:CODE:REORDER(1) -HardFault_Handler - B HardFault_Handler - - PUBWEAK MemManage_Handler - SECTION .text:CODE:REORDER(1) -MemManage_Handler - B MemManage_Handler - - PUBWEAK BusFault_Handler - SECTION .text:CODE:REORDER(1) -BusFault_Handler - B BusFault_Handler - - PUBWEAK UsageFault_Handler - SECTION .text:CODE:REORDER(1) -UsageFault_Handler - B UsageFault_Handler - - PUBWEAK SVC_Handler - SECTION .text:CODE:REORDER(1) -SVC_Handler - B SVC_Handler - - PUBWEAK DebugMon_Handler - SECTION .text:CODE:REORDER(1) -DebugMon_Handler - B DebugMon_Handler - - PUBWEAK PendSV_Handler - SECTION .text:CODE:REORDER(1) -PendSV_Handler - B PendSV_Handler - - PUBWEAK SysTick_Handler - SECTION .text:CODE:REORDER(1) -SysTick_Handler - B SysTick_Handler - - PUBWEAK WWDG_IRQHandler - SECTION .text:CODE:REORDER(1) -WWDG_IRQHandler - B WWDG_IRQHandler - - PUBWEAK PVD_IRQHandler - SECTION .text:CODE:REORDER(1) -PVD_IRQHandler - B PVD_IRQHandler - - PUBWEAK TAMPER_IRQHandler - SECTION .text:CODE:REORDER(1) -TAMPER_IRQHandler - B TAMPER_IRQHandler - - PUBWEAK RTC_IRQHandler - SECTION .text:CODE:REORDER(1) -RTC_IRQHandler - B RTC_IRQHandler - - PUBWEAK FLASH_IRQHandler - SECTION .text:CODE:REORDER(1) -FLASH_IRQHandler - B FLASH_IRQHandler - - PUBWEAK RCC_IRQHandler - SECTION .text:CODE:REORDER(1) -RCC_IRQHandler - B RCC_IRQHandler - - PUBWEAK EXTI0_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI0_IRQHandler - B EXTI0_IRQHandler - - PUBWEAK EXTI1_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI1_IRQHandler - B EXTI1_IRQHandler - - PUBWEAK EXTI2_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI2_IRQHandler - B EXTI2_IRQHandler - - PUBWEAK EXTI3_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI3_IRQHandler - B EXTI3_IRQHandler - - PUBWEAK EXTI4_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI4_IRQHandler - B EXTI4_IRQHandler - - PUBWEAK DMA1_Channel1_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel1_IRQHandler - B DMA1_Channel1_IRQHandler - - PUBWEAK DMA1_Channel2_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel2_IRQHandler - B DMA1_Channel2_IRQHandler - - PUBWEAK DMA1_Channel3_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel3_IRQHandler - B DMA1_Channel3_IRQHandler - - PUBWEAK DMA1_Channel4_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel4_IRQHandler - B DMA1_Channel4_IRQHandler - - PUBWEAK DMA1_Channel5_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel5_IRQHandler - B DMA1_Channel5_IRQHandler - - PUBWEAK DMA1_Channel6_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel6_IRQHandler - B DMA1_Channel6_IRQHandler - - PUBWEAK DMA1_Channel7_IRQHandler - SECTION .text:CODE:REORDER(1) -DMA1_Channel7_IRQHandler - B DMA1_Channel7_IRQHandler - - PUBWEAK ADC1_2_IRQHandler - SECTION .text:CODE:REORDER(1) -ADC1_2_IRQHandler - B ADC1_2_IRQHandler - - PUBWEAK USB_HP_CAN1_TX_IRQHandler - SECTION .text:CODE:REORDER(1) -USB_HP_CAN1_TX_IRQHandler - B USB_HP_CAN1_TX_IRQHandler - - PUBWEAK USB_LP_CAN1_RX0_IRQHandler - SECTION .text:CODE:REORDER(1) -USB_LP_CAN1_RX0_IRQHandler - B USB_LP_CAN1_RX0_IRQHandler - - PUBWEAK CAN1_RX1_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_RX1_IRQHandler - B CAN1_RX1_IRQHandler - - PUBWEAK CAN1_SCE_IRQHandler - SECTION .text:CODE:REORDER(1) -CAN1_SCE_IRQHandler - B CAN1_SCE_IRQHandler - - PUBWEAK EXTI9_5_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI9_5_IRQHandler - B EXTI9_5_IRQHandler - - PUBWEAK TIM1_BRK_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_BRK_IRQHandler - B TIM1_BRK_IRQHandler - - PUBWEAK TIM1_UP_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_UP_IRQHandler - B TIM1_UP_IRQHandler - - PUBWEAK TIM1_TRG_COM_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_TRG_COM_IRQHandler - B TIM1_TRG_COM_IRQHandler - - PUBWEAK TIM1_CC_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM1_CC_IRQHandler - B TIM1_CC_IRQHandler - - PUBWEAK TIM2_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM2_IRQHandler - B TIM2_IRQHandler - - PUBWEAK TIM3_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM3_IRQHandler - B TIM3_IRQHandler - - PUBWEAK TIM4_IRQHandler - SECTION .text:CODE:REORDER(1) -TIM4_IRQHandler - B TIM4_IRQHandler - - PUBWEAK I2C1_EV_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_EV_IRQHandler - B I2C1_EV_IRQHandler - - PUBWEAK I2C1_ER_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C1_ER_IRQHandler - B I2C1_ER_IRQHandler - - PUBWEAK I2C2_EV_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C2_EV_IRQHandler - B I2C2_EV_IRQHandler - - PUBWEAK I2C2_ER_IRQHandler - SECTION .text:CODE:REORDER(1) -I2C2_ER_IRQHandler - B I2C2_ER_IRQHandler - - PUBWEAK SPI1_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI1_IRQHandler - B SPI1_IRQHandler - - PUBWEAK SPI2_IRQHandler - SECTION .text:CODE:REORDER(1) -SPI2_IRQHandler - B SPI2_IRQHandler - - PUBWEAK USART1_IRQHandler - SECTION .text:CODE:REORDER(1) -USART1_IRQHandler - B USART1_IRQHandler - - PUBWEAK USART2_IRQHandler - SECTION .text:CODE:REORDER(1) -USART2_IRQHandler - B USART2_IRQHandler - - PUBWEAK USART3_IRQHandler - SECTION .text:CODE:REORDER(1) -USART3_IRQHandler - B USART3_IRQHandler - - PUBWEAK EXTI15_10_IRQHandler - SECTION .text:CODE:REORDER(1) -EXTI15_10_IRQHandler - B EXTI15_10_IRQHandler - - PUBWEAK RTCAlarm_IRQHandler - SECTION .text:CODE:REORDER(1) -RTCAlarm_IRQHandler - B RTCAlarm_IRQHandler - - PUBWEAK USBWakeUp_IRQHandler - SECTION .text:CODE:REORDER(1) -USBWakeUp_IRQHandler - B USBWakeUp_IRQHandler - - END -/******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/ diff --git a/src/lib/Kbuild b/src/lib/Kbuild new file mode 100644 index 0000000000..b2698d35d3 --- /dev/null +++ b/src/lib/Kbuild @@ -0,0 +1,46 @@ +obj-$(CONFIG_PRINT_ON_SEGER_RT) += Segger_RTT/RTT +obj-y += CMSIS/STM32F4xx/Source/system_stm32f4xx.o + +# FatFS +obj-y += FatFS/ff.o +obj-y += FatFS/ffunicode.o + +# STM32F4xx_StdPeriph_Driver +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_adc.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dbgmcu.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dma.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_exti.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_flash.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_gpio.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_i2c.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_iwdg.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_misc.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_spi.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_syscfg.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_tim.o +obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_usart.o + +# STM32_USB_Device_Library +obj-y += STM32_USB_Device_Library/Core/src/usbd_core.o +obj-y += STM32_USB_Device_Library/Core/src/usbd_ioreq.o +obj-y += STM32_USB_Device_Library/Core/src/usbd_req.o + +# STM32_USB_OTG_Driver +obj-y += STM32_USB_OTG_Driver/src/usb_core.o +obj-y += STM32_USB_OTG_Driver/src/usb_dcd_int.o +obj-y += STM32_USB_OTG_Driver/src/usb_dcd.o + +# vl53l1 +obj-y += vl53l1/core/src/vl53l1_api_calibration.o +obj-y += vl53l1/core/src/vl53l1_api_core.o +obj-y += vl53l1/core/src/vl53l1_api_debug.o +obj-y += vl53l1/core/src/vl53l1_api.o +obj-y += vl53l1/core/src/vl53l1_api_preset_modes.o +obj-y += vl53l1/core/src/vl53l1_api_strings.o +obj-y += vl53l1/core/src/vl53l1_core.o +obj-y += vl53l1/core/src/vl53l1_core_support.o +obj-y += vl53l1/core/src/vl53l1_error_strings.o +obj-y += vl53l1/core/src/vl53l1_register_funcs.o +obj-y += vl53l1/core/src/vl53l1_silicon_core.o +obj-y += vl53l1/core/src/vl53l1_wait.o diff --git a/src/modules/Kbuild b/src/modules/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/src/modules/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/src/modules/interface/bootloader.h b/src/modules/interface/bootloader.h new file mode 100644 index 0000000000..cb993271ed --- /dev/null +++ b/src/modules/interface/bootloader.h @@ -0,0 +1,49 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie Firmware + * + * Copyright (C) Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * @file bootloader.h + * Functions to handle transitioning from the firmware to bootloader (DFU) mode on startup + * + */ + +/* ST includes */ +#include "stm32f4xx.h" + +/** + * @brief Check if memory is set after software + * reboot indicating we need to branch to the bootloader. + * Run everytime on startup. + */ +void check_enter_bootloader(); + +/** + * @brief Initiate the procedure to reboot into the bootloader. + * + * This function does not return, instead setting a flag to + * jump to the bootloader on the next start and + * issuing a software reset. + * + * @param r0 The register to utilize when jumping + * @param bl_addr The bootloader address to jump to + */ +void enter_bootloader(uint32_t r0, uint32_t bl_addr); \ No newline at end of file diff --git a/src/modules/interface/estimator.h b/src/modules/interface/estimator.h index a2195d17ce..3a2c6572b3 100644 --- a/src/modules/interface/estimator.h +++ b/src/modules/interface/estimator.h @@ -25,13 +25,16 @@ */ #pragma once +#include "autoconf.h" #include "stabilizer_types.h" typedef enum { anyEstimator = 0, complementaryEstimator, +#ifdef CONFIG_ESTIMATOR_KALMAN_ENABLE kalmanEstimator, -#ifdef OOT_ESTIMATOR +#endif +#ifdef CONFIG_ESTIMATOR_OOT OutOfTreeEstimator, #endif StateEstimatorTypeCount, @@ -158,7 +161,7 @@ static inline void estimatorEnqueueSweepAngles(const sweepAngleMeasurement_t *sw // Helper function for state estimators bool estimatorDequeue(measurement_t *measurement); -#ifdef OOT_ESTIMATOR +#ifdef CONFIG_ESTIMATOR_OOT void estimatorOutOfTreeInit(void); bool estimatorOutOfTreeTest(void); void estimatorOutOfTree(state_t *state, const uint32_t tick); diff --git a/src/modules/interface/param.h b/src/modules/interface/param.h index 46db2cd99e..ed18ee09e7 100644 --- a/src/modules/interface/param.h +++ b/src/modules/interface/param.h @@ -131,8 +131,6 @@ typedef float * (*paramGetterFloat)(void); { \ .type = TYPE, .name = #NAME, .address = (void*)(ADDRESS), .callback = 0, .getter = 0, }, -//#define PARAM_GROUP_START_SYNC(NAME, LOCK) PARAM_ADD_GROUP(PARAM_GROUP | PARAM_START, NAME, LOCK); - #define PARAM_GROUP_STOP(NAME) \ PARAM_ADD_GROUP(PARAM_GROUP | PARAM_STOP, stop_##NAME, 0x0) \ }; diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h index 2dd48b52cf..568b299be4 100644 --- a/src/modules/interface/pptraj.h +++ b/src/modules/interface/pptraj.h @@ -85,7 +85,7 @@ struct poly4d { float p[4][PP_SIZE]; float duration; // TODO use int millis instead? -} __attribute__((packed)); +}; // construct a 4d zero polynomial. struct poly4d poly4d_zero(float duration); diff --git a/src/modules/interface/queuemonitor.h b/src/modules/interface/queuemonitor.h index 5018e53d8c..b585d4c120 100644 --- a/src/modules/interface/queuemonitor.h +++ b/src/modules/interface/queuemonitor.h @@ -29,8 +29,9 @@ #include "FreeRTOS.h" +#include "autoconf.h" -#ifdef DEBUG_QUEUE_MONITOR +#ifdef CONFIG_DEBUG_QUEUE_MONITOR #include "queue.h" void queueMonitorInit(); @@ -41,6 +42,6 @@ void qmRegisterQueue(xQueueHandle* xQueue, char* fileName, char* queueName); #else #define DEBUG_QUEUE_MONITOR_REGISTER(queue) -#endif // DEBUG_QUEUE_MONITOR +#endif // CONFIG_DEBUG_QUEUE_MONITOR -#endif // __QUEUE_MONITOR_H__ \ No newline at end of file +#endif // __QUEUE_MONITOR_H__ diff --git a/src/modules/src/Kbuild b/src/modules/src/Kbuild new file mode 100644 index 0000000000..5787f0756d --- /dev/null +++ b/src/modules/src/Kbuild @@ -0,0 +1,64 @@ +obj-y += app_channel.o +obj-$(CONFIG_APP_ENABLE) += app_handler.o +obj-y += bootloader.o +obj-y += attitude_pid_controller.o +obj-y += collision_avoidance.o +obj-y += collision_avoidance.o +obj-y += commander.o +obj-y += comm.o +obj-y += console.o +obj-y += controller_indi.o +obj-y += controller_mellinger.o +obj-y += controller.o +obj-y += controller_pid.o +obj-y += crtp_commander_generic.o +obj-y += crtp_commander_high_level.o +obj-y += crtp_commander.o +obj-y += crtp_commander_rpyt.o +obj-y += crtp_localization_service.o +obj-y += crtp.o +obj-y += crtpservice.o +obj-y += esp_deck_flasher.o +obj-y += estimator_complementary.o +obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += estimator_kalman.o +obj-y += estimator.o +obj-y += eventtrigger.o +obj-y += extrx.o +obj-y += health.o +obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += kalman_supervisor.o +obj-y += log.o +obj-y += mem.o +obj-y += msp.o +obj-y += outlierFilter.o +obj-y += param_logic.o +obj-y += param_task.o +obj-y += peer_localization.o +obj-y += pid.o +obj-y += planner.o +obj-y += platformservice.o +obj-y += position_controller_indi.o +obj-y += position_controller_pid.o +obj-y += position_estimator_altitude.o +obj-y += power_distribution_stock.o +obj-y += pptraj_compressed.o +obj-y += pptraj.o +obj-y += queuemonitor.o +obj-y += range.o +obj-y += sensfusion6.o +obj-y += sound_cf2.o +obj-y += stabilizer.o +obj-y += static_mem.o +obj-y += supervisor.o +obj-y += sysload.o +obj-y += system.o +obj-y += tdoaEngineInstance.o +obj-y += worker.o + +# Kalman core +obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += kalman_core/ + +# Lighthouse +obj-y += lighthouse/ + +# App +obj-y += cvmrs.o \ No newline at end of file diff --git a/src/modules/src/Kconfig b/src/modules/src/Kconfig new file mode 100644 index 0000000000..6360885f40 --- /dev/null +++ b/src/modules/src/Kconfig @@ -0,0 +1,125 @@ +menu "Controllers and Estimators" + +choice + prompt "Default controller" + default CONFIG_CONTROLLER_ANY + +config CONTROLLER_ANY + bool "Any Controller" + help + Do not care which controller is used, if any + +config CONTROLLER_PID + bool "PID controller" + help + Use the PID (proportional–integral–derivative) controller as default + +config CONTROLLER_INDI + bool "INDI controller" + help + Use the INDI (Incremental non-linear dynamic inversion) controller as default + +config CONTROLLER_MELLINGER + bool "Mellinger controller" + help + Use the Mellinger controller as default + +endchoice + +config ESTIMATOR_KALMAN_ENABLE + bool "Enable Kalman Estimator" + default y + help + Enable the Kalman (EKF) estimator. + +choice + prompt "Default estimator" + default CONFIG_ESTIMATOR_ANY + +config ESTIMATOR_ANY + bool "Any Estimator" + help + Do not care which estimator is used, if any + +config ESTIMATOR_KALMAN + bool "Kalman estimator" + depends on ESTIMATOR_KALMAN_ENABLE + help + Use the (extended) Kalman filter (EKF) estimator as default + +config ESTIMATOR_COMPLEMENTARY + bool "Complementary estimator" + help + Use the lightweight complementary estimator as default + +endchoice + +config ESTIMATOR_OOT + bool "Out-of-tree estimator" + default n + +endmenu + +menu "Motor configuration" + +choice + prompt "ESC protocol" + default MOTORS_ESC_PROTOCOL_STANDARD_PWM + + config MOTORS_ESC_PROTOCOL_STANDARD_PWM + bool "Standard PWM" + + config MOTORS_ESC_PROTOCOL_ONESHOT125 + bool "OneShot125" + + config MOTORS_ESC_PROTOCOL_ONESHOT42 + bool "OneShot42" + +endchoice + +config MOTORS_START_DISARMED + bool "Set disarmed state after boot" + default n + help + When enabled, the firmware will boot in disarmed state and one needs to + arm the drone explicitly before starting the motors + +config MOTORS_DEFAULT_IDLE_THRUST + int "Default idle thrust for motors in armed state" + range 0 65535 + default 0 + depends on MOTORS_START_DISARMED + help + Default thrust for motors when idling in armed state, expressed as an + integer in the range 0 to 65535. This can be overridden with parameters; + the value specified here applies to the case when the persistent storage + does not contain an idle thrust value. + +config MOTORS_DEFAULT_PROP_TEST_PWM_RATIO + int "Override default PWM ratio to use during motor tests" + range 0 65535 + default 0 + help + PWM ratio to use during motor tests, expressed as an integer in the range + 0 to 65535. Zero means not to override the PWM ratio; in this case, the + drone uses full thrust for brushed motors and does not spin up brushless + motors (for safety reasons). It is therefore advised to set this parameter + explicitly for brushless builds to enable the motor test functionality. + + The value specified here can also be overridden with parameters. + +endmenu + +menu "Parameter subsystem" + +config PARAM_SILENT_UPDATES + bool "Silent parameter updates" + default n + help + Do not send CRTP packets when a parameter is updated. Note that this may + break some functionality in clients that expect to receive feedback + about parameter updates, so this option should be enabled if you know + what you are doing. + +endmenu + diff --git a/src/modules/src/app_handler.c b/src/modules/src/app_handler.c index 69d1c45719..d1b2b27335 100644 --- a/src/modules/src/app_handler.c +++ b/src/modules/src/app_handler.c @@ -26,6 +26,8 @@ #include +#include "autoconf.h" + #include "FreeRTOS.h" #include "task.h" @@ -34,17 +36,9 @@ #include "app.h" -#ifndef APP_STACKSIZE -#define APP_STACKSIZE 300 -#endif - -#ifndef APP_PRIORITY -#define APP_PRIORITY 0 -#endif - static bool isInit = false; -STATIC_MEM_TASK_ALLOC(appTask, APP_STACKSIZE); +STATIC_MEM_TASK_ALLOC(appTask, CONFIG_APP_STACKSIZE); static void appTask(void *param); @@ -54,7 +48,7 @@ void __attribute__((weak)) appInit() return; } - STATIC_MEM_TASK_CREATE(appTask, appTask, "app", NULL, APP_PRIORITY); + STATIC_MEM_TASK_CREATE(appTask, appTask, "app", NULL, CONFIG_APP_PRIORITY); isInit = true; } diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c index cf20e03f18..4eac9ed4a7 100644 --- a/src/modules/src/attitude_pid_controller.c +++ b/src/modules/src/attitude_pid_controller.c @@ -21,7 +21,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * attitude_pid_controller.c: Attitude controler using PID correctors + * attitude_pid_controller.c: Attitude controller using PID correctors */ #include @@ -49,12 +49,41 @@ static inline int16_t saturateSignedInt16(float in) return (int16_t)in; } -PidObject pidRollRate; -PidObject pidPitchRate; -PidObject pidYawRate; -PidObject pidRoll; -PidObject pidPitch; -PidObject pidYaw; +PidObject pidRollRate = { + .kp = PID_ROLL_RATE_KP, + .ki = PID_ROLL_RATE_KI, + .kd = PID_ROLL_RATE_KD, +}; + +PidObject pidPitchRate = { + .kp = PID_PITCH_RATE_KP, + .ki = PID_PITCH_RATE_KI, + .kd = PID_PITCH_RATE_KD, +}; + +PidObject pidYawRate = { + .kp = PID_YAW_RATE_KP, + .ki = PID_YAW_RATE_KI, + .kd = PID_YAW_RATE_KD, +}; + +PidObject pidRoll = { + .kp = PID_ROLL_KP, + .ki = PID_ROLL_KI, + .kd = PID_ROLL_KD, +}; + +PidObject pidPitch = { + .kp = PID_PITCH_KP, + .ki = PID_PITCH_KI, + .kd = PID_PITCH_KD, +}; + +PidObject pidYaw = { + .kp = PID_YAW_KP, + .ki = PID_YAW_KI, + .kd = PID_YAW_KD, +}; static int16_t rollOutput; static int16_t pitchOutput; @@ -68,22 +97,22 @@ void attitudeControllerInit(const float updateDt) return; //TODO: get parameters from configuration manager instead - pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, + pidInit(&pidRollRate, 0, pidRollRate.kp, pidRollRate.ki, pidRollRate.kd, updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE); - pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD, + pidInit(&pidPitchRate, 0, pidPitchRate.kp, pidPitchRate.ki, pidPitchRate.kd, updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE); - pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD, + pidInit(&pidYawRate, 0, pidYawRate.kp, pidYawRate.ki, pidYawRate.kd, updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE); pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT); pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT); pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT); - pidInit(&pidRoll, 0, PID_ROLL_KP, PID_ROLL_KI, PID_ROLL_KD, updateDt, + pidInit(&pidRoll, 0, pidRoll.kp, pidRoll.ki, pidRoll.kd, updateDt, ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE); - pidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, updateDt, + pidInit(&pidPitch, 0, pidPitch.kp, pidPitch.ki, pidPitch.kd, updateDt, ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE); - pidInit(&pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, updateDt, + pidInit(&pidYaw, 0, pidYaw.kp, pidYaw.ki, pidYaw.kd, updateDt, ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE); pidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT); @@ -258,39 +287,39 @@ PARAM_GROUP_START(pid_attitude) /** * @brief Proportional gain for the PID roll controller */ -PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRoll.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, roll_kp, &pidRoll.kp) /** * @brief Integral gain for the PID roll controller */ -PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRoll.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, roll_ki, &pidRoll.ki) /** * @brief Derivative gain for the PID roll controller */ -PARAM_ADD(PARAM_FLOAT, roll_kd, &pidRoll.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, roll_kd, &pidRoll.kd) /** * @brief Proportional gain for the PID pitch controller */ -PARAM_ADD(PARAM_FLOAT, pitch_kp, &pidPitch.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, pitch_kp, &pidPitch.kp) /** * @brief Integral gain for the PID pitch controller */ -PARAM_ADD(PARAM_FLOAT, pitch_ki, &pidPitch.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, pitch_ki, &pidPitch.ki) /** * @brief Derivative gain for the PID pitch controller */ -PARAM_ADD(PARAM_FLOAT, pitch_kd, &pidPitch.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, pitch_kd, &pidPitch.kd) /** * @brief Proportional gain for the PID yaw controller */ -PARAM_ADD(PARAM_FLOAT, yaw_kp, &pidYaw.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yaw_kp, &pidYaw.kp) /** * @brief Integral gain for the PID yaw controller */ -PARAM_ADD(PARAM_FLOAT, yaw_ki, &pidYaw.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yaw_ki, &pidYaw.ki) /** * @brief Derivative gain for the PID yaw controller */ -PARAM_ADD(PARAM_FLOAT, yaw_kd, &pidYaw.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yaw_kd, &pidYaw.kd) PARAM_GROUP_STOP(pid_attitude) /** @@ -301,37 +330,37 @@ PARAM_GROUP_START(pid_rate) /** * @brief Proportional gain for the PID roll rate controller */ -PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRollRate.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, roll_kp, &pidRollRate.kp) /** * @brief Integral gain for the PID roll rate controller */ -PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRollRate.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, roll_ki, &pidRollRate.ki) /** * @brief Derivative gain for the PID roll rate controller */ -PARAM_ADD(PARAM_FLOAT, roll_kd, &pidRollRate.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, roll_kd, &pidRollRate.kd) /** * @brief Proportional gain for the PID pitch rate controller */ -PARAM_ADD(PARAM_FLOAT, pitch_kp, &pidPitchRate.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, pitch_kp, &pidPitchRate.kp) /** * @brief Integral gain for the PID pitch rate controller */ -PARAM_ADD(PARAM_FLOAT, pitch_ki, &pidPitchRate.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, pitch_ki, &pidPitchRate.ki) /** * @brief Derivative gain for the PID pitch rate controller */ -PARAM_ADD(PARAM_FLOAT, pitch_kd, &pidPitchRate.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, pitch_kd, &pidPitchRate.kd) /** * @brief Proportional gain for the PID yaw rate controller */ -PARAM_ADD(PARAM_FLOAT, yaw_kp, &pidYawRate.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yaw_kp, &pidYawRate.kp) /** * @brief Integral gain for the PID yaw rate controller */ -PARAM_ADD(PARAM_FLOAT, yaw_ki, &pidYawRate.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yaw_ki, &pidYawRate.ki) /** * @brief Derivative gain for the PID yaw rate controller */ -PARAM_ADD(PARAM_FLOAT, yaw_kd, &pidYawRate.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yaw_kd, &pidYawRate.kd) PARAM_GROUP_STOP(pid_rate) diff --git a/src/modules/src/bootloader.c b/src/modules/src/bootloader.c new file mode 100644 index 0000000000..57e168c9ad --- /dev/null +++ b/src/modules/src/bootloader.c @@ -0,0 +1,84 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie Firmware + * + * Copyright (C) Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * @file bootloader.c + * Functions to handle transitioning from the firmware to bootloader (DFU) mode on startup + * + */ + +#include "bootloader.h" + +// bootloader code based from micropython machine_bootloader function + +// STM32H7 has ECC and writes to RAM must be 64-bit so they are fully committed +// to actual SRAM before a system reset occurs. +#define BL_STATE_PTR ((uint64_t *) SRAM2_BASE) //start of 16kb SRAM bank in stm32f405 +#define BL_STATE_KEY (0x5a5) //arbitrary bit pattern used as a marker +#define BL_STATE_KEY_MASK (0xfff) +#define BL_STATE_KEY_SHIFT (32) +#define BL_STATE_INVALID (0) +#define BL_STATE_VALID(reg, addr) ((uint64_t)(reg) | ((uint64_t)((addr) | BL_STATE_KEY)) << BL_STATE_KEY_SHIFT) +#define BL_STATE_GET_REG(s) ((s) & 0xffffffff) +#define BL_STATE_GET_KEY(s) (((s) >> BL_STATE_KEY_SHIFT) & BL_STATE_KEY_MASK) +#define BL_STATE_GET_ADDR(s) (((s) >> BL_STATE_KEY_SHIFT) & ~BL_STATE_KEY_MASK) + + +/** + * @brief Branch directly to the bootloader address, setting the + * stack pointer and destination address first. + * Based from the micropython machine_bootloader function. + * + * @param r0 The register to utilize + * @param bl_addr The bootloader address to jump to + */ +static void branch_to_bootloader(uint32_t r0, uint32_t bl_addr){ + __asm volatile ( + "ldr r2, [r1, #0]\n" // get address of stack pointer + "msr msp, r2\n" // get stack pointer + "ldr r2, [r1, #4]\n" // get address of destination + "bx r2\n" // branch to bootloader + ); + //unreachable code + while(1); +} + +void check_enter_bootloader(){ + uint64_t bl_state = *BL_STATE_PTR; + //set to invalid for next boot + *BL_STATE_PTR = BL_STATE_INVALID; + + if(BL_STATE_GET_KEY(bl_state) == BL_STATE_KEY && (RCC->CSR & RCC_CSR_SFTRSTF)){ + //if botloader data valid and was just reset with NVIC_SystemReset + + //remap memory to system flash + SYSCFG_MemoryRemapConfig(SYSCFG_MemoryRemap_SystemFlash); + branch_to_bootloader(BL_STATE_GET_REG(bl_state), BL_STATE_GET_ADDR(bl_state)); + } +} + +void enter_bootloader(uint32_t r0, uint32_t bl_addr){ + //set bootloader state values + *BL_STATE_PTR = BL_STATE_VALID(r0, bl_addr); + + NVIC_SystemReset(); +} \ No newline at end of file diff --git a/src/modules/src/controller.c b/src/modules/src/controller.c index 820f2b7c72..f18c19ff5e 100644 --- a/src/modules/src/controller.c +++ b/src/modules/src/controller.c @@ -7,6 +7,8 @@ #include "controller_mellinger.h" #include "controller_indi.h" +#include "autoconf.h" + #define DEFAULT_CONTROLLER ControllerTypePID static ControllerType currentController = ControllerTypeAny; @@ -38,7 +40,17 @@ void controllerInit(ControllerType controller) { currentController = DEFAULT_CONTROLLER; } - ControllerType forcedController = CONTROLLER_NAME; + #if defined(CONFIG_CONTROLLER_PID) + #define CONTROLLER ControllerTypePID + #elif defined(CONFIG_CONTROLLER_INDI) + #define CONTROLLER ControllerTypeINDI + #elif defined(CONFIG_CONTROLLER_MELLINGER) + #define CONTROLLER ControllerTypeMellinger + #else + #define CONTROLLER ControllerTypeAny + #endif + + ControllerType forcedController = CONTROLLER; if (forcedController != ControllerTypeAny) { DEBUG_PRINT("Controller type forced\n"); currentController = forcedController; diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c index 4a2dd25252..f45206a85d 100644 --- a/src/modules/src/controller_mellinger.c +++ b/src/modules/src/controller_mellinger.c @@ -322,25 +322,25 @@ void controllerMellinger(control_t *control, setpoint_t *setpoint, } PARAM_GROUP_START(ctrlMel) -PARAM_ADD(PARAM_FLOAT, kp_xy, &kp_xy) -PARAM_ADD(PARAM_FLOAT, kd_xy, &kd_xy) -PARAM_ADD(PARAM_FLOAT, ki_xy, &ki_xy) -PARAM_ADD(PARAM_FLOAT, i_range_xy, &i_range_xy) -PARAM_ADD(PARAM_FLOAT, kp_z, &kp_z) -PARAM_ADD(PARAM_FLOAT, kd_z, &kd_z) -PARAM_ADD(PARAM_FLOAT, ki_z, &ki_z) -PARAM_ADD(PARAM_FLOAT, i_range_z, &i_range_z) -PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) -PARAM_ADD(PARAM_FLOAT, massThrust, &massThrust) -PARAM_ADD(PARAM_FLOAT, kR_xy, &kR_xy) -PARAM_ADD(PARAM_FLOAT, kR_z, &kR_z) -PARAM_ADD(PARAM_FLOAT, kw_xy, &kw_xy) -PARAM_ADD(PARAM_FLOAT, kw_z, &kw_z) -PARAM_ADD(PARAM_FLOAT, ki_m_xy, &ki_m_xy) -PARAM_ADD(PARAM_FLOAT, ki_m_z, &ki_m_z) -PARAM_ADD(PARAM_FLOAT, kd_omega_rp, &kd_omega_rp) -PARAM_ADD(PARAM_FLOAT, i_range_m_xy, &i_range_m_xy) -PARAM_ADD(PARAM_FLOAT, i_range_m_z, &i_range_m_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kp_xy, &kp_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kd_xy, &kd_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, ki_xy, &ki_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_xy, &i_range_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kp_z, &kp_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kd_z, &kd_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, ki_z, &ki_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_z, &i_range_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, mass, &g_vehicleMass) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, massThrust, &massThrust) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kR_xy, &kR_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kR_z, &kR_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kw_xy, &kw_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kw_z, &kw_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, ki_m_xy, &ki_m_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, ki_m_z, &ki_m_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, kd_omega_rp, &kd_omega_rp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_m_xy, &i_range_m_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_m_z, &i_range_m_z) PARAM_GROUP_STOP(ctrlMel) LOG_GROUP_START(ctrlMel) diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c index fd8745653d..4d45d89a9b 100644 --- a/src/modules/src/crtp_commander_generic.c +++ b/src/modules/src/crtp_commander_generic.c @@ -420,22 +420,22 @@ PARAM_GROUP_START(cmdrCPPM) /** * @brief Config of max roll rate at max stick input [DPS] (default: 720) */ -PARAM_ADD(PARAM_FLOAT, rateRoll, &s_CppmEmuRollMaxRateDps) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, rateRoll, &s_CppmEmuRollMaxRateDps) /** * @brief Config of max pitch rate at max stick input [DPS] (default: 720) */ -PARAM_ADD(PARAM_FLOAT, ratePitch, &s_CppmEmuPitchMaxRateDps) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, ratePitch, &s_CppmEmuPitchMaxRateDps) /** * @brief Config of max pitch angle at max stick input [DEG] (default: 50) */ -PARAM_ADD(PARAM_FLOAT, angPitch, &s_CppmEmuPitchMaxAngleDeg) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, angPitch, &s_CppmEmuPitchMaxAngleDeg) /** * @brief Config of max roll angle at max stick input [DEG] (default: 50) */ -PARAM_ADD(PARAM_FLOAT, angRoll, &s_CppmEmuRollMaxAngleDeg) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, angRoll, &s_CppmEmuRollMaxAngleDeg) /** * @brief Config of max yaw rate at max stick input [DPS] (default: 400) */ -PARAM_ADD(PARAM_FLOAT, rateYaw, &s_CppmEmuYawMaxRateDps) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, rateYaw, &s_CppmEmuYawMaxRateDps) PARAM_GROUP_STOP(cmdrCPPM) diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c index 1468992ca4..585170d163 100644 --- a/src/modules/src/crtp_localization_service.c +++ b/src/modules/src/crtp_localization_service.c @@ -217,8 +217,11 @@ static void extPosePackedHandler(const CRTPPacket* pk) { static void lpsShortLppPacketHandler(CRTPPacket* pk) { if (pk->size >= 2) { +#ifdef CONFIG_DECK_LOCO bool success = lpsSendLppShort(pk->data[1], &pk->data[2], pk->size-2); - +#else + bool success = false; +#endif pk->port = CRTP_PORT_LOCALIZATION; pk->channel = GENERIC_TYPE; pk->size = 3; @@ -383,45 +386,45 @@ LOG_GROUP_STOP(ext_pos) /** * Logging variables for (external) positioning data stream through ctrp - */ + */ LOG_GROUP_START(locSrv) /** - * @brief Position X measurement from external system - */ + * @brief Position X measurement from external system + */ LOG_ADD_CORE(LOG_FLOAT, x, &ext_pose.x) /** - * @brief Position Y measurement from external system - */ + * @brief Position Y measurement from external system + */ LOG_ADD_CORE(LOG_FLOAT, y, &ext_pose.y) /** * @brief Position Z measurement from external system - */ + */ LOG_ADD_CORE(LOG_FLOAT, z, &ext_pose.z) /** * @brief Quaternion x meas from an external system - */ + */ LOG_ADD_CORE(LOG_FLOAT, qx, &ext_pose.quat.x) /** * @brief Quaternion y meas from an external system - */ + */ LOG_ADD_CORE(LOG_FLOAT, qy, &ext_pose.quat.y) /** * @brief Quaternion z meas from an external system - */ + */ LOG_ADD_CORE(LOG_FLOAT, qz, &ext_pose.quat.z) /** * @brief Quaternion w meas from an external system - */ + */ LOG_ADD_CORE(LOG_FLOAT, qw, &ext_pose.quat.w) LOG_GROUP_STOP(locSrv) /** * Logging variables for (external) positioning data stream through Compressed - */ + */ LOG_GROUP_START(locSrvZ) /** * @brief time when data was received last (ms/ticks) - */ + */ LOG_ADD_CORE(LOG_UINT16, tick, &tickOfLastPacket) // time when data was received last (ms/ticks) LOG_GROUP_STOP(locSrvZ) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 56870a88d7..9446bd030d 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -33,9 +33,10 @@ void appMain() logVarId_t z_id = logGetVarId("stateEstimateZ", "z"); logVarId_t quat_id = logGetVarId("stateEstimateZ", "quat"); - cpx_packet.route.destination = GAP8; - cpx_packet.route.function = APP; - cpx_packet.route.source = STM32; + cpx_packet.route.destination = CPX_T_GAP8; + cpx_packet.route.function = CPX_F_APP; + cpx_packet.route.source = CPX_T_STM32; + cpx_packet.dataLength = sizeof(StatePacket_t); StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; state_packet->cmd = 0; @@ -53,6 +54,6 @@ void appMain() state_packet->z = logGetInt(z_id); state_packet->quat = logGetInt(quat_id); - cpxSendPacketBlocking(&cpx_packet, sizeof(StatePacket_t)); + cpxSendPacketBlocking(&cpx_packet); } } \ No newline at end of file diff --git a/src/modules/src/estimator.c b/src/modules/src/estimator.c index cad7e09a7f..cea0871459 100644 --- a/src/modules/src/estimator.c +++ b/src/modules/src/estimator.c @@ -69,6 +69,7 @@ static EstimatorFcns estimatorFunctions[] = { .update = estimatorComplementary, .name = "Complementary", }, +#ifdef CONFIG_ESTIMATOR_KALMAN_ENABLE { .init = estimatorKalmanInit, .deinit = NOT_IMPLEMENTED, @@ -76,7 +77,8 @@ static EstimatorFcns estimatorFunctions[] = { .update = estimatorKalman, .name = "Kalman", }, -#ifdef OOT_ESTIMATOR +#endif +#ifdef CONFIG_ESTIMATOR_OOT { .init = estimatorOutOfTreeInit, .deinit = NOT_IMPLEMENTED, @@ -103,7 +105,15 @@ void stateEstimatorSwitchTo(StateEstimatorType estimator) { newEstimator = DEFAULT_ESTIMATOR; } - StateEstimatorType forcedEstimator = ESTIMATOR_NAME; + #if defined(CONFIG_ESTIMATOR_KALMAN) + #define ESTIMATOR kalmanEstimator + #elif defined(CONFIG_ESTIMATOR_COMPLEMENTARY) + #define ESTIMATOR complementaryEstimator + #else + #define ESTIMATOR anyEstimator + #endif + + StateEstimatorType forcedEstimator = ESTIMATOR; if (forcedEstimator != anyEstimator) { DEBUG_PRINT("Estimator type forced\n"); newEstimator = forcedEstimator; @@ -196,7 +206,7 @@ void estimatorEnqueue(const measurement_t *measurement) { eventTrigger(&eventTrigger_estTOF); break; case MeasurementTypeAbsoluteHeight: - // no payload needed, see LPS_2D_POSITION_HEIGHT + // no payload needed, see CONFIG_DECK_LOCO_2D_POSITION eventTrigger(&eventTrigger_estAbsoluteHeight); break; case MeasurementTypeFlow: diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index f4e762a5e6..423e62fc8d 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -602,35 +602,35 @@ PARAM_GROUP_START(kalman) /** * @brief Process noise for x and y acceleration */ - PARAM_ADD_CORE(PARAM_FLOAT, pNAcc_xy, &coreParams.procNoiseAcc_xy) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, pNAcc_xy, &coreParams.procNoiseAcc_xy) /** * @brief Process noise for z acceleration */ - PARAM_ADD_CORE(PARAM_FLOAT, pNAcc_z, &coreParams.procNoiseAcc_z) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, pNAcc_z, &coreParams.procNoiseAcc_z) /** * @brief Process noise for velocity */ - PARAM_ADD_CORE(PARAM_FLOAT, pNVel, &coreParams.procNoiseVel) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, pNVel, &coreParams.procNoiseVel) /** * @brief Process noise for position */ - PARAM_ADD_CORE(PARAM_FLOAT, pNPos, &coreParams.procNoisePos) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, pNPos, &coreParams.procNoisePos) /** * @brief Process noise for attitude */ - PARAM_ADD_CORE(PARAM_FLOAT, pNAtt, &coreParams.procNoiseAtt) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, pNAtt, &coreParams.procNoiseAtt) /** * @brief Measurement noise for barometer */ - PARAM_ADD_CORE(PARAM_FLOAT, mNBaro, &coreParams.measNoiseBaro) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, mNBaro, &coreParams.measNoiseBaro) /** * @brief Measurement noise for roll/pitch gyros */ - PARAM_ADD_CORE(PARAM_FLOAT, mNGyro_rollpitch, &coreParams.measNoiseGyro_rollpitch) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, mNGyro_rollpitch, &coreParams.measNoiseGyro_rollpitch) /** * @brief Measurement noise for yaw gyro */ - PARAM_ADD_CORE(PARAM_FLOAT, mNGyro_yaw, &coreParams.measNoiseGyro_yaw) + PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, mNGyro_yaw, &coreParams.measNoiseGyro_yaw) /** * @brief Initial X after reset [m] */ diff --git a/src/modules/src/health.c b/src/modules/src/health.c index 9166f03569..7149f93b2e 100644 --- a/src/modules/src/health.c +++ b/src/modules/src/health.c @@ -44,19 +44,16 @@ #include "motors.h" #include "sensors.h" #include "pm.h" +#include "autoconf.h" #include "static_mem.h" -#ifndef DEFAULT_PROP_TEST_PWM_RATIO -# define DEFAULT_PROP_TEST_PWM_RATIO 0 -#endif - #define PROPTEST_NBR_OF_VARIANCE_VALUES 100 static bool startPropTest = false; static bool startBatTest = false; -static uint16_t propTestPWMRatio = DEFAULT_PROP_TEST_PWM_RATIO; +static uint16_t propTestPWMRatio = CONFIG_MOTORS_DEFAULT_PROP_TEST_PWM_RATIO; static uint32_t i = 0; NO_DMA_CCM_SAFE_ZERO_INIT static float accX[PROPTEST_NBR_OF_VARIANCE_VALUES]; diff --git a/src/modules/src/kalman_core/Kbuild b/src/modules/src/kalman_core/Kbuild new file mode 100644 index 0000000000..bacc492bcb --- /dev/null +++ b/src/modules/src/kalman_core/Kbuild @@ -0,0 +1,12 @@ +obj-y += kalman_core.o +obj-y += mm_absolute_height.o +obj-y += mm_distance.o +obj-y += mm_distance_robust.o +obj-y += mm_flow.o +obj-y += mm_pose.o +obj-y += mm_position.o +obj-y += mm_sweep_angles.o +obj-y += mm_tdoa.o +obj-y += mm_tdoa_robust.o +obj-y += mm_tof.o +obj-y += mm_yaw_error.o diff --git a/src/modules/src/kalman_core/kalman_core.c b/src/modules/src/kalman_core/kalman_core.c index fa2ce377a9..79d0b44d23 100644 --- a/src/modules/src/kalman_core/kalman_core.c +++ b/src/modules/src/kalman_core/kalman_core.c @@ -67,7 +67,7 @@ // #define DEBUG_STATE_CHECK // the reversion of pitch and roll to zero -#ifdef LPS_2D_POSITION_HEIGHT +#ifdef CONFIG_DECK_LOCO_2D_POSITION #define ROLLPITCH_ZERO_REVERSION (0.0f) #else #define ROLLPITCH_ZERO_REVERSION (0.001f) diff --git a/src/modules/src/lighthouse/Kbuild b/src/modules/src/lighthouse/Kbuild new file mode 100644 index 0000000000..cb4dafd440 --- /dev/null +++ b/src/modules/src/lighthouse/Kbuild @@ -0,0 +1,5 @@ +obj-y += lighthouse_core.o +obj-y += lighthouse_deck_flasher.o +obj-y += lighthouse_position_est.o +obj-y += lighthouse_storage.o +obj-y += lighthouse_transmit.o diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index 5c16233066..3162b3f3d2 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/src/modules/src/lighthouse/lighthouse_core.c @@ -37,6 +37,7 @@ #include "log.h" #include "param.h" #include "statsCnt.h" +#include "autoconf.h" #define DEBUG_MODULE "LH" #include "debug.h" @@ -85,6 +86,8 @@ static STATS_CNT_RATE_DEFINE(bs0Rate, HALF_SECOND); static STATS_CNT_RATE_DEFINE(bs1Rate, HALF_SECOND); static statsCntRateLogger_t* bsRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&bs0Rate, &bs1Rate}; +// A bitmap that indicates which base stations that are available +static uint16_t baseStationAvailabledMap; // A bitmap that indicates which base staions that are received static uint16_t baseStationReceivedMapWs; @@ -136,6 +139,10 @@ static void modifyBit(uint16_t *bitmap, const int index, const bool value) { void lighthouseCoreInit() { lighthouseStorageInitializeSystemTypeFromStorage(); lighthousePositionEstInit(); + + for (int i = 0; i < PULSE_PROCESSOR_N_BASE_STATIONS; i++) { + modifyBit(&baseStationAvailabledMap, i, true); + } } void lighthouseCoreLedTimer() @@ -209,7 +216,7 @@ TESTABLE_STATIC bool getUartFrameRaw(lighthouseUartFrame_t *frame) { for(int i = 0; i < UART_FRAME_LENGTH; i++) { while(!uart1GetDataWithTimeout((uint8_t*)&data[i], 2)) { - lighthouseTransmitProcessTimeout(); + lighthouseTransmitProcessTimeout(); } if ((unsigned char)data[i] == 0xff) { syncCounter += 1; @@ -273,7 +280,7 @@ void lighthouseCoreSetLeds(lighthouseCoreLedState_t red, lighthouseCoreLedState_ // estimator as pre-calculated. // 1 = Sweep angles pushed into the estimator. Yaw error calculated outside the estimator // and pushed to the estimator as a pre-calculated value. -#ifdef LIGHTHOUSE_AS_GROUNDTRUTH +#ifdef CONFIG_DECK_LIGHTHOUSE_AS_GROUNDTRUTH static uint8_t estimationMethod = 0; #else static uint8_t estimationMethod = 1; @@ -487,7 +494,7 @@ void lighthouseCoreTask(void *param) { else if(!frame.isSyncFrame) { STATS_CNT_RATE_EVENT(&frameRate); lighthouseTransmitProcessFrame(&frame); - + deckHealthCheck(&lighthouseCoreState, &frame, now_ms); lighthouseUpdateSystemType(); if (pulseProcessorProcessPulse) { @@ -773,7 +780,7 @@ LOG_ADD(LOG_UINT8, comSync, &uartSynchronized) /** * @brief Bit field indicating which base stations that are received by the lighthouse deck * - * The lowest bit mapps to base station channel 1 and the highest to channel 16. + * The lowest bit maps to base station channel 1 and the highest to channel 16. */ LOG_ADD_CORE(LOG_UINT16, bsReceive, &baseStationReceivedMap) @@ -782,21 +789,21 @@ LOG_ADD_CORE(LOG_UINT16, bsReceive, &baseStationReceivedMap) * * A bit will be set if there is calibration and geometry data for the base station, and sweeps are received. * - * The lowest bit mapps to base station channel 1 and the highest to channel 16. + * The lowest bit maps to base station channel 1 and the highest to channel 16. */ LOG_ADD_CORE(LOG_UINT16, bsActive, &baseStationActiveMap) /** * @brief Bit field that indicates which base stations that have received calibration data that was different to what was stored in memory * - * The lowest bit mapps to base station channel 1 and the highest to channel 16. + * The lowest bit maps to base station channel 1 and the highest to channel 16. */ LOG_ADD_CORE(LOG_UINT16, bsCalUd, &baseStationCalibUpdatedMap) /** * @brief Bit field that indicates which base stations that have received calibration data over the air * - * The lowest bit mapps to base station channel 1 and the highest to channel 16. + * The lowest bit maps to base station channel 1 and the highest to channel 16. */ LOG_ADD_CORE(LOG_UINT16, bsCalCon, &baseStationCalibConfirmedMap) @@ -830,4 +837,12 @@ PARAM_ADD_CORE(PARAM_UINT8, bsCalibReset, &calibStatusReset) * (default: 2) */ PARAM_ADD_CORE(PARAM_UINT8, systemType, &systemType) + +/** + * @brief Bit field that indicates which base stations that are supported by the system + * + * The lowest bit maps to base station channel 1 and the highest to channel 16. + */ +PARAM_ADD_CORE(PARAM_UINT16 | PARAM_RONLY, bsAvailable, &baseStationAvailabledMap) + PARAM_GROUP_STOP(lighthouse) diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c index 2fdf616418..d719325191 100644 --- a/src/modules/src/lighthouse/lighthouse_position_est.c +++ b/src/modules/src/lighthouse/lighthouse_position_est.c @@ -35,6 +35,7 @@ #include "param.h" #include "statsCnt.h" #include "mem.h" +#include "autoconf.h" #include "lighthouse_position_est.h" #include "lighthouse_geometry.h" @@ -265,7 +266,7 @@ static void estimatePositionCrossingBeams(const pulseProcessor_t *state, pulsePr if (isfinite(ext_pos.pos[0]) && isfinite(ext_pos.pos[1]) && isfinite(ext_pos.pos[2])) { ext_pos.stdDev = 0.01; ext_pos.source = MeasurementSourceLighthouse; - #ifndef LIGHTHOUSE_AS_GROUNDTRUTH + #ifndef CONFIG_DECK_LIGHTHOUSE_AS_GROUNDTRUTH estimatorEnqueuePosition(&ext_pos); #endif } diff --git a/src/modules/src/param_logic.c b/src/modules/src/param_logic.c index 13a7dae03e..941b02efed 100644 --- a/src/modules/src/param_logic.c +++ b/src/modules/src/param_logic.c @@ -32,6 +32,7 @@ #include "crc32.h" #include "debug.h" #include "cfassert.h" +#include "autoconf.h" #if 0 #define PARAM_DEBUG(fmt, ...) DEBUG_PRINT("D/param " fmt, ## __VA_ARGS__) @@ -566,7 +567,7 @@ void paramSetInt(paramVarId_t varid, int valuei) pk.size += paramSet(varid.index, (void *)&valuei); -#ifndef SILENT_PARAM_UPDATES +#ifndef CONFIG_PARAM_SILENT_UPDATES crtpSendPacketBlock(&pk); #endif } @@ -589,7 +590,7 @@ void paramSetFloat(paramVarId_t varid, float valuef) pk.size += 4; } -#ifndef SILENT_PARAM_UPDATES +#ifndef CONFIG_PARAM_SILENT_UPDATES crtpSendPacketBlock(&pk); #endif } diff --git a/src/modules/src/position_controller_pid.c b/src/modules/src/position_controller_pid.c index aef6c95465..82a23ed923 100644 --- a/src/modules/src/position_controller_pid.c +++ b/src/modules/src/position_controller_pid.c @@ -34,17 +34,11 @@ #include "num.h" #include "position_controller.h" -struct pidInit_s { - float kp; - float ki; - float kd; -}; struct pidAxis_s { PidObject pid; - struct pidInit_s init; - stab_mode_t previousMode; + stab_mode_t previousMode; float setpoint; float output; @@ -95,16 +89,16 @@ float velZFiltCutoff = 20.0f; #ifndef UNIT_TEST static struct this_s this = { .pidVX = { - .init = { - .kp = 25.0f, - .ki = 1.0f, - .kd = 0.0f, + .pid = { + .kp = 25.0f, + .ki = 1.0f, + .kd = 0.0f, }, .pid.dt = DT, }, .pidVY = { - .init = { + .pid = { .kp = 25.0f, .ki = 1.0f, .kd = 0.0f, @@ -113,7 +107,7 @@ static struct this_s this = { }, #ifdef IMPROVED_BARO_Z_HOLD .pidVZ = { - .init = { + .pid = { .kp = 3.0f, .ki = 1.0f, .kd = 1.5f, //kd can be lowered for improved stability, but results in slower response time. @@ -122,7 +116,7 @@ static struct this_s this = { }, #else .pidVZ = { - .init = { + .pid = { .kp = 25.0f, .ki = 15.0f, .kd = 0, @@ -131,7 +125,7 @@ static struct this_s this = { }, #endif .pidX = { - .init = { + .pid = { .kp = 2.0f, .ki = 0.0f, .kd = 0.0f, @@ -140,7 +134,7 @@ static struct this_s this = { }, .pidY = { - .init = { + .pid = { .kp = 2.0f, .ki = 0.0f, .kd = 0.0f, @@ -149,7 +143,7 @@ static struct this_s this = { }, .pidZ = { - .init = { + .pid = { .kp = 2.0f, .ki = 0.5f, .kd = 0.0f, @@ -167,18 +161,18 @@ static struct this_s this = { void positionControllerInit() { - pidInit(&this.pidX.pid, this.pidX.setpoint, this.pidX.init.kp, this.pidX.init.ki, this.pidX.init.kd, + pidInit(&this.pidX.pid, this.pidX.setpoint, this.pidX.pid.kp, this.pidX.pid.ki, this.pidX.pid.kd, this.pidX.pid.dt, POSITION_RATE, posFiltCutoff, posFiltEnable); - pidInit(&this.pidY.pid, this.pidY.setpoint, this.pidY.init.kp, this.pidY.init.ki, this.pidY.init.kd, + pidInit(&this.pidY.pid, this.pidY.setpoint, this.pidY.pid.kp, this.pidY.pid.ki, this.pidY.pid.kd, this.pidY.pid.dt, POSITION_RATE, posFiltCutoff, posFiltEnable); - pidInit(&this.pidZ.pid, this.pidZ.setpoint, this.pidZ.init.kp, this.pidZ.init.ki, this.pidZ.init.kd, + pidInit(&this.pidZ.pid, this.pidZ.setpoint, this.pidZ.pid.kp, this.pidZ.pid.ki, this.pidZ.pid.kd, this.pidZ.pid.dt, POSITION_RATE, posZFiltCutoff, posZFiltEnable); - pidInit(&this.pidVX.pid, this.pidVX.setpoint, this.pidVX.init.kp, this.pidVX.init.ki, this.pidVX.init.kd, + pidInit(&this.pidVX.pid, this.pidVX.setpoint, this.pidVX.pid.kp, this.pidVX.pid.ki, this.pidVX.pid.kd, this.pidVX.pid.dt, POSITION_RATE, velFiltCutoff, velFiltEnable); - pidInit(&this.pidVY.pid, this.pidVY.setpoint, this.pidVY.init.kp, this.pidVY.init.ki, this.pidVY.init.kd, + pidInit(&this.pidVY.pid, this.pidVY.setpoint, this.pidVY.pid.kp, this.pidVY.pid.ki, this.pidVY.pid.kd, this.pidVY.pid.dt, POSITION_RATE, velFiltCutoff, velFiltEnable); - pidInit(&this.pidVZ.pid, this.pidVZ.setpoint, this.pidVZ.init.kp, this.pidVZ.init.ki, this.pidVZ.init.kd, + pidInit(&this.pidVZ.pid, this.pidVZ.setpoint, this.pidVZ.pid.kp, this.pidVZ.pid.ki, this.pidVZ.pid.kd, this.pidVZ.pid.dt, POSITION_RATE, velZFiltCutoff, velZFiltEnable); } @@ -411,49 +405,49 @@ PARAM_GROUP_START(velCtlPid) /** * @brief Proportional gain for the velocity PID in the body-yaw-aligned X direction */ -PARAM_ADD(PARAM_FLOAT, vxKp, &this.pidVX.pid.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vxKp, &this.pidVX.pid.kp) /** * @brief Integral gain for the velocity PID in the body-yaw-aligned X direction */ -PARAM_ADD(PARAM_FLOAT, vxKi, &this.pidVX.pid.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vxKi, &this.pidVX.pid.ki) /** * @brief Derivative gain for the velocity PID in the body-yaw-aligned X direction */ -PARAM_ADD(PARAM_FLOAT, vxKd, &this.pidVX.pid.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vxKd, &this.pidVX.pid.kd) /** * @brief Proportional gain for the velocity PID in the body-yaw-aligned Y direction */ -PARAM_ADD(PARAM_FLOAT, vyKp, &this.pidVY.pid.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vyKp, &this.pidVY.pid.kp) /** * @brief Integral gain for the velocity PID in the body-yaw-aligned Y direction */ -PARAM_ADD(PARAM_FLOAT, vyKi, &this.pidVY.pid.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vyKi, &this.pidVY.pid.ki) /** * @brief Derivative gain for the velocity PID in the body-yaw-aligned Y direction */ -PARAM_ADD(PARAM_FLOAT, vyKd, &this.pidVY.pid.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vyKd, &this.pidVY.pid.kd) /** * @brief Proportional gain for the velocity PID in the global Z direction */ -PARAM_ADD(PARAM_FLOAT, vzKp, &this.pidVZ.pid.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vzKp, &this.pidVZ.pid.kp) /** * @brief Integral gain for the velocity PID in the global Z direction */ -PARAM_ADD(PARAM_FLOAT, vzKi, &this.pidVZ.pid.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vzKi, &this.pidVZ.pid.ki) /** * @brief Derivative gain for the velocity PID in the global Z direction */ -PARAM_ADD(PARAM_FLOAT, vzKd, &this.pidVZ.pid.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vzKd, &this.pidVZ.pid.kd) /** * @brief Feed-forward gain for the velocity PID in the body-yaw-aligned X direction (in degrees per m/s) */ -PARAM_ADD(PARAM_FLOAT, vxKFF, &kFFx) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vxKFF, &kFFx) /** * @brief Feed-forward gain for the velocity PID in the body-yaw-aligned Y direction (in degrees per m/s) */ -PARAM_ADD(PARAM_FLOAT, vyKFF, &kFFy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, vyKFF, &kFFy) PARAM_GROUP_STOP(velCtlPid) @@ -466,70 +460,70 @@ PARAM_GROUP_START(posCtlPid) /** * @brief Proportional gain for the position PID in the body-yaw-aligned X direction */ -PARAM_ADD(PARAM_FLOAT, xKp, &this.pidX.pid.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, xKp, &this.pidX.pid.kp) /** * @brief Proportional gain for the position PID in the body-yaw-aligned X direction */ -PARAM_ADD(PARAM_FLOAT, xKi, &this.pidX.pid.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, xKi, &this.pidX.pid.ki) /** * @brief Derivative gain for the position PID in the body-yaw-aligned X direction */ -PARAM_ADD(PARAM_FLOAT, xKd, &this.pidX.pid.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, xKd, &this.pidX.pid.kd) /** * @brief Proportional gain for the position PID in the body-yaw-aligned Y direction */ -PARAM_ADD(PARAM_FLOAT, yKp, &this.pidY.pid.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yKp, &this.pidY.pid.kp) /** * @brief Integral gain for the position PID in the body-yaw-aligned Y direction */ -PARAM_ADD(PARAM_FLOAT, yKi, &this.pidY.pid.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yKi, &this.pidY.pid.ki) /** * @brief Derivative gain for the position PID in the body-yaw-aligned Y direction */ -PARAM_ADD(PARAM_FLOAT, yKd, &this.pidY.pid.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yKd, &this.pidY.pid.kd) /** * @brief Proportional gain for the position PID in the global Z direction */ -PARAM_ADD(PARAM_FLOAT, zKp, &this.pidZ.pid.kp) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, zKp, &this.pidZ.pid.kp) /** * @brief Integral gain for the position PID in the global Z direction */ -PARAM_ADD(PARAM_FLOAT, zKi, &this.pidZ.pid.ki) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, zKi, &this.pidZ.pid.ki) /** * @brief Derivative gain for the position PID in the global Z direction */ -PARAM_ADD(PARAM_FLOAT, zKd, &this.pidZ.pid.kd) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, zKd, &this.pidZ.pid.kd) /** * @brief Approx. thrust needed for hover */ -PARAM_ADD(PARAM_UINT16, thrustBase, &this.thrustBase) +PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, thrustBase, &this.thrustBase) /** * @brief Min. thrust value to output */ -PARAM_ADD(PARAM_UINT16, thrustMin, &this.thrustMin) +PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, thrustMin, &this.thrustMin) /** * @brief Roll absolute limit */ -PARAM_ADD(PARAM_FLOAT, rLimit, &rLimit) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, rLimit, &rLimit) /** * @brief Pitch absolute limit */ -PARAM_ADD(PARAM_FLOAT, pLimit, &pLimit) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, pLimit, &pLimit) /** * @brief Maximum body-yaw-aligned X velocity */ -PARAM_ADD(PARAM_FLOAT, xVelMax, &xVelMax) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, xVelMax, &xVelMax) /** * @brief Maximum body-yaw-aligned Y velocity */ -PARAM_ADD(PARAM_FLOAT, yVelMax, &yVelMax) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, yVelMax, &yVelMax) /** * @brief Maximum Z Velocity */ -PARAM_ADD(PARAM_FLOAT, zVelMax, &zVelMax) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, zVelMax, &zVelMax) PARAM_GROUP_STOP(posCtlPid) diff --git a/src/modules/src/position_estimator_altitude.c b/src/modules/src/position_estimator_altitude.c index 01eabf0b88..8d690756a9 100644 --- a/src/modules/src/position_estimator_altitude.c +++ b/src/modules/src/position_estimator_altitude.c @@ -128,27 +128,27 @@ LOG_ADD(LOG_FLOAT, velocityZ, &state.velocityZ) LOG_GROUP_STOP(posEstAlt) /** - * Tuning setttings for the altititude estimator/filtering + * Tuning setttings for the altitude estimator/filtering */ PARAM_GROUP_START(posEstAlt) /** * @brief parameter alpha IIR Filter above sea level/true altitude (baro) */ -PARAM_ADD_CORE(PARAM_FLOAT, estAlphaAsl, &state.estAlphaAsl) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, estAlphaAsl, &state.estAlphaAsl) /** * @brief parameter alpha IIR Filter Height (zranger) */ -PARAM_ADD_CORE(PARAM_FLOAT, estAlphaZr, &state.estAlphaZrange) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, estAlphaZr, &state.estAlphaZrange) /** * @brief Multiplying factor for adding velocity */ -PARAM_ADD(PARAM_FLOAT, velFactor, &state.velocityFactor) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, velFactor, &state.velocityFactor) /** * @brief Blendning factor to avoid accumulate error */ -PARAM_ADD(PARAM_FLOAT, velZAlpha, &state.velZAlpha) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, velZAlpha, &state.velZAlpha) /** * @brief Vertical acceleration deadband */ -PARAM_ADD_CORE(PARAM_FLOAT, vAccDeadband, &state.vAccDeadband) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, vAccDeadband, &state.vAccDeadband) PARAM_GROUP_STOP(posEstAlt) diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c index edcdc8a755..9d428fe5ef 100644 --- a/src/modules/src/power_distribution_stock.c +++ b/src/modules/src/power_distribution_stock.c @@ -34,6 +34,7 @@ #include "platform.h" #include "motors.h" #include "debug.h" +#include "autoconf.h" static bool motorSetEnable = false; @@ -51,8 +52,10 @@ static struct { uint16_t m4; } motorPowerSet; -#ifndef DEFAULT_IDLE_THRUST -#define DEFAULT_IDLE_THRUST 0 +#ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST +# define DEFAULT_IDLE_THRUST 0 +#else +# define DEFAULT_IDLE_THRUST CONFIG_MOTORS_DEFAULT_IDLE_THRUST #endif static uint32_t idleThrust = DEFAULT_IDLE_THRUST; @@ -173,7 +176,7 @@ PARAM_GROUP_START(powerDist) * it takes time to start up the motor. Then a * common value is between 3000 - 6000. */ -PARAM_ADD_CORE(PARAM_UINT32, idleThrust, &idleThrust) +PARAM_ADD_CORE(PARAM_UINT32 | PARAM_PERSISTENT, idleThrust, &idleThrust) PARAM_GROUP_STOP(powerDist) /** diff --git a/src/modules/src/queuemonitor.c b/src/modules/src/queuemonitor.c index 35463266b9..bdfc05d8b5 100644 --- a/src/modules/src/queuemonitor.c +++ b/src/modules/src/queuemonitor.c @@ -28,7 +28,7 @@ #include "queuemonitor.h" -#ifdef DEBUG_QUEUE_MONITOR +#ifdef CONFIG_DEBUG_QUEUE_MONITOR #include #include "timers.h" @@ -171,4 +171,4 @@ static void timerHandler(xTimerHandle timer) { debugPrint(); } -#endif // DEBUG_QUEUE_MONITOR +#endif // CONFIG_DEBUG_QUEUE_MONITOR diff --git a/src/modules/src/sensfusion6.c b/src/modules/src/sensfusion6.c index 633e4c9d12..09d3db6e5b 100644 --- a/src/modules/src/sensfusion6.c +++ b/src/modules/src/sensfusion6.c @@ -30,16 +30,16 @@ #include "param.h" #include "physicalConstants.h" -//#define MADWICK_QUATERNION_IMU +#include "autoconf.h" -#ifdef MADWICK_QUATERNION_IMU +#ifdef CONFIG_IMU_MADGWICK_QUATERNION #define BETA_DEF 0.01f // 2 * proportional gain #else // MAHONY_QUATERNION_IMU #define TWO_KP_DEF (2.0f * 0.4f) // 2 * proportional gain #define TWO_KI_DEF (2.0f * 0.001f) // 2 * integral gain #endif -#ifdef MADWICK_QUATERNION_IMU +#ifdef CONFIG_IMU_MADGWICK_QUATERNION float beta = BETA_DEF; // 2 * proportional gain (Kp) #else // MAHONY_QUATERNION_IMU float twoKp = TWO_KP_DEF; // 2 * proportional gain (Kp) @@ -96,7 +96,7 @@ void sensfusion6UpdateQ(float gx, float gy, float gz, float ax, float ay, float } } -#ifdef MADWICK_QUATERNION_IMU +#ifdef CONFIG_IMU_MADGWICK_QUATERNION // Implementation of Madgwick's IMU and AHRS algorithms. // See: http://www.x-io.co.uk/open-source-ahrs-with-x-imu // @@ -171,8 +171,8 @@ static void sensfusion6UpdateQImpl(float gx, float gy, float gz, float ax, float qy *= recipNorm; qz *= recipNorm; } -#else // MAHONY_QUATERNION_IMU -// Madgwick's implementation of Mayhony's AHRS algorithm. +#else +// Madgwick's implementation of Mahony's AHRS algorithm. // See: http://www.x-io.co.uk/open-source-ahrs-with-x-imu // // Date Author Notes @@ -374,18 +374,18 @@ LOG_GROUP_STOP(sensfusion6) * accurate attitude measurements. */ PARAM_GROUP_START(sensfusion6) -#ifdef MADWICK_QUATERNION_IMU +#ifdef CONFIG_IMU_MADGWICK_QUATERNION PARAM_ADD(PARAM_FLOAT, beta, &beta) #else // MAHONY_QUATERNION_IMU /** * @brief Integral gain (default: 0.002) */ -PARAM_ADD_CORE(PARAM_FLOAT, kp, &twoKp) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kp, &twoKp) /** * @brief Propotional gain (default: 0.8) */ -PARAM_ADD_CORE(PARAM_FLOAT, ki, &twoKi) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki, &twoKi) #endif PARAM_ADD(PARAM_FLOAT, baseZacc, &baseZacc) PARAM_GROUP_STOP(sensfusion6) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index ea542726b8..2d2080c788 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -284,13 +284,14 @@ static void stabilizerTask(void* param) powerDistribution(&control); } +#ifdef CONFIG_DECK_USD // Log data to uSD card if configured - if ( usddeckLoggingEnabled() + if (usddeckLoggingEnabled() && usddeckLoggingMode() == usddeckLoggingMode_SynchronousStabilizer && RATE_DO_EXECUTE(usddeckFrequency(), tick)) { usddeckTriggerLogging(); } - +#endif calcSensorToOutputLatency(&sensorData); tick++; STATS_CNT_RATE_EVENT(&stabilizerRate); diff --git a/src/modules/src/system.c b/src/modules/src/system.c index c6de303900..2853aa38f0 100644 --- a/src/modules/src/system.c +++ b/src/modules/src/system.c @@ -70,8 +70,9 @@ #include "peer_localization.h" #include "cfassert.h" #include "i2cdev.h" +#include "autoconf.h" -#ifndef START_DISARMED +#ifndef CONFIG_MOTORS_START_DISARMED #define ARM_INIT true #else #define ARM_INIT false @@ -139,7 +140,7 @@ void systemInit(void) buzzerInit(); peerLocalizationInit(); -#ifdef APP_ENABLED +#ifdef CONFIG_APP_ENABLE appInit(); #endif @@ -166,15 +167,12 @@ void systemTask(void *arg) ledInit(); ledSet(CHG_LED, 1); -#ifdef DEBUG_QUEUE_MONITOR +#ifdef CONFIG_DEBUG_QUEUE_MONITOR queueMonitorInit(); #endif -#ifdef ENABLE_UART1 - uart1Init(9600); -#endif -#ifdef ENABLE_UART2 - uart2Init(115200); +#ifdef CONFIG_DEBUG_PRINT_ON_UART1 + uart1Init(115200); #endif initUsecTimer(); @@ -187,7 +185,11 @@ void systemTask(void *arg) commanderInit(); StateEstimatorType estimator = anyEstimator; + + #ifdef CONFIG_ESTIMATOR_KALMAN_ENABLE estimatorKalmanTaskInit(); + #endif + deckInit(); estimator = deckGetRequiredEstimator(); stabilizerInit(estimator); @@ -230,10 +232,14 @@ void systemTask(void *arg) pass = false; DEBUG_PRINT("stabilizer [FAIL]\n"); } + + #ifdef CONFIG_ESTIMATOR_KALMAN_ENABLE if (estimatorKalmanTaskTest() == false) { pass = false; DEBUG_PRINT("estimatorKalmanTask [FAIL]\n"); } + #endif + if (deckTest() == false) { pass = false; DEBUG_PRINT("deck [FAIL]\n"); @@ -425,7 +431,7 @@ PARAM_ADD_CORE(PARAM_INT8 | PARAM_RONLY, selftestPassed, &selftestPassed) /** * @brief Set to nonzero to force system to be armed */ -PARAM_ADD(PARAM_INT8, forceArm, &forceArm) +PARAM_ADD(PARAM_INT8 | PARAM_PERSISTENT, forceArm, &forceArm) PARAM_GROUP_STOP(sytem) diff --git a/src/platform/Kbuild b/src/platform/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/src/platform/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/src/platform/platform.h b/src/platform/interface/platform.h similarity index 86% rename from src/platform/platform.h rename to src/platform/interface/platform.h index 2ff1fe225e..5e50019a9c 100644 --- a/src/platform/platform.h +++ b/src/platform/interface/platform.h @@ -1,13 +1,13 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ * * Crazyflie control firmware * - * Copyright (C) 2011-2018 Bitcraze AB + * Copyright (C) 2011-2022 Bitcraze AB * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -29,23 +29,25 @@ #include #include "motors.h" +#include "autoconf.h" + #define PLATFORM_DEVICE_TYPE_STRING_MAX_LEN (32 + 1) #define PLATFORM_DEVICE_TYPE_MAX_LEN (4 + 1) typedef enum { - #ifdef SENSOR_INCLUDED_BMI088_BMP388 + #ifdef CONFIG_SENSORS_BMI088_BMP388 SensorImplementation_bmi088_bmp388, #endif - #ifdef SENSOR_INCLUDED_BMI088_SPI_BMP388 + #ifdef CONFIG_SENSORS_BMI088_SPI SensorImplementation_bmi088_spi_bmp388, #endif - #ifdef SENSOR_INCLUDED_MPU9250_LPS25H + #ifdef CONFIG_SENSORS_MPU9250_LPS25H SensorImplementation_mpu9250_lps25h, #endif - #ifdef SENSOR_INCLUDED_BOSCH + #ifdef CONFIG_SENSORS_BOSCH SensorImplementation_bosch, #endif @@ -61,7 +63,7 @@ typedef struct { } platformConfig_t; /** - * Initilizes all platform specific things. + * Initializes all platform specific things. */ int platformInit(void); diff --git a/src/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h new file mode 100644 index 0000000000..568c7b5513 --- /dev/null +++ b/src/platform/interface/platform_defaults.h @@ -0,0 +1,40 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2022 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * platform_defaults.h - platform specific default values + */ + +#pragma once + +#define __INCLUDED_FROM_PLATFORM_DEFAULTS__ + +#ifdef CONFIG_PLATFORM_CF2 + #include "platform_defaults_cf2.h" +#endif +#ifdef CONFIG_PLATFORM_BOLT + #include "platform_defaults_bolt.h" +#endif +#ifdef CONFIG_PLATFORM_TAG + #include "platform_defaults_tag.h" +#endif diff --git a/src/platform/interface/platform_defaults_bolt.h b/src/platform/interface/platform_defaults_bolt.h new file mode 100644 index 0000000000..08f42bb993 --- /dev/null +++ b/src/platform/interface/platform_defaults_bolt.h @@ -0,0 +1,43 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2022 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * platform_defaults_bolt.h - platform specific default values for the bolt platform + */ + +#pragma once + +#ifndef __INCLUDED_FROM_PLATFORM_DEFAULTS__ + #pragma GCC error "Do not include this file directly, include platform_defaults.h instead." +#endif + +// Defines for default values in the bolt platform + +// Default values for battery limits +#define DEFAULT_BAT_LOW_VOLTAGE 6.4f +#define DEFAULT_BAT_CRITICAL_LOW_VOLTAGE 6.0f +#define DEFAULT_BAT_LOW_DURATION_TO_TRIGGER_SEC 5 + +// Default value for system shutdown in minutes after radio silence. +// Requires kbuild config ENABLE_AUTO_SHUTDOWN to be activated. +#define DEFAULT_SYSTEM_SHUTDOWN_TIMEOUT_MIN 5 diff --git a/src/platform/interface/platform_defaults_cf2.h b/src/platform/interface/platform_defaults_cf2.h new file mode 100644 index 0000000000..019e612ac5 --- /dev/null +++ b/src/platform/interface/platform_defaults_cf2.h @@ -0,0 +1,44 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2022 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * platform_defaults_cf2.h - platform specific default values for the cf2 platform + */ + +#pragma once + +#ifndef __INCLUDED_FROM_PLATFORM_DEFAULTS__ + #pragma GCC error "Do not include this file directly, include platform_defaults.h instead." +#endif + +// Defines for default values in the cf2 platform + +// Default values for battery limits +#define DEFAULT_BAT_LOW_VOLTAGE 3.2f +#define DEFAULT_BAT_CRITICAL_LOW_VOLTAGE 3.0f +#define DEFAULT_BAT_LOW_DURATION_TO_TRIGGER_SEC 5 + +// Default value for system shutdown in minutes after radio silence. +// Requires kbuild config ENABLE_AUTO_SHUTDOWN to be activated. +#define DEFAULT_SYSTEM_SHUTDOWN_TIMEOUT_MIN 5 + diff --git a/src/platform/interface/platform_defaults_tag.h b/src/platform/interface/platform_defaults_tag.h new file mode 100644 index 0000000000..327a5cf9df --- /dev/null +++ b/src/platform/interface/platform_defaults_tag.h @@ -0,0 +1,43 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2022 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * platform_defaults_tag.h - platform specific default values for the Tag platform + */ + +#pragma once + +#ifndef __INCLUDED_FROM_PLATFORM_DEFAULTS__ + #pragma GCC error "Do not include this file directly, include platform_defaults.h instead." +#endif + +// Defines for default values in the tag platform + +// Default values for battery limits +#define DEFAULT_BAT_LOW_VOLTAGE 3.2f +#define DEFAULT_BAT_CRITICAL_LOW_VOLTAGE 3.0f +#define DEFAULT_BAT_LOW_DURATION_TO_TRIGGER_SEC 5 + +// Default value for system shutdown in minutes after radio silence. +// Requires kbuild config ENABLE_AUTO_SHUTDOWN to be activated. +#define DEFAULT_SYSTEM_SHUTDOWN_TIMEOUT_MIN 5 diff --git a/src/platform/src/Kbuild b/src/platform/src/Kbuild new file mode 100644 index 0000000000..80a513e5db --- /dev/null +++ b/src/platform/src/Kbuild @@ -0,0 +1,6 @@ +obj-$(CONFIG_PLATFORM_BOLT) += platform_bolt.o +obj-$(CONFIG_PLATFORM_CF2) += platform_cf2.o +obj-$(CONFIG_PLATFORM_TAG) += platform_tag.o +obj-y += platform.o +obj-y += platform_stm32f4.o +obj-y += platform_utils.o diff --git a/src/platform/platform.c b/src/platform/src/platform.c similarity index 99% rename from src/platform/platform.c rename to src/platform/src/platform.c index afb09ba947..f1dccf98b2 100644 --- a/src/platform/platform.c +++ b/src/platform/src/platform.c @@ -113,4 +113,3 @@ bool platformConfigPhysicalLayoutAntennasAreClose() { const MotorPerifDef** platformConfigGetMotorMapping() { return active_config->motorMap; } - diff --git a/src/platform/src/platform_bolt.c b/src/platform/src/platform_bolt.c new file mode 100644 index 0000000000..6a08cd5459 --- /dev/null +++ b/src/platform/src/platform_bolt.c @@ -0,0 +1,82 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2022 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * platform_bolt.c - platform functionality for the Crazyflie Bolt + */ + + +#define DEBUG_MODULE "PLATFORM" + +#include + +#include "platform.h" +#include "exti.h" +#include "nvic.h" +#include "debug.h" + +static platformConfig_t configs[] = { +#ifdef CONFIG_SENSORS_BMI088_SPI + { // Old ID of Crazyflie Bolt + .deviceType = "RZ10", + .deviceTypeName = "Crazyflie Bolt", + .sensorImplementation = SensorImplementation_bmi088_spi_bmp388, + .physicalLayoutAntennasAreClose = false, + .motorMap = motorMapBoltBrushless, + }, + { + .deviceType = "CB10", + .deviceTypeName = "Crazyflie Bolt", + .sensorImplementation = SensorImplementation_bmi088_spi_bmp388, + .physicalLayoutAntennasAreClose = false, + .motorMap = motorMapBoltBrushless, + }, + { + .deviceType = "CB11", + .deviceTypeName = "Crazyflie Bolt 1.1", + .sensorImplementation = SensorImplementation_bmi088_spi_bmp388, + .physicalLayoutAntennasAreClose = false, + .motorMap = motorMapBolt11Brushless, + } +#endif +}; + +const platformConfig_t* platformGetListOfConfigurations(int* nrOfConfigs) { + *nrOfConfigs = sizeof(configs) / sizeof(platformConfig_t); + return configs; +} + +void platformInitHardware() { + //Low level init: Clock and Interrupt controller + nvicInit(); + + //EXTI interrupts + extiInit(); +} + + +// Config functions ------------------------ + +const char* platformConfigGetPlatformName() { + return "bolt"; +} diff --git a/src/platform/platform_cf2.c b/src/platform/src/platform_cf2.c similarity index 78% rename from src/platform/platform_cf2.c rename to src/platform/src/platform_cf2.c index fb250dad5d..00bc1eb870 100644 --- a/src/platform/platform_cf2.c +++ b/src/platform/src/platform_cf2.c @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2011-2018 Bitcraze AB + * Copyright (C) 2011-2022 Bitcraze AB * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -34,6 +34,7 @@ #include "debug.h" static platformConfig_t configs[] = { +#ifdef CONFIG_SENSORS_MPU9250_LPS25H { .deviceType = "CF20", .deviceTypeName = "Crazyflie 2.0", @@ -41,6 +42,8 @@ static platformConfig_t configs[] = { .physicalLayoutAntennasAreClose = true, .motorMap = motorMapDefaultBrushed, }, +#endif +#ifdef CONFIG_SENSORS_BMI088_BMP388 { .deviceType = "CF21", .deviceTypeName = "Crazyflie 2.1", @@ -48,20 +51,7 @@ static platformConfig_t configs[] = { .physicalLayoutAntennasAreClose = false, .motorMap = motorMapDefaultBrushed, }, - { // Old ID of Crzyflie Bolt - .deviceType = "RZ10", - .deviceTypeName = "Crazyflie Bolt", - .sensorImplementation = SensorImplementation_bmi088_spi_bmp388, - .physicalLayoutAntennasAreClose = false, - .motorMap = motorMapBoltBrushless, - }, - { - .deviceType = "CB10", - .deviceTypeName = "Crazyflie Bolt", - .sensorImplementation = SensorImplementation_bmi088_spi_bmp388, - .physicalLayoutAntennasAreClose = false, - .motorMap = motorMapBoltBrushless, - } +#endif }; const platformConfig_t* platformGetListOfConfigurations(int* nrOfConfigs) { diff --git a/src/platform/platform_stm32f4.c b/src/platform/src/platform_stm32f4.c similarity index 100% rename from src/platform/platform_stm32f4.c rename to src/platform/src/platform_stm32f4.c diff --git a/src/platform/platform_tag.c b/src/platform/src/platform_tag.c similarity index 100% rename from src/platform/platform_tag.c rename to src/platform/src/platform_tag.c diff --git a/src/platform/platform_utils.c b/src/platform/src/platform_utils.c similarity index 100% rename from src/platform/platform_utils.c rename to src/platform/src/platform_utils.c diff --git a/src/utils/Kbuild b/src/utils/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/src/utils/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/src/utils/interface/debug.h b/src/utils/interface/debug.h index 842a5976b3..f6bb63b136 100644 --- a/src/utils/interface/debug.h +++ b/src/utils/interface/debug.h @@ -25,8 +25,9 @@ */ #include "config.h" #include "console.h" +#include "autoconf.h" -#ifdef DEBUG_PRINT_ON_UART +#ifdef CONFIG_DEBUG_PRINT_ON_UART1 #include "uart1.h" #define uartPrintf uart1Printf #endif @@ -49,7 +50,7 @@ void debugInit(void); // Empty defines when running unit tests #define DEBUG_PRINT(fmt, ...) #define DEBUG_PRINT_OS(fmt, ...) -#elif defined(DEBUG_PRINT_ON_UART) +#elif defined(CONFIG_DEBUG_PRINT_ON_UART1) #define DEBUG_PRINT(fmt, ...) uartPrintf(DEBUG_FMT(fmt), ##__VA_ARGS__) #define DEBUG_PRINT_OS(fmt, ...) uartPrintf(DEBUG_FMT(fmt), ##__VA_ARGS__) #elif defined(DEBUG_PRINT_ON_SWO) diff --git a/src/utils/src/Kbuild b/src/utils/src/Kbuild new file mode 100644 index 0000000000..19d9450b48 --- /dev/null +++ b/src/utils/src/Kbuild @@ -0,0 +1,43 @@ +obj-y += cfassert.o +obj-y += clockCorrectionEngine.o +obj-y += configblockeeprom.o +obj-y += cpuid.o +obj-y += crc32.o +obj-y += debug.o +obj-y += eprintf.o + +### Implementations for abort(), malloc() and free(), needed to interact with apps written in C++ +obj-y += abort.o +obj-y += malloc.o + +### Add rules for handling generated version.c + +obj-y += version.o +version-objs := version_gen.o + +src/utils/src/version_gen.c: src/utils/src/version.vtpl + $(PYTHON) $(srctree)/tools/make/versionTemplate.py --crazyflie-base $(srctree) $< $@ + +### + +obj-y += filter.o +obj-y += FreeRTOS-openocd.o +obj-y += kve/kve.o +obj-y += kve/kve_storage.o + +# Lighthouse +obj-y += lighthouse/lighthouse_calibration.o +obj-y += lighthouse/lighthouse_geometry.o +obj-y += lighthouse/ootx_decoder.o +obj-y += lighthouse/pulse_processor.o +obj-y += lighthouse/pulse_processor_v1.o +obj-y += lighthouse/pulse_processor_v2.o +obj-y += num.o +obj-y += rateSupervisor.o +obj-y += sleepus.o +obj-y += statsCnt.o + +# TDoA +obj-y += tdoa/tdoaEngine.o +obj-y += tdoa/tdoaStats.o +obj-y += tdoa/tdoaStorage.o diff --git a/src/utils/src/kve/kve_storage.c b/src/utils/src/kve/kve_storage.c index b025ab0038..870e15f180 100644 --- a/src/utils/src/kve/kve_storage.c +++ b/src/utils/src/kve/kve_storage.c @@ -135,7 +135,7 @@ size_t kveStorageFindItemByPrefix(kveMemory_t *kve, size_t address, const char *prefix, char *keyBuffer, size_t *itemAddress) { - static char searchBuffer[255]; + static uint8_t searchBuffer[255]; size_t currentAddress = address; uint16_t length; uint8_t keyLength; diff --git a/src/utils/src/lighthouse/pulse_processor_v2.c b/src/utils/src/lighthouse/pulse_processor_v2.c index f5b865ddb1..66e31c906c 100644 --- a/src/utils/src/lighthouse/pulse_processor_v2.c +++ b/src/utils/src/lighthouse/pulse_processor_v2.c @@ -151,14 +151,19 @@ TESTABLE_STATIC bool processWorkspaceBlock(const pulseProcessorFrame_t slots[], TESTABLE_STATIC void augmentFramesInWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorkspace) { const int slotsUsed = pulseWorkspace->slotsUsed; - for (int i = 0; i < slotsUsed - 1; i++) { - pulseProcessorFrame_t* previousFrame = &pulseWorkspace->slots[i]; - const pulseProcessorFrame_t* frame = &pulseWorkspace->slots[i + 1]; - if (! previousFrame->channelFound) { - if (frame->channelFound) { - previousFrame->channel = frame->channel; - previousFrame->channelFound = frame->channelFound; - i++; + bool channelIsKnown = false; + uint8_t channel = 0; + + for (int i = slotsUsed - 1; i >= 0; i--) { + pulseProcessorFrame_t* frame = &pulseWorkspace->slots[i]; + + if (frame->channelFound) { + channel = frame->channel; + channelIsKnown = true; + } else { + if (channelIsKnown) { + frame->channel = channel; + frame->channelFound = true; } } } @@ -212,7 +217,7 @@ TESTABLE_STATIC void clearWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorks pulseWorkspace->slotsUsed = 0; } -static bool processFrame(const pulseProcessorFrame_t* frameData, pulseProcessorV2PulseWorkspace_t* pulseWorkspace, pulseProcessorV2BlockWorkspace_t* blockWorkspace) { +TESTABLE_STATIC bool processFrame(const pulseProcessorFrame_t* frameData, pulseProcessorV2PulseWorkspace_t* pulseWorkspace, pulseProcessorV2BlockWorkspace_t* blockWorkspace) { int nrOfBlocks = 0; // Sensor timestamps may arrive in the wrong order, we need an abs() when checking the diff diff --git a/src/utils/src/malloc.c b/src/utils/src/malloc.c new file mode 100644 index 0000000000..48cfa7a409 --- /dev/null +++ b/src/utils/src/malloc.c @@ -0,0 +1,22 @@ +#include "FreeRTOS.h" + +void* malloc(size_t size) +{ + void* ptr = NULL; + + if (size > 0) + { + ptr = pvPortMalloc(size); + } + + return ptr; +} + +void free(void* ptr) +{ + if (ptr) + { + vPortFree(ptr); + } +} + diff --git a/test/deck/core/test_deck_memory.c b/test/deck/core/test_deck_memory.c index 5be96bc20a..e7e5185f41 100644 --- a/test/deck/core/test_deck_memory.c +++ b/test/deck/core/test_deck_memory.c @@ -13,13 +13,16 @@ bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_ // Fixtures -uint8_t stockDeckProperties; -uint8_t stockDeckPropertiesFcn() { return stockDeckProperties; } +uint8_t stockDeckPropertiesPrimary; +uint8_t stockDeckPropertiesPrimaryFcn() { return stockDeckPropertiesPrimary; } + +uint8_t stockDeckPropertiesSecondary; +uint8_t stockDeckPropertiesSecondaryFcn() { return stockDeckPropertiesSecondary; } uint32_t read_vAddr; uint8_t read_len; bool read_isCalled; -bool mockRead(const uint32_t vAddr, const uint8_t len, uint8_t* buffer) { read_isCalled = true; if (!read_vAddr) read_vAddr = vAddr; read_len += len; return true; } +bool mockRead(const uint32_t vAddr, const uint8_t len, uint8_t* buffer) { read_isCalled = true; read_vAddr = vAddr; read_len = len; return true; } uint32_t write_vAddr; uint8_t write_len; @@ -27,23 +30,28 @@ bool write_isCalled; bool mockWrite(const uint32_t vAddr, const uint8_t len, const uint8_t* buffer) { write_isCalled = true; write_vAddr = vAddr; write_len = len; return true; } char* stockName = "A name"; -DeckMemDef_t stockMemDef; +DeckMemDef_t stockPrimaryMemDef; +DeckMemDef_t stockSecondaryMemDef; DeckDriver stockDriver; DeckInfo stockInfo; // Constants -#define SIZE_ONE_DECK 0x40 -#define MAX_NR_DECKS 4 -#define BUF_SIZE (SIZE_ONE_DECK * MAX_NR_DECKS) +#define SIZE_ONE_DECK_INFO 0x20 + +#define DECK_0_INFO_PRIMARY (1 + 0 * SIZE_ONE_DECK_INFO) +#define DECK_0_INFO_SECONDARY (1 + 1 * SIZE_ONE_DECK_INFO) +#define DECK_1_INFO_PRIMARY (1 + 2 * SIZE_ONE_DECK_INFO) +#define DECK_1_INFO_SECONDARY (1 + 3 * SIZE_ONE_DECK_INFO) +#define DECK_2_INFO_PRIMARY (1 + 4 * SIZE_ONE_DECK_INFO) +#define DECK_2_INFO_SECONDARY (1 + 5 * SIZE_ONE_DECK_INFO) +#define DECK_3_INFO_PRIMARY (1 + 6 * SIZE_ONE_DECK_INFO) +#define DECK_3_INFO_SECONDARY (1 + 7 * SIZE_ONE_DECK_INFO) + +#define BUF_SIZE 0xff uint8_t buffer[BUF_SIZE]; -#define DECK_0 (1 + 0 * SIZE_ONE_DECK) -#define DECK_1 (1 + 1 * SIZE_ONE_DECK) -#define DECK_2 (1 + 2 * SIZE_ONE_DECK) -#define DECK_3 (1 + 3 * SIZE_ONE_DECK) - -#define DECK_MEM_SIZE 0x10000000 +#define DECK_MEM_MAP_SIZE 0x10000000 const uint8_t MASK_IS_VALID = 1; const uint8_t MASK_IS_STARTED = 2; @@ -53,37 +61,25 @@ const uint8_t MASK_SUPPORTS_UPGRADE = 16; const uint8_t MASK_UPGRADE_REQUIRED = 32; const uint8_t MASK_BOOTLOADER_ACTIVE = 64; -// emulate the size the CRTP protocol will enforce -#define READ_CHUNK_SIZE 20 - -bool handleMemReadWrapper(uint32_t memAddr, size_t len, uint8_t *buffer) -{ - size_t bytesRead = 0; - - while (bytesRead < len) { - size_t readLen = len - bytesRead; - if (readLen > READ_CHUNK_SIZE) { - readLen = READ_CHUNK_SIZE; - } - if (!handleMemRead(memAddr + bytesRead, readLen, buffer + bytesRead)) { - return false; - } - bytesRead += readLen; - } - - return true; -} void setUp() { memset(buffer, 0, BUF_SIZE); - stockDeckProperties = 0; + stockDeckPropertiesPrimary = 0; + stockDeckPropertiesSecondary = 0; // Stock deck definition - memset(&stockMemDef, 0, sizeof(stockMemDef)); - stockMemDef.properties = stockDeckPropertiesFcn; + memset(&stockPrimaryMemDef, 0, sizeof(stockPrimaryMemDef)); + stockPrimaryMemDef.properties = stockDeckPropertiesPrimaryFcn; + + memset(&stockSecondaryMemDef, 0, sizeof(stockSecondaryMemDef)); + stockSecondaryMemDef.properties = stockDeckPropertiesSecondaryFcn; memset(&stockDriver, 0, sizeof(stockDriver)); - stockDriver.memoryDef = &stockMemDef; + stockDriver.memoryDef = &stockPrimaryMemDef; + + // Set no secondary memory as default to simplify testing + stockDriver.memoryDefSecondary = 0; + stockDriver.name = stockName; memset(&stockInfo, 0, sizeof(stockInfo)); @@ -98,268 +94,304 @@ void setUp() { write_len = 0; } -void testFirstByteIsVersion() { +void testFirstByteOfInfoIsVersion() { // Fixture - deckCount_IgnoreAndReturn(0); + deckCount_ExpectAndReturn(0); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, 1, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(1, buffer[0]); + TEST_ASSERT_EQUAL_UINT8(2, buffer[0]); } void testNoInstalledDecks() { // Fixture - deckCount_IgnoreAndReturn(0); + deckCount_ExpectAndReturn(0); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_0]); - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_1]); - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_2]); - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_3]); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_0_INFO_PRIMARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_0_INFO_SECONDARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_1_INFO_PRIMARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_1_INFO_SECONDARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_2_INFO_PRIMARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_2_INFO_SECONDARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_3_INFO_PRIMARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_3_INFO_SECONDARY] & MASK_IS_VALID); } -void testTwoDecskWithoutMemorySupport() { +void testTwoDecksWithoutMemorySupport() { // Fixture DeckDriver driverNoMem = { .memoryDef = 0 }; DeckInfo deckNoMem = { .driver = &driverNoMem }; - deckCount_IgnoreAndReturn(2); + deckCount_ExpectAndReturn(2); deckInfo_ExpectAndReturn(0, &deckNoMem); - deckInfo_IgnoreAndReturn(&deckNoMem); + deckInfo_ExpectAndReturn(1, &deckNoMem); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_0]); - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_1]); - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_2]); - TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_3]); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_0_INFO_PRIMARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_0_INFO_SECONDARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_1_INFO_PRIMARY] & MASK_IS_VALID); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_1_INFO_SECONDARY] & MASK_IS_VALID); } -void testOneDeckReadSupported() { +void testOneDeckReadPrimarySupported() { // Fixture - stockMemDef.read = mockRead; + stockPrimaryMemDef.read = mockRead; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_READ, buffer[DECK_0]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_READ, buffer[DECK_0_INFO_PRIMARY]); } -void testOneDeckWriteSupported() { +void testOneDeckWritePrimarySupported() { // Fixture - stockMemDef.write = mockWrite; + stockPrimaryMemDef.write = mockWrite; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_WRITE, buffer[DECK_0]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_WRITE, buffer[DECK_0_INFO_PRIMARY]); } -void testOneDeckUpgradeupported() { +void testOneDeckUpgradePrimarySupported() { // Fixture - stockMemDef.write = mockWrite; - stockMemDef.supportsUpgrade = true; + stockPrimaryMemDef.write = mockWrite; + stockPrimaryMemDef.supportsUpgrade = true; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_WRITE | MASK_SUPPORTS_UPGRADE, buffer[DECK_0]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_WRITE | MASK_SUPPORTS_UPGRADE, buffer[DECK_0_INFO_PRIMARY]); } -void testOneDeckStarted() { +void testOneDeckPrimaryStarted() { // Fixture - stockDeckProperties = DECK_MEMORY_MASK_STARTED; + stockDeckPropertiesPrimary = DECK_MEMORY_MASK_STARTED; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_IS_STARTED, buffer[DECK_0]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_IS_STARTED, buffer[DECK_0_INFO_PRIMARY]); } -void testOneDeckRequiresUpgrade() { +void testOneDeckRequiresPrimaryUpgrade() { // Fixture - stockDeckProperties = DECK_MEMORY_MASK_UPGRADE_REQUIRED; + stockDeckPropertiesPrimary = DECK_MEMORY_MASK_UPGRADE_REQUIRED; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_UPGRADE_REQUIRED, buffer[DECK_0]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_UPGRADE_REQUIRED, buffer[DECK_0_INFO_PRIMARY]); } -void testOneDeckInBootloaderMode() { +void testOneDeckPrimaryInBootloaderMode() { // Fixture - stockDeckProperties = DECK_MEMORY_MASK_BOOT_LOADER_ACTIVE; + stockDeckPropertiesPrimary = DECK_MEMORY_MASK_BOOT_LOADER_ACTIVE; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_BOOTLOADER_ACTIVE, buffer[DECK_0]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_BOOTLOADER_ACTIVE, buffer[DECK_0_INFO_PRIMARY]); } -void testOneDeckRequiredHash() { +void testOneDeckSecondaryInfoNotSetByPrimary() { // Fixture - stockMemDef.requiredHash = 0x12345678; + stockPrimaryMemDef.write = mockWrite; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(0x78, buffer[DECK_0 + 1]); - TEST_ASSERT_EQUAL_UINT8(0x56, buffer[DECK_0 + 2]); - TEST_ASSERT_EQUAL_UINT8(0x34, buffer[DECK_0 + 3]); - TEST_ASSERT_EQUAL_UINT8(0x12, buffer[DECK_0 + 4]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_WRITE, buffer[DECK_0_INFO_PRIMARY]); + TEST_ASSERT_EQUAL_UINT8(0, buffer[DECK_0_INFO_SECONDARY]); } -void testOneDeckRequiredLength() { +void testOneDeckAtleastOneSecondaryInfoBitIsSet() { // Fixture - stockMemDef.requiredSize = 0x12345678; + stockPrimaryMemDef.write = mockWrite; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + stockDriver.memoryDefSecondary = &stockSecondaryMemDef; + stockSecondaryMemDef.read = mockRead; + + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(0x78, buffer[DECK_0 + 5]); - TEST_ASSERT_EQUAL_UINT8(0x56, buffer[DECK_0 + 6]); - TEST_ASSERT_EQUAL_UINT8(0x34, buffer[DECK_0 + 7]); - TEST_ASSERT_EQUAL_UINT8(0x12, buffer[DECK_0 + 8]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_WRITE, buffer[DECK_0_INFO_PRIMARY]); + TEST_ASSERT_EQUAL_UINT8(MASK_IS_VALID | MASK_SUPPORTS_READ, buffer[DECK_0_INFO_SECONDARY]); } -void testBaseAddressSecondDeck() { +void testOneDeckRequiredHashPrimary() { // Fixture - deckCount_IgnoreAndReturn(2); - deckInfo_IgnoreAndReturn(&stockInfo); + stockPrimaryMemDef.requiredHash = 0x12345678; - uint32_t expected = DECK_MEM_SIZE * 2; + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, BUF_SIZE, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - uint32_t actual = 0; - memcpy(&actual, &buffer[DECK_1 + 0x0009], 4); - - TEST_ASSERT_EQUAL_UINT32(expected, actual); + TEST_ASSERT_EQUAL_UINT8(0x78, buffer[DECK_0_INFO_PRIMARY + 1]); + TEST_ASSERT_EQUAL_UINT8(0x56, buffer[DECK_0_INFO_PRIMARY + 2]); + TEST_ASSERT_EQUAL_UINT8(0x34, buffer[DECK_0_INFO_PRIMARY + 3]); + TEST_ASSERT_EQUAL_UINT8(0x12, buffer[DECK_0_INFO_PRIMARY + 4]); } -void testReadOfFiveFirstBytes() { +void testOneDeckRequiredLengthPrimary() { // Fixture - stockMemDef.requiredHash = 0x12345678; + stockPrimaryMemDef.requiredSize = 0x12345678; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(0, 5, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT8(1, buffer[0]); // Version - TEST_ASSERT_EQUAL_UINT32(1, buffer[1]); // valid bit - TEST_ASSERT_EQUAL_UINT32(0x78, buffer[2]); // Required hash - TEST_ASSERT_EQUAL_UINT32(0x56, buffer[3]); // Required hash - TEST_ASSERT_EQUAL_UINT32(0x34, buffer[4]); // Required hash + TEST_ASSERT_EQUAL_UINT8(0x78, buffer[DECK_0_INFO_PRIMARY + 5]); + TEST_ASSERT_EQUAL_UINT8(0x56, buffer[DECK_0_INFO_PRIMARY + 6]); + TEST_ASSERT_EQUAL_UINT8(0x34, buffer[DECK_0_INFO_PRIMARY + 7]); + TEST_ASSERT_EQUAL_UINT8(0x12, buffer[DECK_0_INFO_PRIMARY + 8]); } -void testReadNotFromZero() { +void testBaseAddressSecondDeckPrimary() { // Fixture - stockMemDef.requiredHash = 0x12345678; + deckCount_ExpectAndReturn(2); + deckInfo_ExpectAndReturn(0, &stockInfo); + deckInfo_ExpectAndReturn(1, &stockInfo); - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + uint32_t expected = DECK_MEM_MAP_SIZE * 2; // Test - handleMemReadWrapper(3, 2, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT32(0x56, buffer[0]); // Required hash - TEST_ASSERT_EQUAL_UINT32(0x34, buffer[1]); // Required hash + uint32_t actual = 0; + memcpy(&actual, &buffer[DECK_1_INFO_PRIMARY + 0x0009], 4); + + TEST_ASSERT_EQUAL_UINT32(expected, actual); } -void testReadOfFiveFirstBytesFromThirdDeck() { +void testBaseAddressSecondDeckSecondary() { // Fixture - stockMemDef.requiredHash = 0x12345678; + deckCount_ExpectAndReturn(2); + deckInfo_ExpectAndReturn(0, &stockInfo); + deckInfo_ExpectAndReturn(1, &stockInfo); + + stockDriver.memoryDefSecondary = &stockSecondaryMemDef; - deckCount_IgnoreAndReturn(3); - deckInfo_IgnoreAndReturn(&stockInfo); + uint32_t expected = DECK_MEM_MAP_SIZE * 2 + DECK_MEM_MAP_SIZE; // Test - handleMemReadWrapper(DECK_2, 5, buffer); + handleMemRead(0, BUF_SIZE, buffer); // Assert - TEST_ASSERT_EQUAL_UINT32(1, buffer[0]); // valid bit - TEST_ASSERT_EQUAL_UINT32(0x78, buffer[1]); // Required hash - TEST_ASSERT_EQUAL_UINT32(0x56, buffer[2]); // Required hash - TEST_ASSERT_EQUAL_UINT32(0x34, buffer[3]); // Required hash - TEST_ASSERT_EQUAL_UINT32(0x12, buffer[4]); // Required hash + uint32_t actual = 0; + memcpy(&actual, &buffer[DECK_1_INFO_SECONDARY + 0x0009], 4); + + TEST_ASSERT_EQUAL_UINT32(expected, actual); } -void testReadPartOfSecondAndThirdDeck() { +void testReadOfFiveFirstBytes() { // Fixture - DeckDriver driverNoMem = { .memoryDef = 0 }; - DeckInfo deckNoMem = { .driver = &driverNoMem }; + stockPrimaryMemDef.requiredHash = 0x12345678; - stockMemDef.requiredHash = 0x12345678; - - deckCount_IgnoreAndReturn(3); - deckInfo_IgnoreAndReturn(&deckNoMem); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - handleMemReadWrapper(DECK_2 - 1, 5, buffer); + handleMemRead(0, 5, buffer); // Assert - TEST_ASSERT_EQUAL_UINT32(0, buffer[0]); // last byte of deck 2 + TEST_ASSERT_EQUAL_UINT8(2, buffer[0]); // Version TEST_ASSERT_EQUAL_UINT32(1, buffer[1]); // valid bit TEST_ASSERT_EQUAL_UINT32(0x78, buffer[2]); // Required hash TEST_ASSERT_EQUAL_UINT32(0x56, buffer[3]); // Required hash TEST_ASSERT_EQUAL_UINT32(0x34, buffer[4]); // Required hash } -void testReadFromDeck() { +void testReadNotFromZero() { + // Fixture + stockPrimaryMemDef.requiredHash = 0x12345678; + + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); + + // Test + handleMemRead(3, 2, buffer); + + // Assert + TEST_ASSERT_EQUAL_UINT32(0x56, buffer[0]); // Required hash + TEST_ASSERT_EQUAL_UINT32(0x34, buffer[1]); // Required hash +} + +void testReadFromPrimaryDeckMemory() { + // Fixture + stockPrimaryMemDef.read = mockRead; + + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); + + // Test + bool actual = handleMemRead(DECK_MEM_MAP_SIZE + 100, 30, buffer); + + // Assert + TEST_ASSERT_TRUE(read_isCalled); + TEST_ASSERT_EQUAL_UINT32(100, read_vAddr); + TEST_ASSERT_EQUAL_UINT8(30, read_len); + TEST_ASSERT_TRUE(actual) +} + +void testReadFromSecondaryDeckMemory() { // Fixture - stockMemDef.read = mockRead; + stockDriver.memoryDefSecondary = &stockSecondaryMemDef; + stockSecondaryMemDef.read = mockRead; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - bool actual = handleMemReadWrapper(DECK_MEM_SIZE + 100, 30, buffer); + bool actual = handleMemRead(DECK_MEM_MAP_SIZE * 2 + 100, 30, buffer); // Assert TEST_ASSERT_TRUE(read_isCalled); @@ -370,13 +402,13 @@ void testReadFromDeck() { void testReadFromDeckWithoutReadFunction() { // Fixture - stockMemDef.read = 0; + stockPrimaryMemDef.read = 0; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - bool actual = handleMemReadWrapper(DECK_MEM_SIZE + 100, 30, buffer); + bool actual = handleMemRead(DECK_MEM_MAP_SIZE + 100, 30, buffer); // Assert TEST_ASSERT_FALSE(read_isCalled); @@ -385,25 +417,43 @@ void testReadFromDeckWithoutReadFunction() { void testReadFromNonExistingDeck() { // Fixture - deckCount_IgnoreAndReturn(0); + deckCount_ExpectAndReturn(0); // Test - bool actual = handleMemReadWrapper(DECK_MEM_SIZE + 100, 30, buffer); + bool actual = handleMemRead(DECK_MEM_MAP_SIZE + 100, 30, buffer); // Assert TEST_ASSERT_FALSE(read_isCalled); TEST_ASSERT_FALSE(actual); } -void testWriteToDeck() { +void testWriteToPrimaryDeckMemory() { + // Fixture + stockPrimaryMemDef.write = mockWrite; + + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); + + // Test + bool actual = handleMemWrite(DECK_MEM_MAP_SIZE + 100, 30, buffer); + + // Assert + TEST_ASSERT_TRUE(write_isCalled); + TEST_ASSERT_EQUAL_UINT32(100, write_vAddr); + TEST_ASSERT_EQUAL_UINT8(30, write_len); + TEST_ASSERT_TRUE(actual) +} + +void testWriteToSecondaryDeckMemory() { // Fixture - stockMemDef.write = mockWrite; + stockSecondaryMemDef.write = mockWrite; + stockDriver.memoryDefSecondary = &stockSecondaryMemDef; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - bool actual = handleMemWrite(DECK_MEM_SIZE + 100, 30, buffer); + bool actual = handleMemWrite(DECK_MEM_MAP_SIZE * 2 + 100, 30, buffer); // Assert TEST_ASSERT_TRUE(write_isCalled); @@ -414,13 +464,13 @@ void testWriteToDeck() { void testWriteToDeckWithoutWriteFunction() { // Fixture - stockMemDef.write = 0; + stockPrimaryMemDef.write = 0; - deckCount_IgnoreAndReturn(1); - deckInfo_IgnoreAndReturn(&stockInfo); + deckCount_ExpectAndReturn(1); + deckInfo_ExpectAndReturn(0, &stockInfo); // Test - bool actual = handleMemWrite(DECK_MEM_SIZE + 100, 30, buffer); + bool actual = handleMemWrite(DECK_MEM_MAP_SIZE + 100, 30, buffer); // Assert TEST_ASSERT_FALSE(write_isCalled); @@ -429,10 +479,10 @@ void testWriteToDeckWithoutWriteFunction() { void testWriteToNonExistingDeck() { // Fixture - deckCount_IgnoreAndReturn(0); + deckCount_ExpectAndReturn(0); // Test - bool actual = handleMemWrite(DECK_MEM_SIZE + 100, 30, buffer); + bool actual = handleMemWrite(DECK_MEM_MAP_SIZE + 100, 30, buffer); // Assert TEST_ASSERT_FALSE(write_isCalled); diff --git a/test/utils/src/kve/test_kve.c b/test/utils/src/kve/test_kve.c new file mode 100644 index 0000000000..3fb7019a39 --- /dev/null +++ b/test/utils/src/kve/test_kve.c @@ -0,0 +1,215 @@ +// File under test kve.c +#include "kve/kve.h" +#include "kve/kve_storage.h" + +#include +#include +#include +#include + +#include "unity.h" + +#define KVE_PARTITION_LENGTH (7*1024) + +uint8_t kveData[KVE_PARTITION_LENGTH]; + +static size_t read(size_t address, void* data, size_t length) +{ + if ((length == 0) || (address + length > KVE_PARTITION_LENGTH)) { + return 0; + } + + memcpy(data, &kveData[address], length); + + return length; +} + +static size_t write(size_t address, const void* data, size_t length) +{ + if ((length == 0) || (address + length > KVE_PARTITION_LENGTH)) { + return 0; + } + + memcpy(&kveData[address], data, length); + + return length; +} + +static void flush(void) +{ + // Not valid for RAM memory implementation. +} + +static kveMemory_t kve = { + .memorySize = KVE_PARTITION_LENGTH, + .read = read, + .write = write, + .flush = flush, +}; + +static bool fromStorageOneKey(const char *key, void *buffer, size_t length) +{ + + (void)length; + (void)buffer; + TEST_ASSERT_EQUAL_STRING("prm/testEmpty", key); + +// For debug printing +// uint8_t *byteBuf = (uint8_t *)buffer; +// printf("%s:%i:", key, (int)length); +// +// for (int i = 0; i < (int)length; i++) +// { +// printf("%.2X", byteBuf[i]); +// } +// printf("\n"); + + return true; +} + +static bool fromFullStorageAllKeys(const char *key, void *buffer, size_t length) +{ + static uint32_t i = 0; + (void)length; + (void)buffer; + char keyString[30]; + +// For debug printing +// uint8_t *byteBuf = (uint8_t *)buffer; +// printf("%s:%i\n", key, (int)length); + + sprintf(keyString, "prm/test.value%i", i++); + TEST_ASSERT_EQUAL_STRING(keyString, key); + + return true; +} + +static void fillKveMemory(void) +{ + int i; + char keyString[30]; + // Fill memory + for (i = 0; i < (KVE_PARTITION_LENGTH / 10); i++) + { + sprintf(keyString, "prm/test.value%i", i); + if (!kveStore(&kve, keyString, &i, sizeof(i))) + { + break; + } + } + //printf("Nr stored:%i\n", i); +} + +//-----------------------------Test cases -------------------------------- // + +void setUp(void) { + // The full memory is initialized to zero + memset(kveData, 0, KVE_PARTITION_LENGTH); + kveFormat(&kve); +} + +void tearDown(void) { + // Empty +} + +void testSetupKve(void) { + // Fixture + // Test + bool actual = kveCheck(&kve); + + // Assert + TEST_ASSERT_EQUAL(true, actual); +} + +void testFetchEmpty(void) { + // Fixture + uint8_t buffer[8]; + // Test + bool actual = kveFetch(&kve, "testEmpty", buffer, 8); + + // Assert + TEST_ASSERT_EQUAL(false, actual); +} + +void testStoreAndReadFirstKeyValue(void) { + // Fixture + bool expectedStore = true; + int expectedRead = 4; + uint32_t u32Store = 0xBEAF; + uint32_t u32Read = 0; + // Test + bool actualStore = kveStore(&kve, "testEmpty", &u32Store, sizeof(uint32_t)); + int actualRead = kveFetch(&kve, "testEmpty", &u32Read, sizeof(uint32_t)); + +// printf("size:%i\n", actualRead); + + // Assert + TEST_ASSERT_EQUAL(expectedStore, actualStore); + TEST_ASSERT_EQUAL(expectedRead, actualRead); + TEST_ASSERT_EQUAL_UINT32(u32Store, u32Read); +} + +void testOneStoredForEach(void) { + // Fixture + uint32_t u32Store = 0xBEAF; + + // Test + bool actualStore = kveStore(&kve, "prm/testEmpty", &u32Store, sizeof(uint32_t)); + + // Assert + bool actual = kveForeach(&kve, "", fromStorageOneKey); + TEST_ASSERT_EQUAL(true, actualStore); + TEST_ASSERT_EQUAL(true, actual); +} + +void testFullStorageForEach(void) { + // Fixture + uint32_t u32Store = 0xBEAF; + // Fill memory + fillKveMemory(); + + // Test + // Assert + bool actual = kveForeach(&kve, "", fromFullStorageAllKeys); + TEST_ASSERT_EQUAL(true, actual); +} + +void testStoreWhenMemoryIsFull(void) { + // Fixture + bool expected = false; + uint32_t u32Store = 0xBEAF; + // Fill memory + fillKveMemory(); + // Test + bool actual = kveStore(&kve, "prm/full", &u32Store, sizeof(uint32_t)); + // Assert + TEST_ASSERT_EQUAL(expected, actual); +} + +void testRemoveAndStoreWhenMemoryIsFull(void) { + // Fixture + uint32_t u32Store = 0xBEAF; + // Fill memory + fillKveMemory(); + // Test + bool actualDelete = kveDelete(&kve, "prm/test.value10"); + bool actualStore = kveStore(&kve, "prm/test.hole10", &u32Store, sizeof(uint32_t)); + // Assert + TEST_ASSERT_EQUAL(true, actualDelete); + TEST_ASSERT_EQUAL(true, actualStore); +} + +void testRemoveAndStoreBiggerWhenMemoryIsFull(void) { + // Fixture + uint32_t u32Store = 0xBEAF; + // Fill memory + fillKveMemory(); + // Test + bool actualFull = kveStore(&kve, "prm/full", &u32Store, sizeof(uint32_t)); + bool actualDelete = kveDelete(&kve, "prm/test.value1"); + bool actualStore = kveStore(&kve, "prm/test.bigger1000", &u32Store, sizeof(uint32_t)); + // Assert + TEST_ASSERT_EQUAL(false, actualFull); + TEST_ASSERT_EQUAL(true, actualDelete); + TEST_ASSERT_EQUAL(false, actualStore); +} diff --git a/test/utils/src/lighthouse/test_pulse_processor_v2.c b/test/utils/src/lighthouse/test_pulse_processor_v2.c index 3efff2184f..db89347115 100644 --- a/test/utils/src/lighthouse/test_pulse_processor_v2.c +++ b/test/utils/src/lighthouse/test_pulse_processor_v2.c @@ -13,6 +13,7 @@ // Functions under test void clearWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorkspace); +int processFrame(const pulseProcessorFrame_t* frameData, pulseProcessorV2PulseWorkspace_t* pulseWorkspace, pulseProcessorV2BlockWorkspace_t* blockWorkspace); bool storePulse(const pulseProcessorFrame_t* frameData, pulseProcessorV2PulseWorkspace_t* pulseWorkspace); void augmentFramesInWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorkspace); bool processWorkspaceBlock(const pulseProcessorFrame_t slots[], pulseProcessorV2SweepBlock_t* block); @@ -28,10 +29,12 @@ static void addFrameToWs(uint8_t sensor, uint32_t timestamp, uint32_t offset, bo static void setUpOotxDecoderProcessBitCallCounter(); static void setUpSlowbitFrame(); static void clearSlowbitState(); +static void validateProcessFrame(int expectedNrOfBlocks, uint8_t sensor, uint32_t timestamp, uint32_t offset, uint8_t channel, bool channelFound); static pulseProcessor_t state; static pulseProcessorV2PulseWorkspace_t ws; static pulseProcessorV2SweepBlock_t block; +static pulseProcessorV2BlockWorkspace_t blockWorkspace; static pulseProcessorFrame_t slowbitFrame; static int nrOfCallsToOotxDecoderProcessBit; @@ -138,6 +141,42 @@ void testThatCleanFramesAreAugmented() { TEST_ASSERT_TRUE(ws.slots[4].channelFound); } +void testThatMultipleFramesAreAugmented() { + // Fixture + uint8_t expectedChan1 = 2; + uint8_t expectedChan2 = 5; + + clearWorkspace(&ws); + + addFrameToWs(0, A_TS, NO_OFFSET, false, 0); + addFrameToWs(1, A_TS, NO_OFFSET, false, 0); + addFrameToWs(2, A_TS, NO_OFFSET, false, 0); + addFrameToWs(3, A_TS, AN_OFFSET, true, expectedChan1); + + addFrameToWs(0, A_TS, NO_OFFSET, false, 0); + addFrameToWs(1, A_TS, NO_OFFSET, false, 0); + addFrameToWs(2, A_TS, NO_OFFSET, false, 0); + addFrameToWs(3, A_TS, AN_OFFSET, true, expectedChan2); + + // Test + augmentFramesInWorkspace(&ws); + + // Assert + TEST_ASSERT_EQUAL_UINT8(expectedChan1, ws.slots[0].channel); + TEST_ASSERT_TRUE(ws.slots[0].channelFound); + TEST_ASSERT_EQUAL_UINT8(expectedChan1, ws.slots[1].channel); + TEST_ASSERT_TRUE(ws.slots[1].channelFound); + TEST_ASSERT_EQUAL_UINT8(expectedChan1, ws.slots[2].channel); + TEST_ASSERT_TRUE(ws.slots[2].channelFound); + + TEST_ASSERT_EQUAL_UINT8(expectedChan2, ws.slots[4].channel); + TEST_ASSERT_TRUE(ws.slots[4].channelFound); + TEST_ASSERT_EQUAL_UINT8(expectedChan2, ws.slots[5].channel); + TEST_ASSERT_TRUE(ws.slots[5].channelFound); + TEST_ASSERT_EQUAL_UINT8(expectedChan2, ws.slots[6].channel); + TEST_ASSERT_TRUE(ws.slots[6].channelFound); +} + void testThatUnCleanFramesAreAugmented() { // Fixture uint8_t expectedChan1 = 2; @@ -160,7 +199,7 @@ void testThatUnCleanFramesAreAugmented() { // Assert TEST_ASSERT_EQUAL_UINT8(expectedChan1, ws.slots[1].channel); - TEST_ASSERT_FALSE(ws.slots[3].channelFound); + TEST_ASSERT_TRUE(ws.slots[3].channelFound); TEST_ASSERT_EQUAL_UINT8(expectedChan2, ws.slots[4].channel); TEST_ASSERT_FALSE(ws.slots[7].channelFound); } @@ -272,6 +311,32 @@ void testThatProcessBlockSetsOffsets() { TEST_ASSERT_TRUE(ok); } + +// Sensor: 1 TS:c53a8f Width: 10f Chan: None offset: 0 BeamWord:10b2c +// Sensor: 3 TS:c53a79 Width: 125 Chan: None offset: 0 BeamWord:1a42c +// Sensor: 2 TS:c53cbb Width: 11e Chan: 2(1) offset: 44731 BeamWord:1409a +// Sensor: 0 TS:c53cd4 Width: 11e Chan: 2(1) offset: 0 BeamWord:0269a + +void testThatProcessBlockSetsOffsetsWithOffsetOnThirdFrame() { + // Fixture + clearWorkspace(&ws); + addFrameToWs(SWEEP_SENS_0, OFFSET_BASE + TIMESTAMP_STEP * 0, NO_OFFSET, true, 3); + addFrameToWs(SWEEP_SENS_1, OFFSET_BASE + TIMESTAMP_STEP * 1, NO_OFFSET, true, 3); + addFrameToWs(SWEEP_SENS_2, OFFSET_BASE + TIMESTAMP_STEP * 2, OFFSET_BASE, true, 3); + addFrameToWs(SWEEP_SENS_3, OFFSET_BASE + TIMESTAMP_STEP * 3, NO_OFFSET, true, 3); + + // Test + bool ok = processWorkspaceBlock(&ws.slots[0], &block); + + // Assert + TEST_ASSERT_EQUAL_UINT32(OFFSET_BASE - TIMESTAMP_STEP * 2, block.offset[SWEEP_SENS_0]); + TEST_ASSERT_EQUAL_UINT32(OFFSET_BASE - TIMESTAMP_STEP * 1, block.offset[SWEEP_SENS_1]); + TEST_ASSERT_EQUAL_UINT32(OFFSET_BASE + TIMESTAMP_STEP * 0, block.offset[SWEEP_SENS_2]); + TEST_ASSERT_EQUAL_UINT32(OFFSET_BASE + TIMESTAMP_STEP * 1, block.offset[SWEEP_SENS_3]); + + TEST_ASSERT_TRUE(ok); +} + void testThatProcessBlockSetsOffsetsWhenTimeStampWraps() { // Fixture setOffsetBase(0x00ffff00); @@ -484,9 +549,69 @@ void testThatNonFullSlowbitMessageIsNotReported() { TEST_ASSERT_FALSE(actual); } +// Recording with one base station in a random orientation +void testRecordedSequence1Bs_1() { + // Fixture + clearWorkspace(&ws); + + // Test and assert + validateProcessFrame(0, 0, 2156620, 0, 0, false); + validateProcessFrame(0, 2, 2156972, 165916, 0, true); + validateProcessFrame(0, 1, 2157193, 0, 0, true); + validateProcessFrame(0, 3, 2157561, 0, 0, true); + + validateProcessFrame(1, 2, 2290186, 0, 0, false); + validateProcessFrame(0, 0, 2290608, 299556, 0, true); + validateProcessFrame(0, 3, 2290750, 0, 0, true); + validateProcessFrame(0, 1, 2291154, 0, 0, true); + + validateProcessFrame(1, 0, 2635114, 0, 0, false); + validateProcessFrame(0, 2, 2635466, 165920, 0, true); + validateProcessFrame(0, 1, 2635687, 0, 0, true); + validateProcessFrame(0, 3, 2636051, 0, 0, true); + + validateProcessFrame(1, 2, 2768686, 0, 0, false); + validateProcessFrame(0, 0, 2769102, 299556, 0, true); + validateProcessFrame(0, 3, 2769244, 0, 0, true); + validateProcessFrame(0, 1, 2769648, 0, 0, true); +} + +void testIssue901Data() { + // Fixture + clearWorkspace(&ws); + + // Test and assert + validateProcessFrame(0, 1, 0xc53a8f, 0, 0, false); + validateProcessFrame(0, 3, 0xc53a79, 0, 0, false); + validateProcessFrame(0, 2, 0xc53cbb, 44731, 0, true); + validateProcessFrame(0, 0, 0xc53cd4, 0, 0, true); + + // Fake data from now on + validateProcessFrame(1, 1, 0xc53a8f + 130000, 0, 0, false); + validateProcessFrame(0, 3, 0xc53a79 + 130000, 0, 0, false); + validateProcessFrame(0, 2, 0xc53cbb + 130000, 44731, 0, true); + validateProcessFrame(0, 0, 0xc53cd4 + 130000, 0, 0, true); +} + + // Helpers ------------------------------------------------ +static void validateProcessFrame(int expectedNrOfBlocks, uint8_t sensor, uint32_t timestamp, uint32_t offset, uint8_t channel, bool channelFound) { + pulseProcessorFrame_t frameData; + frameData.sensor = sensor; + frameData.timestamp = timestamp; + frameData.offset = offset; + frameData.channel = channel; + frameData.channelFound = channelFound; + + int actual = processFrame(&frameData, &ws, &blockWorkspace); + + char errorMsg[200]; + sprintf(errorMsg, "Failed for timestamp=%ul", timestamp); + TEST_ASSERT_EQUAL_INT32_MESSAGE(expectedNrOfBlocks, actual, errorMsg); +} + static void addFrameToWs(uint8_t sensor, uint32_t timestamp, uint32_t offset, bool channelFound, uint8_t channel) { pulseProcessorFrame_t frame; diff --git a/tools/build/check_elf b/tools/build/check_elf index f0d1cf1d0b..1b7b85b40d 100755 --- a/tools/build/check_elf +++ b/tools/build/check_elf @@ -2,4 +2,4 @@ set -e scriptDir=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -python3 ${scriptDir}/../verify/elf_sanity.py ${scriptDir}/../../*.elf +python3 ${scriptDir}/../verify/elf_sanity.py ${scriptDir}/../../build/*.elf diff --git a/tools/build/make_app b/tools/build/make_app new file mode 100755 index 0000000000..8539020154 --- /dev/null +++ b/tools/build/make_app @@ -0,0 +1,23 @@ +#!/usr/bin/env bash +set -e + +# Use this script to make an app, mainly intended for the toolbelt +# +# The scipt taks one requierd argument, which is the path to the app to build (must be in the source tree) +# Optional aguments can be added that will be passed on to make. +# +# "tb make_app " behaves as calling "make" in the directory where the app is located, meaning that any +# arguments for make can just be appended. +# +# Eamples: +# tb make_app examples/app_hello_world -j8 +# tb make_app examples/app_hello_world clean + +scriptDir=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +app_path="$1" +make_args="${@: 2}" + +cd ${app_path} +make ${make_args} +cd - diff --git a/tools/gen-dox/Doxyfile b/tools/gen-dox/Doxyfile index 0e56d0dc65..31573b757d 100644 --- a/tools/gen-dox/Doxyfile +++ b/tools/gen-dox/Doxyfile @@ -291,7 +291,7 @@ OPTIMIZE_OUTPUT_VHDL = NO # Note that for custom extensions you also need to set FILE_PATTERNS otherwise # the files are not read by doxygen. -EXTENSION_MAPPING = +EXTENSION_MAPPING = vtpl=C # If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments # according to the Markdown format, which allows for more readable @@ -817,6 +817,7 @@ INPUT_ENCODING = UTF-8 FILE_PATTERNS = *.c \ *.h \ + *.vtpl \ # The RECURSIVE tag can be used to specify whether or not subdirectories should # be searched for input files as well. @@ -2071,6 +2072,7 @@ PREDEFINED = "__attribute__(x)=" \ "PARAM_ADD(TYPE, NAME, ADDRESS)=TYPE NAME;" \ "PARAM_ADD_WITH_CALLBACK(TYPE, NAME, ADDRESS)=TYPE NAME;" \ "PARAM_ADD_CORE(TYPE, NAME, ADDRESS)=/** @@ingroup PARAM_CORE_GROUP */ TYPE NAME;" \ + "PARAM_ADD_CORE_WITH_CALLBACK(TYPE, NAME, ADDRESS, CALLBACK)=/** @@ingroup PARAM_CORE_GROUP */ TYPE NAME;" \ "PARAM_GROUP_START(NAME)=class fake_param_class_##NAME {" \ "PARAM_GROUP_STOP(NAME)=};" diff --git a/tools/kbuild/Makefile.kbuild b/tools/kbuild/Makefile.kbuild new file mode 100644 index 0000000000..010ea26974 --- /dev/null +++ b/tools/kbuild/Makefile.kbuild @@ -0,0 +1,827 @@ +VERSION = 1 +PATCHLEVEL = 0 +SUBLEVEL = 0 +EXTRAVERSION = +NAME = KBuild Template + +# *DOCUMENTATION* +# To see a list of typical targets execute "make help" +# More info can be located in ./README +# Comments in this file are targeted only to the developer, do not +# expect to learn how to build the kernel reading this file. + +# o Do not use make's built-in rules and variables +# (this increases performance and avoids hard-to-debug behaviour); +# o Look for make include files relative to root of kernel src +MAKEFLAGS += -rR --include-dir=$(CURDIR) + +# Avoid funny character set dependencies +unexport LC_ALL +LC_COLLATE=C +LC_NUMERIC=C +export LC_COLLATE LC_NUMERIC + +# Avoid interference with shell env settings +unexport GREP_OPTIONS + +# We are using a recursive build, so we need to do a little thinking +# to get the ordering right. +# +# Most importantly: sub-Makefiles should only ever modify files in +# their own directory. If in some directory we have a dependency on +# a file in another dir (which doesn't happen often, but it's often +# unavoidable when linking the built-in.o targets which finally +# turn into firmware), we will call a sub make in that other dir, and +# after that we are sure that everything which is in that other dir +# is now up to date. +# +# The only cases where we need to modify files which have global +# effects are thus separated out and done before the recursive +# descending is started. They are now explicitly listed as the +# prepare rule. + +# Beautify output +# --------------------------------------------------------------------------- +# +# Normally, we echo the whole command before executing it. By making +# that echo $($(quiet)$(cmd)), we now have the possibility to set +# $(quiet) to choose other forms of output instead, e.g. +# +# quiet_cmd_cc_o_c = Compiling $(RELDIR)/$@ +# cmd_cc_o_c = $(CC) $(c_flags) -c -o $@ $< +# +# If $(quiet) is empty, the whole command will be printed. +# If it is set to "quiet_", only the short version will be printed. +# If it is set to "silent_", nothing will be printed at all, since +# the variable $(silent_cmd_cc_o_c) doesn't exist. +# +# A simple variant is to prefix commands with $(Q) - that's useful +# for commands that shall be hidden in non-verbose mode. +# +# $(Q)ln $@ :< +# +# If KBUILD_VERBOSE equals 0 then the above command will be hidden. +# If KBUILD_VERBOSE equals 1 then the above command is displayed. +# +# To put more focus on warnings, be less verbose as default +# Use 'make V=1' to see the full commands + +ifeq ("$(origin V)", "command line") + KBUILD_VERBOSE = $(V) +endif +ifndef KBUILD_VERBOSE + KBUILD_VERBOSE = 0 +endif + +ifeq ($(KBUILD_VERBOSE),1) + quiet = + Q = +else + quiet=quiet_ + Q = @ +endif + +# If the user is running make -s (silent mode), suppress echoing of +# commands + +ifneq ($(filter 4.%,$(MAKE_VERSION)),) # make-4 +ifneq ($(filter %s ,$(firstword x$(MAKEFLAGS))),) + quiet=silent_ +endif +else # make-3.8x +ifneq ($(filter s% -s%,$(MAKEFLAGS)),) + quiet=silent_ +endif +endif + +export quiet Q KBUILD_VERBOSE + +# kbuild supports saving output files in a separate directory. +# To locate output files in a separate directory two syntaxes are supported. +# In both cases the working directory must be the root of the kernel src. +# 1) O= +# Use "make O=dir/to/store/output/files/" +# +# 2) Set KBUILD_OUTPUT +# Set the environment variable KBUILD_OUTPUT to point to the directory +# where the output files shall be placed. +# export KBUILD_OUTPUT=dir/to/store/output/files/ +# make +# +# The O= assignment takes precedence over the KBUILD_OUTPUT environment +# variable. + +# KBUILD_SRC is set on invocation of make in OBJ directory +# KBUILD_SRC is not intended to be used by the regular user (for now) +ifeq ($(KBUILD_SRC),) + +# OK, Make called in directory where kernel src resides +# Do we want to locate output files in a separate directory? +ifeq ("$(origin O)", "command line") + KBUILD_OUTPUT := $(O) +endif + +# That's our default target when none is given on the command line +PHONY := _all +_all: + +# Cancel implicit rules on top Makefile +$(CURDIR)/Makefile Makefile: ; + +ifneq ($(words $(subst :, ,$(CURDIR))), 1) + $(error main directory cannot contain spaces nor colons) +endif + +ifneq ($(KBUILD_OUTPUT),) +# Invoke a second make in the output directory, passing relevant variables +# check that the output directory actually exists +saved-output := $(KBUILD_OUTPUT) +KBUILD_OUTPUT := $(shell mkdir -p $(KBUILD_OUTPUT) && cd $(KBUILD_OUTPUT) \ + && /bin/pwd) +$(if $(KBUILD_OUTPUT),, \ + $(error failed to create output directory "$(saved-output)")) + +PHONY += $(MAKECMDGOALS) sub-make + +$(filter-out _all sub-make $(CURDIR)/Makefile, $(MAKECMDGOALS)) _all: sub-make + @: + +sub-make: + $(Q)$(MAKE) -C $(KBUILD_OUTPUT) KBUILD_SRC=$(CURDIR) \ + -f $(CURDIR)/Makefile $(filter-out _all sub-make,$(MAKECMDGOALS)) + +# Leave processing to above invocation of make +skip-makefile := 1 +endif # ifneq ($(KBUILD_OUTPUT),) +endif # ifeq ($(KBUILD_SRC),) + +# We process the rest of the Makefile if this is the final invocation of make +ifeq ($(skip-makefile),) + +# Do not print "Entering directory ...", +# but we want to display it when entering to the output directory +# so that IDEs/editors are able to understand relative filenames. +MAKEFLAGS += --no-print-directory + +# Call a source code checker (by default, "sparse") as part of the +# C compilation. +# +# Use 'make C=1' to enable checking of only re-compiled files. +# Use 'make C=2' to enable checking of *all* source files, regardless +# of whether they are re-compiled or not. +# +# See the file "Documentation/sparse.txt" for more details, including +# where to get the "sparse" utility. + +ifeq ("$(origin C)", "command line") + KBUILD_CHECKSRC = $(C) +endif +ifndef KBUILD_CHECKSRC + KBUILD_CHECKSRC = 0 +endif + +PHONY += all +_all: all + +ifeq ($(KBUILD_SRC),) + # building in the source tree + srctree := . +else + ifeq ($(KBUILD_SRC)/,$(dir $(CURDIR))) + # building in a subdirectory of the source tree + srctree := .. + else + srctree := $(KBUILD_SRC) + endif +endif + +objtree := . +src := $(srctree) +obj := $(objtree) + +VPATH := $(srctree) + +export srctree objtree VPATH + +ARCH ?= $(SUBARCH) +CROSS_COMPILE ?= $(CONFIG_CROSS_COMPILE:"%"=%) + +KCONFIG_CONFIG ?= .config +export KCONFIG_CONFIG + +# SHELL used by kbuild +CONFIG_SHELL := $(shell if [ -x "$$BASH" ]; then echo $$BASH; \ + else if [ -x /bin/bash ]; then echo /bin/bash; \ + else echo sh; fi ; fi) + +HOSTCC = gcc +HOSTCXX = g++ +HOSTCFLAGS = -Wall -Wmissing-prototypes -Wstrict-prototypes -O2 +HOSTCFLAGS += -fomit-frame-pointer -std=gnu89 +HOSTCXXFLAGS = -O2 + +ifeq ($(shell $(HOSTCC) -v 2>&1 | grep -c "clang version"), 1) +# These flags are clang-only +HOSTCFLAGS += -Wno-unused-value -Wno-unused-parameter \ + -Wno-missing-field-initializers -fno-delete-null-pointer-checks \ + -Wno-format-security +else +# These flags are gcc-only +HOSTCFLAGS += -Wno-format-overflow +endif + +# Decide whether to build built-in, modular, or both. +# Normally, just do built-in. + +KBUILD_BUILTIN := 1 + +export KBUILD_BUILTIN +export KBUILD_CHECKSRC KBUILD_SRC + + +# We need some generic definitions (do not try to remake the file). +scripts/Kbuild.include: ; +include scripts/Kbuild.include + +# Make variables (CC, etc...) +AS = $(CROSS_COMPILE)as +LD = $(CROSS_COMPILE)gcc +CC = $(CROSS_COMPILE)gcc +CXX = $(CROSS_COMPILE)g++ +CPP = $(CC) -E +AR = $(CROSS_COMPILE)ar +NM = $(CROSS_COMPILE)nm +STRIP = $(CROSS_COMPILE)strip +OBJCOPY = $(CROSS_COMPILE)objcopy +OBJDUMP = $(CROSS_COMPILE)objdump +SIZE = $(CROSS_COMPILE)size + +ifneq ($(OOT_USES_CXX),) +LD = $(CROSS_COMPILE)g++ +endif + +AWK = awk +PERL = perl +PYTHON ?= python3 +CHECK = sparse + +CHECKFLAGS := -D__linux__ -Dlinux -D__STDC__ -Dunix -D__unix__ \ + -Wbitwise -Wno-return-void $(CF) +NOSTDINC_FLAGS = +CFLAGS_KERNEL = +AFLAGS_KERNEL = + +# Use firmwareINCLUDE when you must reference the include/ directory. +# Needed to be compatible with the O= option +firmwareINCLUDE := \ + $(if $(KBUILD_SRC), -I$(srctree)/include) \ + -Iinclude -include include/generated/autoconf.h + +KBUILD_CPPFLAGS := -D__firmware__ -fno-exceptions + +KBUILD_CFLAGS := -Wall -Wmissing-braces \ + -fno-strict-aliasing -ffunction-sections \ + -fdata-sections -Wdouble-promotion \ + -std=gnu11 -DCRAZYFLIE_FW $(INCLUDES) + +KBUILD_AFLAGS_KERNEL := +KBUILD_AFLAGS := -D__ASSEMBLY__ + +KERNELVERSION = $(VERSION)$(if $(PATCHLEVEL),.$(PATCHLEVEL)$(if $(SUBLEVEL),.$(SUBLEVEL)))$(EXTRAVERSION) + +export VERSION PATCHLEVEL SUBLEVEL KERNELVERSION +export ARCH CONFIG_SHELL HOSTCC HOSTCFLAGS CROSS_COMPILE AS LD CC CXX +export CPP AR NM STRIP OBJCOPY OBJDUMP +export MAKE AWK PERL PYTHON +export HOSTCXX HOSTCXXFLAGS CHECK CHECKFLAGS + +export KBUILD_CPPFLAGS NOSTDINC_FLAGS firmwareINCLUDE OBJCOPYFLAGS LDFLAGS +export KBUILD_CFLAGS CFLAGS_KERNEL +export KBUILD_AFLAGS AFLAGS_KERNEL +export KBUILD_AFLAGS_KERNEL KBUILD_CFLAGS_KERNEL +export KBUILD_ARFLAGS + +# Files to ignore in find ... statements + +export RCS_FIND_IGNORE := \( -name SCCS -o -name BitKeeper -o -name .svn -o \ + -name CVS -o -name .pc -o -name .hg -o -name .git \) \ + -prune -o +export RCS_TAR_IGNORE := --exclude SCCS --exclude BitKeeper --exclude .svn \ + --exclude CVS --exclude .pc --exclude .hg --exclude .git + +# =========================================================================== +# Rules shared between *config targets and build targets + +# Basic helpers built in scripts/ +PHONY += scripts_basic +scripts_basic: + $(Q)$(MAKE) $(build)=scripts/basic + +# To avoid any implicit rule to kick in, define an empty command. +scripts/basic/%: scripts_basic ; + +PHONY += outputmakefile +# outputmakefile generates a Makefile in the output directory, if using a +# separate output directory. This allows convenient use of make in the +# output directory. +outputmakefile: +ifneq ($(KBUILD_SRC),) + $(Q)ln -fsn $(srctree) source + $(Q)$(CONFIG_SHELL) $(srctree)/scripts/mkmakefile \ + $(srctree) $(objtree) $(VERSION) $(PATCHLEVEL) +endif + +# To make sure we do not include .config for any of the *config targets +# catch them early, and hand them over to scripts/kconfig/Makefile +# It is allowed to specify more targets when calling make, including +# mixing *config targets and build targets. +# For example 'make oldconfig all'. +# Detect when mixed targets is specified, and make a second invocation +# of make so .config is not included in this case either (for *config). + +version_h := include/generated/version.h + +no-dot-config-targets := clean mrproper distclean \ + cscope help% %docs check% coccicheck \ + $(version_h) headers_% archheaders archscripts \ + kernelversion %src-pkg + +config-targets := 0 +mixed-targets := 0 +dot-config := 1 + +ifneq ($(filter $(no-dot-config-targets), $(MAKECMDGOALS)),) + ifeq ($(filter-out $(no-dot-config-targets), $(MAKECMDGOALS)),) + dot-config := 0 + endif +endif + +ifneq ($(filter config %config,$(MAKECMDGOALS)),) + config-targets := 1 + ifneq ($(words $(MAKECMDGOALS)),1) + mixed-targets := 1 + endif +endif + +# install need also be processed one by one +ifneq ($(filter install,$(MAKECMDGOALS)),) + mixed-targets := 1 +endif + +ifeq ($(mixed-targets),1) +# =========================================================================== +# We're called with mixed targets (*config and build targets). +# Handle them one by one. + +PHONY += $(MAKECMDGOALS) __build_one_by_one + +$(filter-out __build_one_by_one, $(MAKECMDGOALS)): __build_one_by_one + @: + +__build_one_by_one: + $(Q)set -e; \ + for i in $(MAKECMDGOALS); do \ + $(MAKE) -f $(srctree)/Makefile $$i; \ + done + +else +ifeq ($(config-targets),1) +# =========================================================================== +# *config targets only - make sure prerequisites are updated, and descend +# in scripts/kconfig to make the *config target + +# Read arch specific Makefile to set KBUILD_DEFCONFIG as needed. +# KBUILD_DEFCONFIG may point out an alternative default configuration +# used for 'make defconfig' +-include arch/$(ARCH)/Makefile +export KBUILD_DEFCONFIG KBUILD_KCONFIG + +config: scripts_basic outputmakefile FORCE + $(Q)$(MAKE) $(build)=scripts/kconfig $@ + +%config: scripts_basic outputmakefile FORCE + $(Q)$(MAKE) $(build)=scripts/kconfig $@ + +else +# =========================================================================== +# Build targets only - this includes firmware, arch specific targets, clean +# targets and others. In general all targets except *config targets. + +# Additional helpers built in scripts/ +# Carefully list dependencies so we do not try to build scripts twice +# in parallel +PHONY += scripts +scripts: scripts_basic include/config/auto.conf include/config/tristate.conf + $(Q)$(MAKE) $(build)=$(@) + +ifeq ($(dot-config),1) +# Read in config +-include include/config/auto.conf + +# Read in dependencies to all Kconfig* files, make sure to run +# oldconfig if changes are detected. +-include include/config/auto.conf.cmd + +# To avoid any implicit rule to kick in, define an empty command +$(KCONFIG_CONFIG) include/config/auto.conf.cmd: ; + +# If .config is newer than include/config/auto.conf, someone tinkered +# with it and forgot to run make oldconfig. +# if auto.conf.cmd is missing then we are probably in a cleaned tree so +# we execute the config step to be sure to catch updated Kconfig files +include/config/%.conf: $(KCONFIG_CONFIG) include/config/auto.conf.cmd + $(Q)$(MAKE) -f $(srctree)/Makefile silentoldconfig + + +else +# Dummy target needed, because used as prerequisite +include/config/auto.conf: ; +endif # $(dot-config) + +# The all: target is the default when no target is given on the +# command line. +# This allow a user to issue only 'make' to build the application + + +# The arch Makefile can set ARCH_{CPP,A,C}FLAGS to override the default +# values of the respective KBUILD_* variables +ARCH_CPPFLAGS := +ARCH_AFLAGS := +-include arch/$(ARCH)/Makefile + +KBUILD_CFLAGS += $(call cc-option,-fno-delete-null-pointer-checks,) + +# Tell gcc to never replace conditional load with a non-conditional one +KBUILD_CFLAGS += $(call cc-option,--param=allow-store-data-races=0) + +ifeq ($(cc-name),clang) +KBUILD_CPPFLAGS += $(call cc-option,-Qunused-arguments,) +KBUILD_CPPFLAGS += $(call cc-option,-Wno-unknown-warning-option,) +KBUILD_CFLAGS += $(call cc-disable-warning, unused-variable) +KBUILD_CFLAGS += $(call cc-disable-warning, format-invalid-specifier) +KBUILD_CFLAGS += $(call cc-disable-warning, gnu) +# Quiet clang warning: comparison of unsigned expression < 0 is always false +KBUILD_CFLAGS += $(call cc-disable-warning, tautological-compare) +# CLANG uses a _MergedGlobals as optimization, but this breaks modpost, as the +# source of a reference will be _MergedGlobals and not on of the whitelisted names. +# See modpost pattern 2 +KBUILD_CFLAGS += $(call cc-option, -mno-global-merge,) +KBUILD_CFLAGS += $(call cc-option, -fcatch-undefined-behavior) +else + +# These warnings generated too much noise in a regular build. +# Use make W=1 to enable them (see scripts/Makefile.build) +KBUILD_CFLAGS += $(call cc-disable-warning, unused-but-set-variable) +KBUILD_CFLAGS += $(call cc-disable-warning, unused-const-variable) +endif + +ifdef CONFIG_FRAME_POINTER +KBUILD_CFLAGS += -fno-omit-frame-pointer -fno-optimize-sibling-calls +else +KBUILD_CFLAGS += -fomit-frame-pointer +endif + +KBUILD_CFLAGS += $(call cc-option, -fno-var-tracking-assignments) + +ifdef CONFIG_DEBUG_INFO +ifdef CONFIG_DEBUG_INFO_SPLIT +KBUILD_CFLAGS += $(call cc-option, -gsplit-dwarf, -g) +else +KBUILD_CFLAGS += -g +endif +KBUILD_AFLAGS += -Wa,-gdwarf-2 +endif +ifdef CONFIG_DEBUG_INFO_DWARF4 +KBUILD_CFLAGS += $(call cc-option, -gdwarf-4,) +endif + +ifdef CONFIG_DEBUG_INFO_REDUCED +KBUILD_CFLAGS += $(call cc-option, -femit-struct-debug-baseonly) \ + $(call cc-option,-fno-var-tracking) +endif + +# We trigger additional mismatches with less inlining +ifdef CONFIG_DEBUG_SECTION_MISMATCH +KBUILD_CFLAGS += $(call cc-option, -fno-inline-functions-called-once) +endif + +# arch Makefile may override CC so keep this after arch Makefile is included +#NOSTDINC_FLAGS += -nostdinc -isystem $(shell $(CC) -print-file-name=include) +CHECKFLAGS += $(NOSTDINC_FLAGS) + +# disable pointer signed / unsigned warnings in gcc 4.0 +KBUILD_CFLAGS += $(call cc-disable-warning, pointer-sign) + +# disable invalid "can't wrap" optimizations for signed / pointers +KBUILD_CFLAGS += $(call cc-option,-fno-strict-overflow) + +# conserve stack if available +KBUILD_CFLAGS += $(call cc-option,-fconserve-stack) + +# disallow errors like 'EXPORT_GPL(foo);' with missing header +KBUILD_CFLAGS += $(call cc-option,-Werror=implicit-int) + +# Prohibit date/time macros, which would make the build non-deterministic +KBUILD_CFLAGS += $(call cc-option,-Werror=date-time) + +# enforce correct pointer usage +# KBUILD_CFLAGS += $(call cc-option,-Werror=incompatible-pointer-types) + +# use the deterministic mode of AR if available +KBUILD_ARFLAGS := $(call ar-option,D) + +# check for 'asm goto' +ifeq ($(shell $(CONFIG_SHELL) $(srctree)/scripts/gcc-goto.sh $(CC)), y) + KBUILD_CFLAGS += -DCC_HAVE_ASM_GOTO + KBUILD_AFLAGS += -DCC_HAVE_ASM_GOTO +endif + +include scripts/Makefile.extrawarn + +# Add any arch overrides and user supplied CPPFLAGS, AFLAGS and CFLAGS as the +# last assignments +KBUILD_CPPFLAGS += $(ARCH_CPPFLAGS) $(KCPPFLAGS) +KBUILD_AFLAGS += $(ARCH_AFLAGS) $(KAFLAGS) +KBUILD_CFLAGS += $(ARCH_CFLAGS) $(KCFLAGS) + +# Default kernel image to build when no specific target is given. +# KBUILD_IMAGE may be overruled on the command line or +# set in the environment +# Also any assignments in arch/$(ARCH)/Makefile take precedence over +# this default value +export KBUILD_IMAGE ?= firmware + +# +# INSTALL_PATH specifies where to place the updated kernel and system map +# images. Default is /boot, but you can set it to other values +export INSTALL_PATH ?= ./install + +firmware-dirs := $(objs-y) +firmware-objs := $(patsubst %,%/built-in.o, $(objs-y)) +firmware-libs := $(patsubst %,%/lib.a, $(libs-y)) +firmware-all := $(firmware-objs) $(firmware-libs) + +ifneq ($(OOT_USES_CXX),) + firmware-libs += -lstdc++ +endif + +quiet_cmd_firmware = LD $@ + cmd_firmware = $(LD) $(image_LDFLAGS) $(LDFLAGS) $(firmware-objs) $(firmware-libs) -lm -o $@.elf + +firmware: $(firmware-all) FORCE + +$(call if_changed,firmware) + +$(PROG).elf: firmware + @cp firmware.elf $@ + +# The actual objects are generated when descending, +# make sure no implicit rule kicks in +$(sort $(firmware-all)): $(firmware-dirs) ; + +# Handle descending into subdirectories listed in $(firmware-dirs) +# Preset locale variables to speed up the build process. Limit locale +# tweaks to this spot to avoid wrong language settings when running +# make menuconfig etc. +# Error messages still appears in the original language + +PHONY += $(firmware-dirs) +$(firmware-dirs): prepare scripts + $(Q)$(MAKE) $(build)=$@ + + + +# Things we need to do before we recursively start building the application +# are listed in "prepare". +# A multi level approach is used. prepareN is processed before prepareN-1. +# version.h and scripts_basic is processed / created. + +# Listed in dependency order +PHONY += prepare prepare0 prepare1 prepare2 prepare3 + +# prepare3 is used to check if we are building in a separate output directory, +# and if so do: +# 1) Check that make has not been executed in the kernel src $(srctree) +prepare3: +ifneq ($(KBUILD_SRC),) + @$(kecho) ' Using $(srctree) as source for kernel' + $(Q)if [ -f $(srctree)/.config -o -f $(srctree)/include/config/auto.conf ]; then \ + echo >&2 " $(srctree) is not clean, please run 'make mrproper'"; \ + echo >&2 " in the '$(srctree)' directory.";\ + /bin/false; \ + fi; +endif + +# prepare2 creates a makefile if using a separate output directory +prepare2: prepare3 outputmakefile + +prepare1: prepare2 $(version_h) include/config/auto.conf + +archprepare: + +prepare0: prepare1 scripts_basic + $(Q)$(MAKE) $(build)=. + +# All the preparing.. +prepare: prepare0 + +# Generate some files +# --------------------------------------------------------------------------- + +# KERNELRELEASE can change from a few different places, meaning version.h +# needs to be updated, so this check is forced on all builds + +define filechk_version.h + (echo \#define firmware_VERSION_CODE $(shell \ + expr $(VERSION) \* 65536 + 0$(PATCHLEVEL) \* 256 + 0$(SUBLEVEL)); \ + echo '#define firmware_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))';) +endef + +$(version_h): $(srctree)/Makefile FORCE + $(call filechk,version.h) + + +PHONY += headerdep +headerdep: + $(Q)find $(srctree)/include/ -name '*.h' | xargs --max-args 1 \ + $(srctree)/scripts/headerdep.pl -I$(srctree)/include + + +### +# Cleaning is done on three levels. +# make clean Delete most generated files +# make mrproper Delete the current configuration, and all generated files +# make distclean Remove editor backup files, patch leftover files and the like + +# Directories & files removed with 'make clean' +CLEAN_DIRS += +CLEAN_FILES += firmware + +# Directories & files removed with 'make mrproper' +MRPROPER_DIRS += include/config include/generated .tmp_objdiff +MRPROPER_FILES += .config .config.old .version .old_version \ + cscope* GPATH GSYMS + +# clean - Delete most +# +clean: rm-dirs := $(CLEAN_DIRS) +clean: rm-files := $(CLEAN_FILES) +clean-dirs := $(addprefix _clean_, . $(firmware-dirs)) + +PHONY += $(clean-dirs) clean archclean +$(clean-dirs): + $(Q)$(MAKE) $(clean)=$(patsubst _clean_%,%,$@) + +clean: $(clean-dirs) clean_cf + $(call cmd,rmdirs) + $(call cmd,rmfiles) + @find . $(RCS_FIND_IGNORE) \ + \( -name '*.[oas]' -o -name '.*.cmd' \ + -o -name '.*.d' -o -name '.*.tmp' \ + -o -name '.tmp_*.o.*' \ + -o -name '*.gcno' \) -type f -print | xargs rm -f + +# mrproper - Delete all generated files, including .config +# +mrproper: rm-dirs := $(wildcard $(MRPROPER_DIRS)) +mrproper: rm-files := $(wildcard $(MRPROPER_FILES)) +mrproper-dirs := $(addprefix _mrproper_, scripts) + +PHONY += $(mrproper-dirs) mrproper +$(mrproper-dirs): + $(Q)$(MAKE) $(clean)=$(patsubst _mrproper_%,%,$@) + +mrproper: clean $(mrproper-dirs) + $(call cmd,rmdirs) + $(call cmd,rmfiles) + +# distclean +# +PHONY += distclean + +distclean: mrproper + @find $(srctree) $(RCS_FIND_IGNORE) \ + \( -name '*.orig' -o -name '*.rej' -o -name '*~' \ + -o -name '*.bak' -o -name '#*#' -o -name '.*.orig' \ + -o -name '.*.rej' -o -name '*%' -o -name 'core' \) \ + -type f -print | xargs rm -f + + +# Shorthand for $(Q)$(MAKE) -f scripts/Makefile.clean obj=dir +# Usage: +# $(Q)$(MAKE) $(clean)=dir +clean := -f $(if $(KBUILD_SRC),$(srctree)/)scripts/Makefile.clean obj + + +PHONY += help +help: + @echo 'Cleaning targets:' + @echo ' clean - Remove most generated files but keep the config' + @echo ' mrproper - Remove all generated files + config + various backup files' + @echo ' distclean - mrproper + remove editor backup and patch files' + @echo '' + @echo 'Configuration targets:' + @$(MAKE) -f $(srctree)/scripts/kconfig/Makefile help + @echo '' + @echo 'Other generic targets:' + @echo ' all - Build all targets marked with [*]' + @echo '* firmware - Build the application' + @echo ' dir/ - Build all files in dir and below' + @echo ' dir/file.[ois] - Build specified target only' + @echo ' dir/file.lst - Build specified mixed source/assembly target only' + @echo ' (requires a recent binutils and recent build (System.map))' + @echo ' kernelversion - Output the version stored in Makefile (use with make -s)' + echo '' + @echo 'Static analysers' + @echo ' includecheck - Check for duplicate included header files' + @echo ' headerdep - Detect inclusion cycles in headers' + @echo '' + @echo ' make V=0|1 [targets] 0 => quiet build (default), 1 => verbose build' + @echo ' make V=2 [targets] 2 => give reason for rebuild of target' + @echo ' make O=dir [targets] Locate all output files in "dir", including .config' + @echo ' make C=1 [targets] Check all c source with $$CHECK (sparse by default)' + @echo ' make C=2 [targets] Force check of all c source with $$CHECK' + @echo ' make W=n [targets] Enable extra gcc checks, n=1,2,3 where' + @echo ' 1: warnings which may be relevant and do not occur too often' + @echo ' 3: more obscure warnings, can most likely be ignored' + @echo ' Multiple levels can be combined with W=12 or W=123' + @echo '' + @echo 'Execute "make" or "make all" to build all targets marked with [*] ' + @echo 'For further info see the ./README file' + + +# Scripts to check various things for consistency +# --------------------------------------------------------------------------- + +PHONY += includecheck + +includecheck: + find $(srctree)/* $(RCS_FIND_IGNORE) \ + -name '*.[hcS]' -type f -print | sort \ + | xargs $(PERL) -w $(srctree)/scripts/checkincludes.pl + +endif #ifeq ($(config-targets),1) +endif #ifeq ($(mixed-targets),1) + +PHONY += kernelversion image_name + +kernelversion: + @echo $(KERNELVERSION) + +image_name: + @echo $(KBUILD_IMAGE) + + +# Single targets +# --------------------------------------------------------------------------- +# Single targets are compatible with: +# - build with mixed source and output +# - build with separate output dir 'make O=...' +# +# target-dir => where to store outputfile +# build-dir => directory in kernel source tree to use + +build-dir = $(patsubst %/,%,$(dir $@)) +target-dir = $(dir $@) + +%.s: %.c prepare scripts FORCE + $(Q)$(MAKE) $(build)=$(build-dir) $(target-dir)$(notdir $@) +%.i: %.c prepare scripts FORCE + $(Q)$(MAKE) $(build)=$(build-dir) $(target-dir)$(notdir $@) +%.o: %.c prepare scripts FORCE + $(Q)$(MAKE) $(build)=$(build-dir) $(target-dir)$(notdir $@) +%.lst: %.c prepare scripts FORCE + $(Q)$(MAKE) $(build)=$(build-dir) $(target-dir)$(notdir $@) +%.s: %.S prepare scripts FORCE + $(Q)$(MAKE) $(build)=$(build-dir) $(target-dir)$(notdir $@) +%.o: %.S prepare scripts FORCE + $(Q)$(MAKE) $(build)=$(build-dir) $(target-dir)$(notdir $@) +%.symtypes: %.c prepare scripts FORCE + $(Q)$(MAKE) $(build)=$(build-dir) $(target-dir)$(notdir $@) + +# FIXME Should go into a make.lib or something +# =========================================================================== + +quiet_cmd_rmdirs = $(if $(wildcard $(rm-dirs)),CLEAN $(wildcard $(rm-dirs))) + cmd_rmdirs = rm -rf $(rm-dirs) + +quiet_cmd_rmfiles = $(if $(wildcard $(rm-files)),CLEAN $(wildcard $(rm-files))) + cmd_rmfiles = rm -f $(rm-files) + +# read all saved command lines + +targets := $(wildcard $(sort $(targets))) +cmd_files := $(wildcard .*.cmd $(foreach f,$(targets),$(dir $(f)).$(notdir $(f)).cmd)) + +ifneq ($(cmd_files),) + $(cmd_files): ; # Do not try to update included dependency files + include $(cmd_files) +endif + +endif # skip-makefile + +PHONY += FORCE +FORCE: + +# Declare the contents of the .PHONY variable as phony. We keep that +# information in a variable so we can use it in if_changed and friends. +.PHONY: $(PHONY) diff --git a/tools/make/config.mk.example b/tools/make/config.mk.example deleted file mode 100644 index 31aa9fc7c7..0000000000 --- a/tools/make/config.mk.example +++ /dev/null @@ -1,133 +0,0 @@ -## Copy this file to config.mk and modify to get you personal build configuration - -## Weight of the Crazyflie, including decks. The default setting is a Crazyflie 2.X without decks. -# CFLAGS += -DCF_MASS=0.027f // in kg - -## Force device type string -# CFLAGS += -DDEVICE_TYPE_STRING_FORCE="CF20" - -## Force a sensor implementation to be used -# SENSORS=bosch - -## Set CRTP link to E-SKY receiver -# CFLAGS += -DUSE_ESKYLINK - -## Redirect the console output to the UART -# CFLAGS += -DDEBUG_PRINT_ON_UART - -## Redirect the console output to JLINK (using SEGGER RTT) -# DEBUG_PRINT_ON_SEGGER_RTT = 1 - -## Load a deck driver that has no OW memory -# CFLAGS += -DDECK_FORCE=bcBuzzer - -## Load multiple deck drivers that has no OW memory -# CFLAGS += -DDECK_FORCE=bcBuzzer:bcLedRing - -## Enable biq quad deck features -# CFLAGS += -DENABLE_BQ_DECK -# CFLAGS += -DBQ_DECK_ENABLE_PM -# CFLAGS += -DBQ_DECK_ENABLE_OSD - -## Use morse when flashing the LED to indicate that the Crazyflie is calibrated -# CFLAGS += -DCALIBRATED_LED_MORSE - -## Disable LEDs from turning on/flashing -# CFLAGS += -DTURN_OFF_LEDS - -## Set the default LED Ring effect (if not set, effect 6 will be used) -# CFLAGS += -DLEDRING_DEFAULT_EFFECT=0 - -## Set LED Rings to use less LEDs -# CFLAGS += -DLED_RING_NBR_LEDS=6 - -## Set LED Rings to use less more LEDs (only if board is modified) -# CFLAGS += -DLED_RING_NBR_LEDS=24 - -## Do not send CRTP messages when parameter values are updated -# CFLAGS += -DSILENT_PARAM_UPDATES - -## Turn on monitoring of queue usages -# CFLAGS += -DDEBUG_QUEUE_MONITOR - -## Automatically reboot to bootloader before flashing -# CLOAD_CMDS = -w radio://0/100/2M/E7E7E7E7E7 - -## Set number of anchor in LocoPositioningSystem -# CFLAGS += -DLOCODECK_NR_OF_ANCHORS=8 - -## Set alternative pins for LOCO deck (IRQ=IO_2, RESET=IO_3, default are RX1 and TX1) -# CFLAGS += -DLOCODECK_USE_ALT_PINS - -## Set other pin for reset on the LOCO deck. Only works when LOCODECK_USE_ALT_PINS is set. -# For instance useful with Loco + Lighhouse + Flow decks where IO_3 collides with the Flow deck -# CFLAGS += -DLOCODECK_ALT_PIN_RESET=DECK_GPIO_IO4 - -## Disable Low Interference Mode when using Loco Deck -# CFLAGS += -DLOCODECK_NO_LOW_INTERFERENCE - -## Low interference communication -# Set the 'low interference' 2.4GHz TX power. This power is set when the loco deck is initialized -# Possible power are: +4, 0, -4, -8, -12, -16, -20, -30 -# CFLAGS += -DPLATFORM_NRF51_LOW_INTERFERENCE_TX_POWER_DBM="(-12)" - -## Set alternative pins for uSD-deck. Patch soldering required (CS->RX2(PA3), SCLK->TX1(PC10), MISO->RX1(PC11), MOSI->IO_4(PC12)) -# CFLAGS += -DUSDDECK_USE_ALT_PINS_AND_SPI - -## Use J-Link as Debugger/flasher -# OPENOCD_INTERFACE ?= interface/jlink.cfg -# OPENOCD_TARGET ?= target/stm32f4x.cfg -# OPENOCD_CMDS ?= -c "transport select swd" - -## LPS settings ---------------------------------------------------- -## Set operation mode of the LPS system (auto is default) -## TWR -# CFLAGS += -DLPS_TWR_ENABLE=1 -## TDoA2 -# LPS_TDOA_ENABLE=1 -## TDoA3 -# LPS_TDOA3_ENABLE=1 - -## TDoA 3 - experimental -# Enable 2D positioning. The value (1.2) is the height that the tag will move at. Only use in TDoA 3 -# CFLAGS += -DLPS_2D_POSITION_HEIGHT=1.2 - -## Enable longer range (lower bit rate). Only use in TDoA 3 -# Note: Anchors must also be built with this flag -# CFLAGS += -DLPS_LONGER_RANGE - -## Full LPS TX power. -# CFLAGS += -DLPS_FULL_TX_POWER - -## SDCard test configuration ------------------------------------ -# FATFS_DISKIO_TESTS = 1 # Set to 1 to enable FatFS diskio function tests. Erases card. - -## Brushless handling -# Start disarmed, needs to be armed before being able to fly -# CFLAGS += -DSTART_DISARMED -# IDLE motor drive when armed, 0 = 0%, 65535 = 100% (the motors runs as long as the Crazyflie is armed) -# CFLAGS += -DDEFAULT_IDLE_THRUST=5000 -# Default PWM ratio to use when testing the propellers -# CFLAGS += -DDEFAULT_PROP_TEST_PWM_RATIO=10000 - -## Lighthouse handling -# If lighthouse will need to act as a ground truth (so not entered in the kalman filter) -# CFLAGS += -DLIGHTHOUSE_AS_GROUNDTRUTH - -## Improved altitude hold complementary filter -# CFLAGS += -DIMPROVED_BARO_Z_HOLD - -## External CPPM receiver (by default on PA7/MOSI) -# CFLAGS += -DDECK_FORCE=bcCPPM # force the CCPM deck if not using BigQuad -# CFLAGS += -DCPPM_USE_PA2 # use alternative pins for CPPM: PA2(TX2), PA3(RX2), PB4(IO_3), PB5(IO_2) or PB8(IO_1) -# CFLAGS += -DEXTRX_ALT_HOLD # enable altitude hold with external Rx -# CFLAGS += -DEXTRX_ARMING # enable arming with external Rx (setup via "Brushless handling" compile flags) -# CFLAGS += -DEXTRX_TAER # use TAER channel mapping instead of the default AETR - Aileron(Roll), Elevator(Pitch), Thrust, Rudder(Yaw) - -## Euler angles defining nonstandard IMU orientation on the airframe (in degrees) -# CFLAGS += -DIMU_PHI=0.0f -# CFLAGS += -DIMU_THETA=0.0f -# CFLAGS += -DIMU_PSI=0.0f - -## Keep the yaw setpoint within +/- YAW_MAX_DELTA from the current heading -# CFLAGS += -DYAW_MAX_DELTA=45.0f diff --git a/tools/make/oot.mk b/tools/make/oot.mk new file mode 100644 index 0000000000..8464843c9a --- /dev/null +++ b/tools/make/oot.mk @@ -0,0 +1,29 @@ +# +# We tell the firmware that this is an "Out-of-Tree" build so look for +# kbuild files in this directory. +# +OOT ?= $(PWD) + +# +# We need to tell the firmware where to find extra config options for this +# build. +# +OOT_CONFIG ?= $(OOT)/oot-config + +OOT_ARGS ?= -C $(CRAZYFLIE_BASE) OOT=$(OOT) EXTRA_CFLAGS=$(EXTRA_CFLAGS) + +ifneq ($(OOT_USES_CXX),) +OOT_ARGS += OOT_USES_CXX=1 +endif + +.PHONY: all clean + +all: + $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) KCONFIG_ALLCONFIG=$(OOT_CONFIG) alldefconfig + $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) -j 12 + +clean: + $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) clean + +cload: + $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) cload diff --git a/tools/make/platform.mk b/tools/make/platform.mk deleted file mode 100644 index 698417eb3a..0000000000 --- a/tools/make/platform.mk +++ /dev/null @@ -1,35 +0,0 @@ -# Platform handling - -# List all available platform -platforms_files = $(wildcard $(CRAZYFLIE_BASE)/tools/make/platforms/*.mk) -platforms_available = $(patsubst $(CRAZYFLIE_BASE)/tools/make/platforms/%.mk,%, $(platforms_files)) - -# Find out if the requested platform exists -ifeq ($(filter $(PLATFORM),$(platforms_available)),) -$(info Platform '$(PLATFORM)' not found!) -$(info -------------------) -override PLATFORM= -undefine PLATFORM -endif - -# If there is no platform requested or the platform does not exist, print some help -ifndef PLATFORM -PLATFORM_DOC=1 -include $(CRAZYFLIE_BASE)/tools/make/platforms/*.mk -$(info You must provide a platform with 'make PLATFORM=') -$(info Available platforms:) -$(foreach plat,$(platforms_available),$(info - $(plat) : $(PLATFORM_HELP_$(plat)))) -$(info -------------------) -$(info The platform will be written in 'current_platform.mk' and will be used by) -$(info default for subsequent calls.) -$(info if you want to reset and see this message again type 'make mrproper') -$(info -------------------) -$(error Platform not defined) -endif - -# Include the platform makefile configuration -include $(CRAZYFLIE_BASE)/tools/make/platforms/$(PLATFORM).mk - -# Write current platform in a file to make it stick for future call to make -$(shell echo "PLATFORM=$(PLATFORM)" > current_platform.mk) - diff --git a/tools/make/platforms/cf2.mk b/tools/make/platforms/cf2.mk deleted file mode 100644 index 3471b73b10..0000000000 --- a/tools/make/platforms/cf2.mk +++ /dev/null @@ -1,19 +0,0 @@ -# Make configuration for the Crazyflie 2 platform - -PLATFORM_HELP_cf2 = Crazyflie2 platform, includes Crazyflie 2.0, Crazyflie 2.1 and Bolt -PLATFORM_NAME_cf2 = CF2 platform - -CPU=stm32f4 - -######### Sensors configuration ########## -CFLAGS += -DSENSOR_INCLUDED_MPU9250_LPS25H -PROJ_OBJ += sensors_mpu9250_lps25h.o - -CFLAGS += -DSENSOR_INCLUDED_BMI088_BMP388 -CFLAGS += -DSENSOR_INCLUDED_BMI088_SPI_BMP388 -PROJ_OBJ += sensors_bmi088_bmp388.o sensors_bmi088_i2c.o sensors_bmi088_spi.o - -######### Stabilizer configuration ########## -ESTIMATOR ?= any -CONTROLLER ?= Any # one of Any, PID, Mellinger, INDI -POWER_DISTRIBUTION ?= stock diff --git a/tools/make/platforms/tag.mk b/tools/make/platforms/tag.mk deleted file mode 100644 index 8a2f2fd518..0000000000 --- a/tools/make/platforms/tag.mk +++ /dev/null @@ -1,19 +0,0 @@ -# Make configuration for the Tag platform - -PLATFORM_HELP_tag = Tag platform, includes Roadrunner -PLATFORM_NAME_tag = Tag platform - -CPU=stm32f4 - -######### Sensors configuration ########## -CFLAGS += -DSENSOR_INCLUDED_BMI088_BMP388 -PROJ_OBJ += sensors_bmi088_bmp388.o sensors_bmi088_i2c.o - -######### Stabilizer configuration ########## -ESTIMATOR ?= any -CONTROLLER ?= Any # one of Any, PID, Mellinger, INDI -POWER_DISTRIBUTION ?= stock - -######### COMPILE FLAGS ########## -CFLAGS += -DDECK_FORCE=bcDWM1000 -CFLAGS += -DSENSORS_IGNORE_BAROMETER_FAIL diff --git a/tools/make/targets.mk b/tools/make/targets.mk index cf65eac840..bb3db94ccd 100644 --- a/tools/make/targets.mk +++ b/tools/make/targets.mk @@ -32,12 +32,6 @@ CCS_COMMAND_SILENT=" CCS $@" @$(if $(QUIET), ,echo $(CCS_COMMAND$(VERBOSE)) ) @$(CCS_COMMAND) -LD_COMMAND=$(LD) $(LDFLAGS) $(foreach o,$(OBJ),$(BIN)/$(o)) -lm -o $@ -LD_COMMAND_SILENT=" LD $@" -$(PROG).elf: $(OBJ) - @$(if $(QUIET), ,echo $(LD_COMMAND$(VERBOSE)) ) - @$(LD_COMMAND) - HEX_COMMAND=$(OBJCOPY) $< -O ihex $@ HEX_COMMAND_SILENT=" COPY $@" $(PROG).hex: $(PROG).elf @@ -68,14 +62,10 @@ clean_o: clean_version @$(if $(QUIET), ,echo $(CLEAN_O_COMMAND$(VERBOSE)) ) @$(CLEAN_O_COMMAND) -CLEAN_COMMAND=rm -f cf*.elf cf*.hex cf*.bin cf*.dfu cf*.map cf*.py _cf*.so $(BIN)/dep/*.d $(BIN)/*.o $(BIN)/*.c +CLEAN_COMMAND=rm -f *.elf *.hex *.bin *.dfu *.map *.py _cf*.so CLEAN_COMMAND_SILENT=" CLEAN" -clean: +clean_cf: @$(if $(QUIET), ,echo $(CLEAN_COMMAND$(VERBOSE)) ) @$(CLEAN_COMMAND) - -MRPROPER_COMMAND=rm -f current_platform.mk *~ hal/src/*~ hal/interface/*~ tasks/src/*~ tasks/inc/*~ utils/src/*~ utils/inc/*~ tools/make/*~; rm -rf bin/dep/*.d $(BIN)/*.a $(BIN)/vendor/*.o -MRPROPER_COMMAND_SILENT=" MRPROPER" -mrproper: clean - @$(if $(QUIET), ,echo $(MRPROPER_COMMAND$(VERBOSE)) ) - @$(MRPROPER_COMMAND) + @rm -f $(srctree)/$(PROG).* + \ No newline at end of file diff --git a/tools/make/usb-bootloader.py b/tools/make/usb-bootloader.py new file mode 100644 index 0000000000..fc8850b76c --- /dev/null +++ b/tools/make/usb-bootloader.py @@ -0,0 +1,21 @@ +#send a USB request to the crazyflie, causing it to enter DFU mode (bootloader) automatically +#flash new firmware using 'make flash_dfu' + +from time import sleep +import usb +import usb.core + +#find connected crazyflie, usb vendor and product id = 0483:5740 +dev = usb.core.find(idVendor=0x0483, idProduct=0x5740) +if dev is None: + raise ValueError('Could not find any USB connected crazyflie') + +#send signal to enter bootloader +try: + dev.ctrl_transfer(bmRequestType=usb.TYPE_VENDOR, + bRequest=0x01, wValue=0x01, wIndex=2) +except IOError: + #io error expected because the crazyflie will not respond to USB request as it resets into the bootloader + #TODO usbd_cf_Setup function in firmware needs to return USBD_OK before rebooting to fix this + #sleep to allow time for crazyflie to get into DFU mode + sleep(0.5) \ No newline at end of file diff --git a/tools/test/gcc.yml b/tools/test/gcc.yml index 3d9e9d82db..9cab9cd8f9 100644 --- a/tools/test/gcc.yml +++ b/tools/test/gcc.yml @@ -24,6 +24,7 @@ compiler: - *source_path - *unit_tests_path - *mocks_path + - 'build/include/generated' - 'src/config' - 'src/deck/drivers/interface/' - 'src/deck/drivers/src/' @@ -50,7 +51,8 @@ compiler: - 'src/modules/src/' - 'src/modules/src/kalman_core/' - 'src/modules/src/lighthouse/' - - 'src/platform/' + - 'src/platform/interface/' + - 'src/platform/src/' - 'src/utils/interface/' - 'src/utils/interface/lighthouse/' - 'src/utils/interface/tdoa' diff --git a/vendor/Kbuild b/vendor/Kbuild new file mode 100644 index 0000000000..d4e746db65 --- /dev/null +++ b/vendor/Kbuild @@ -0,0 +1,285 @@ +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/CommonTables/arm_common_tables.o +obj-y += CMSIS/CMSIS/DSP/Source/CommonTables/arm_const_structs.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_pid_reset_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_pid_reset_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_pid_reset_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_sin_cos_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/ControllerFunctions/arm_sin_cos_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f64.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_opt_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_opt_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_max_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_max_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_max_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_max_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_mean_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_mean_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_mean_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_mean_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_min_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_min_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_min_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_min_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_power_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_power_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_power_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_power_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_rms_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_rms_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_rms_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_std_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_std_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_std_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_var_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_var_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_var_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_copy_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_copy_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_copy_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_copy_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_fill_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_fill_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_fill_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_fill_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_float_to_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_float_to_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_float_to_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q15_to_float.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q15_to_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q15_to_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q31_to_float.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q31_to_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q31_to_q7.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q7_to_float.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q7_to_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/SupportFunctions/arm_q7_to_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_bitreversal2.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_bitreversal.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix2_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix2_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix2_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix2_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix2_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix2_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix4_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix4_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix4_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix4_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix4_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix4_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix8_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_dct4_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_dct4_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_dct4_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_dct4_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_dct4_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_dct4_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_init_f32.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_init_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_init_q31.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_q15.o +obj-y += CMSIS/CMSIS/DSP/Source/TransformFunctions/arm_rfft_q31.o + +# FreeRTOS +obj-y += FreeRTOS/event_groups.o +obj-y += FreeRTOS/list.o +obj-y += FreeRTOS/portable/GCC/ARM_CM4F/port.o +obj-y += FreeRTOS/portable/MemMang/heap_4.o +obj-y += FreeRTOS/queue.o +obj-y += FreeRTOS/tasks.o +obj-y += FreeRTOS/timers.o + +# libdw1000 +obj-y += libdw1000/src/libdw1000.o +obj-y += libdw1000/src/libdw1000Spi.o From 5e229873e910f523d5253fe2502ab5542958eb82 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 21 Apr 2022 12:18:33 +0200 Subject: [PATCH 39/85] add dbg --- src/modules/src/cvmrs.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 9446bd030d..ed4e3e770a 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -4,6 +4,7 @@ #include "log.h" #include "aideck.h" #include "usec_time.h" +#include "debug.h" #define TASK_FREQ 10 @@ -13,7 +14,7 @@ typedef struct uint64_t timestamp; // usec timestamp from STM32 int16_t x; // compressed [mm] int16_t y; // compressed [mm] - int16_t z; // compressed + int16_t z; // compressed [mm] uint32_t quat; // compressed, see quatcompress.h } __attribute__((packed)) StatePacket_t; // static StatePacket_t cf_state; @@ -33,9 +34,7 @@ void appMain() logVarId_t z_id = logGetVarId("stateEstimateZ", "z"); logVarId_t quat_id = logGetVarId("stateEstimateZ", "quat"); - cpx_packet.route.destination = CPX_T_GAP8; - cpx_packet.route.function = CPX_F_APP; - cpx_packet.route.source = CPX_T_STM32; + cpxInitRoute(CPX_T_STM32, CPX_T_GAP8, CPX_F_APP, &cpx_packet.route); cpx_packet.dataLength = sizeof(StatePacket_t); StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; state_packet->cmd = 0; @@ -54,6 +53,7 @@ void appMain() state_packet->z = logGetInt(z_id); state_packet->quat = logGetInt(quat_id); - cpxSendPacketBlocking(&cpx_packet); + cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */); + // DEBUG_PRINT("cpxSendPacket\n"); } } \ No newline at end of file From eedef6cfef156a31b25563ed68af4f9f31d35f23 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 25 Apr 2022 16:45:42 +0200 Subject: [PATCH 40/85] reduce frequency to be able to stream images --- src/modules/src/cvmrs.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index ed4e3e770a..b13b6ec9b5 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -6,7 +6,7 @@ #include "usec_time.h" #include "debug.h" -#define TASK_FREQ 10 +#define TASK_FREQ 1 typedef struct { From b8e96e5a7b7989a13823838bae136357ebd664b8 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 27 Apr 2022 12:09:45 +0200 Subject: [PATCH 41/85] usecTime synchronization at first mocap event This is a hacky version, that re-uses the fact that mocap information is send via broadcasts at a high frequency. Requires to use the CFs in a mocap space for time synchronization. --- src/hal/interface/usec_time.h | 5 ++++ src/hal/src/usec_time.c | 10 +++++++ src/modules/src/crtp_localization_service.c | 29 +++++++++++++++++++++ 3 files changed, 44 insertions(+) diff --git a/src/hal/interface/usec_time.h b/src/hal/interface/usec_time.h index 8a48ea28ff..6c8160e934 100644 --- a/src/hal/interface/usec_time.h +++ b/src/hal/interface/usec_time.h @@ -32,6 +32,11 @@ */ void initUsecTimer(void); +/** + * Reset the microsecond-resolution timer to 0. + */ +void usecTimerReset(void); + /** * Get microsecond-resolution timestamp. */ diff --git a/src/hal/src/usec_time.c b/src/hal/src/usec_time.c index 6971d308ea..59d302880f 100644 --- a/src/hal/src/usec_time.c +++ b/src/hal/src/usec_time.c @@ -71,6 +71,16 @@ void initUsecTimer(void) isInit = true; } +void usecTimerReset(void) +{ + IF_DEBUG_ASSERT(isInit); + + const uint32_t zero = 0; + __atomic_store(&usecTimerHighCount, &zero, __ATOMIC_SEQ_CST); + + TIM7->CNT = 0; +} + uint64_t usecTimestamp(void) { IF_DEBUG_ASSERT(isInit); diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c index 585170d163..b5e28c835b 100644 --- a/src/modules/src/crtp_localization_service.c +++ b/src/modules/src/crtp_localization_service.c @@ -115,6 +115,7 @@ static bool enableLighthouseAngleStream = false; static float extPosStdDev = 0.01; static float extQuatStdDev = 4.5e-3; static bool isInit = false; +static bool isTimeSynced = false; static uint8_t my_id; static uint16_t tickOfLastPacket; // tick when last packet was received @@ -161,7 +162,29 @@ static void updateLogFromExtPos() ext_pose.z = ext_pos.z; } +/* This function is called from all localization events that use + broadcasts (i.e., pos, pos_packed, pose, pose_packed). The first + time such a broadcast is received, the usecTimer is reset to 0. + Since broadcasts are received simultanously at all Crazyflies, + this should reset the local on-board timer of all Crazyflies + at almost the same exact time. + + TODO: In the future, it would be better to have a dedicated CRTP + functionality for this, rather than "piggybacking" on the + localization service for the motion capture. +*/ + +static void syncTimeIfNeeded() +{ + if (!isTimeSynced) { + usecTimerReset(); + isTimeSynced = true; + } +} + static void extPositionHandler(CRTPPacket* pk) { + syncTimeIfNeeded(); + const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data; ext_pos.x = data->x; @@ -176,6 +199,8 @@ static void extPositionHandler(CRTPPacket* pk) { } static void extPoseHandler(const CRTPPacket* pk) { + syncTimeIfNeeded(); + const struct CrtpExtPose* data = (const struct CrtpExtPose*)&pk->data[1]; ext_pose.x = data->x; @@ -193,6 +218,8 @@ static void extPoseHandler(const CRTPPacket* pk) { } static void extPosePackedHandler(const CRTPPacket* pk) { + syncTimeIfNeeded(); + uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem); for (uint8_t i = 0; i < numItems; ++i) { const extPosePackedItem* item = (const extPosePackedItem*)&pk->data[1 + i * sizeof(extPosePackedItem)]; @@ -306,6 +333,8 @@ static void genericLocHandle(CRTPPacket* pk) static void extPositionPackedHandler(CRTPPacket* pk) { + syncTimeIfNeeded(); + uint8_t numItems = pk->size / sizeof(extPositionPackedItem); for (uint8_t i = 0; i < numItems; ++i) { const extPositionPackedItem* item = (const extPositionPackedItem*)&pk->data[i * sizeof(extPositionPackedItem)]; From 308ed10219eccb3c95c591cf3c9c49676e9a249b Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 16 May 2022 13:45:21 +0200 Subject: [PATCH 42/85] Update cvmrs task freq 1 -> 100 --- src/modules/src/cvmrs.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index b13b6ec9b5..de8be6053e 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -6,7 +6,7 @@ #include "usec_time.h" #include "debug.h" -#define TASK_FREQ 1 +#define TASK_FREQ 100 typedef struct { From d2c2b5ef5730e0df888a11c07e958fc636614db6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 1 Jun 2022 21:52:47 +0200 Subject: [PATCH 43/85] add parameter for yawrate in hlcommander --- src/modules/interface/math3d.h | 34 +++++++++++++++++++++ src/modules/src/crtp_commander_high_level.c | 13 +++++++- test_python/test_math3d.py | 24 +++++++++++++++ 3 files changed, 70 insertions(+), 1 deletion(-) diff --git a/src/modules/interface/math3d.h b/src/modules/interface/math3d.h index 673d5d6124..80d9ed6faf 100644 --- a/src/modules/interface/math3d.h +++ b/src/modules/interface/math3d.h @@ -45,6 +45,35 @@ SOFTWARE. static inline float fsqr(float x) { return x * x; } static inline float radians(float degrees) { return (M_PI_F / 180.0f) * degrees; } static inline float degrees(float radians) { return (180.0f / M_PI_F) * radians; } + +// Normalize radians to be in range [-pi,pi] +// See https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi +static inline float normalize_radians(float radians) +{ + // Copy the sign of the value in radians to the value of pi. + float signed_pi = copysignf(M_PI_F, radians); + // Set the value of difference to the appropriate signed value between pi and -pi. + radians = fmodf(radians + signed_pi, 2 * M_PI_F) - signed_pi; + return radians; +} + +// modulo operation that uses the floored definition (as in Python), rather than +// the truncated definition used for the % operator in C +// See https://en.wikipedia.org/wiki/Modulo_operation +static inline float fmodf_floored(float x, float n) +{ + return x - floorf(x / n) * n; +} + +// compute shortest signed angle between two given angles (in range [-pi, pi]) +// See https://stackoverflow.com/questions/1878907/how-can-i-find-the-difference-between-two-angles +static inline float shortest_signed_angle_radians(float start, float goal) +{ + float diff = goal - start; + float signed_diff = fmodf_floored(diff + M_PI_F, 2 * M_PI_F) - M_PI_F; + return signed_diff; +} + static inline float clamp(float value, float min, float max) { if (value < min) return min; if (value > max) return max; @@ -264,6 +293,11 @@ static inline struct vec vadd4(struct vec a, struct vec b, struct vec c, struct // TODO: make sure it compiles to optimal code return vadd(vadd(a, b), vadd(c, d)); } +// add 5 vectors. +static inline struct vec vadd5(struct vec a, struct vec b, struct vec c, struct vec d, struct vec e) { + // TODO: make sure it compiles to optimal code + return vadd(vadd(vadd(vadd(a, b), c), d), e); +} // subtract b and c from a. static inline struct vec vsub2(struct vec a, struct vec b, struct vec c) { return vadd3(a, vneg(b), vneg(c)); diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index eff53dbe72..24c7951de5 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -111,6 +111,8 @@ static StaticSemaphore_t lockTrajBuffer; static float defaultTakeoffVelocity = 0.5f; static float defaultLandingVelocity = 0.5f; +static float yawrate = 0.0f; + // Trajectory memory handling from the memory module static uint32_t handleMemGetSize(void) { return crtpCommanderHighLevelTrajectoryMemSize(); } static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer); @@ -351,7 +353,13 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat // store the last setpoint pos = ev.pos; vel = ev.vel; - yaw = ev.yaw; + if (yawrate != 0) { + yaw = normalize_radians(yaw + yawrate / ((float)(RATE_HL_COMMANDER))); + setpoint->attitude.yaw = degrees(yaw); + setpoint->attitudeRate.yaw = degrees(yawrate); + } else { + yaw = ev.yaw; + } return true; } @@ -837,4 +845,7 @@ PARAM_ADD_CORE(PARAM_FLOAT, vtoff, &defaultTakeoffVelocity) */ PARAM_ADD_CORE(PARAM_FLOAT, vland, &defaultLandingVelocity) + +PARAM_ADD(PARAM_FLOAT, yawrate, &yawrate) + PARAM_GROUP_STOP(hlCommander) diff --git a/test_python/test_math3d.py b/test_python/test_math3d.py index ef8ba655ad..6bab10f365 100644 --- a/test_python/test_math3d.py +++ b/test_python/test_math3d.py @@ -13,3 +13,27 @@ def test_that_vec_is_converted_to_numpy_array(): # Assert expected = np.array([1, 2, 3]) assert np.allclose(expected, actual) + + +def test_normalize_radians(): + # Fixture + angles = [-100, -5, 0, np.pi + 0.1, -np.pi - 0.1, 100] + + for angle in angles: + # Test + actual = cffirmware.normalize_radians(angle) + # Assert + expected = np.arctan2(np.sin(angle), np.cos(angle)) + assert np.allclose(expected, actual) + + +def test_shortest_signed_angle_radians(): + # Fixture + angle_pairs = [(-np.pi/2, np.pi), (np.pi/2, np.pi), (np.pi, -np.pi/3)] + + for start, goal in angle_pairs: + # Test + actual = cffirmware.shortest_signed_angle_radians(start, goal) + # Assert + expected = np.arctan2(np.sin(goal - start), np.cos(goal - start)) + assert np.allclose(expected, actual) \ No newline at end of file From 0c5b158ba437b9bd472a7522ef7c1b5908a77e2c Mon Sep 17 00:00:00 2001 From: akmaralAW Date: Tue, 7 Jun 2022 14:15:27 +0200 Subject: [PATCH 44/85] automated flight with auto-yaw --- src/modules/src/crtp_commander_high_level.c | 22 +++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 24c7951de5..dfe1e29158 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -112,6 +112,7 @@ static float defaultTakeoffVelocity = 0.5f; static float defaultLandingVelocity = 0.5f; static float yawrate = 0.0f; +static float yawrate_current = 0.0f; // Trajectory memory handling from the memory module static uint32_t handleMemGetSize(void) { return crtpCommanderHighLevelTrajectoryMemSize(); } @@ -353,13 +354,22 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat // store the last setpoint pos = ev.pos; vel = ev.vel; - if (yawrate != 0) { - yaw = normalize_radians(yaw + yawrate / ((float)(RATE_HL_COMMANDER))); + // if (yawrate != 0) { + float delta = yawrate - yawrate_current; + if (delta < -0.01f) { + delta = -0.01f; + } + if (delta > 0.01f) { + delta = 0.01f; + } + yawrate_current += delta; + + yaw = normalize_radians(yaw + yawrate_current / ((float)(RATE_HL_COMMANDER))); setpoint->attitude.yaw = degrees(yaw); - setpoint->attitudeRate.yaw = degrees(yawrate); - } else { - yaw = ev.yaw; - } + setpoint->attitudeRate.yaw = degrees(yawrate_current); + // } else { + // yaw = ev.yaw; + // } return true; } From fced02f9532481d73bf0403a3c3a69611ac83b13 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 8 Jun 2022 15:09:16 +0200 Subject: [PATCH 45/85] Switch to UART for STM->GAP8 This requires a similar change for the GAP8 --- src/modules/src/cvmrs.c | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index de8be6053e..f0d19619db 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -3,6 +3,7 @@ #include "system.h" #include "log.h" #include "aideck.h" +#include "uart1.h" #include "usec_time.h" #include "debug.h" @@ -39,7 +40,10 @@ void appMain() StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; state_packet->cmd = 0; - vTaskDelay(10000); + // Delay is only needed for CPX + // vTaskDelay(60000); + + DEBUG_PRINT("Starting CVMRS task\n"); lastWakeTime = xTaskGetTickCount(); while (1) @@ -53,7 +57,25 @@ void appMain() state_packet->z = logGetInt(z_id); state_packet->quat = logGetInt(quat_id); - cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */); + // cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */); + + uint8_t magic = 0xBC; + uint8_t length = sizeof(StatePacket_t); + uart1SendData(1, &magic); + uart1SendData(1, &length); + uart1SendData(length, (uint8_t*)state_packet); + + // compute crc + uint8_t crc = 0; + for (const uint8_t* p = (uint8_t*)state_packet; p < (uint8_t*)state_packet + length; p++) { + crc ^= *p; + } + uart1SendData(1, &crc); + + if (uart1DidOverrun()) { + DEBUG_PRINT("UART overrun!\n"); + } + // DEBUG_PRINT("cpxSendPacket\n"); } } \ No newline at end of file From 90deb543e1ae0f1c3b6c479ae2c9767040376100 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 8 Jun 2022 20:21:27 +0200 Subject: [PATCH 46/85] enable tumble check --- src/modules/src/supervisor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 609c643cec..5e7038056e 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -117,7 +117,7 @@ void supervisorUpdate(const sensorData_t *data) isFlying = isFlyingCheck(); isTumbled = isTumbledCheck(data); - isTumbled = false; + // isTumbled = false; if (isTumbled && isFlying) { stabilizerSetEmergencyStop(); } From e5074d991ac02b03c58a4d144319a394865cd4bb Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 9 Jun 2022 09:53:38 +0200 Subject: [PATCH 47/85] update power_distribution logic --- src/modules/interface/power_distribution.h | 2 +- src/modules/interface/stabilizer_types.h | 24 +++++++-- src/modules/src/power_distribution_stock.c | 63 +++++++++------------- src/modules/src/stabilizer.c | 18 +++++-- test_python/test_power_distribution.py | 2 +- 5 files changed, 61 insertions(+), 48 deletions(-) diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h index 5e39348004..4c19ddbcf1 100644 --- a/src/modules/interface/power_distribution.h +++ b/src/modules/interface/power_distribution.h @@ -31,6 +31,6 @@ void powerDistributionInit(void); bool powerDistributionTest(void); -void powerDistribution(motors_thrust_t* motorPower, const control_t *control); +void powerDistribution(motors_thrust_t* motorPower, const control_t *control, float maxThrust); #endif //__POWER_DISTRIBUTION_H__ diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 819e72deb2..54a4ab91b5 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -191,11 +191,27 @@ typedef struct control_s { control_mode_t controlMode; } control_t; +typedef enum motors_thrust_mode_e { + motorsThrustModePWM = 0, + motorsThrustModeForce = 1, +} motors_thrust_mode_t; + typedef struct motors_thrust_s { - uint16_t m1; // PWM ratio - uint16_t m2; // PWM ratio - uint16_t m3; // PWM ratio - uint16_t m4; // PWM ratio + union { + struct { + uint16_t m1; // PWM ratio + uint16_t m2; // PWM ratio + uint16_t m3; // PWM ratio + uint16_t m4; // PWM ratio + }; + struct { + float f1; // force in grams + float f2; // force in grams + float f3; // force in grams + float f4; // force in grams + }; + }; + motors_thrust_mode_t mode; } motors_thrust_t; typedef enum mode_e { diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c index 17e422e736..5275ba2700 100644 --- a/src/modules/src/power_distribution_stock.c +++ b/src/modules/src/power_distribution_stock.c @@ -33,10 +33,7 @@ #include "math3d.h" #include "autoconf.h" #include "config.h" // Important, since this defines QUAD_FORMATION_X -// 0 - disable -// 1 - individual motor power -// 2 - all motors use m1 variable setting -static bool motorSetEnable = false; + static uint8_t saturationStatus = 0; enum saturationBits @@ -81,6 +78,7 @@ bool powerDistributionTest(void) static void powerDistributionLegacy(motors_thrust_t* motorPower, const control_t *control) { + motorPower->mode = motorsThrustModePWM; #ifdef QUAD_FORMATION_X int16_t r = control->roll / 2.0f; int16_t p = control->pitch / 2.0f; @@ -113,7 +111,7 @@ static void powerDistributionLegacy(motors_thrust_t* motorPower, const control_t } } -static void powerDistributionForceTorque(motors_thrust_t* motorPower, const control_t *control) +static void powerDistributionForceTorque(motors_thrust_t* motorPower, const control_t *control, float maxThrust) { // On CF2, thrust is mapped 65536 <==> 60 grams thrust = control->thrustSI; @@ -212,7 +210,6 @@ static void powerDistributionForceTorque(motors_thrust_t* motorPower, const cont // for CF2, motorratio directly maps to thrust (not rpm etc.) // Thus, we only need to scale the values here - const float maxThrustInGram = motorsGetMaxThrust(); // g #if 0 const float maxThrust = maxThrustInGram * 9.81f / 1000.0f; // N @@ -247,44 +244,36 @@ static void powerDistributionForceTorque(motors_thrust_t* motorPower, const cont // collective-thrust saturation: skip for now #endif - for (int i = 0; i < 4; ++i) { - float forceInGrams = clamp(motorForce[i] / 9.81f * 1000.0f, 0, maxThrustInGram); - motorsSetThrust(i, forceInGrams); - } + + motorPower->mode = motorsThrustModeForce; + motorPower->f1 = clamp(motorForce[0] / 9.81f * 1000.0f, 0, maxThrust); + motorPower->f2 = clamp(motorForce[1] / 9.81f * 1000.0f, 0, maxThrust); + motorPower->f3 = clamp(motorForce[2] / 9.81f * 1000.0f, 0, maxThrust); + motorPower->f4 = clamp(motorForce[3] / 9.81f * 1000.0f, 0, maxThrust); } -static void powerDistributionForce(motors_thrust_t* motorPower, const control_t *control) +static void powerDistributionForce(motors_thrust_t* motorPower, const control_t *control, float maxThrust) { - const float maxThrustInGram = motorsGetMaxThrust(); // g - for (int i = 0; i < 4; ++i) { - float forceInGrams = control->normalizedForces[i] * maxThrustInGram; - motorsSetThrust(i, forceInGrams); - } + motorPower->mode = motorsThrustModeForce; + motorPower->f1 = control->normalizedForces[0] * maxThrust; + motorPower->f2 = control->normalizedForces[1] * maxThrust; + motorPower->f3 = control->normalizedForces[2] * maxThrust; + motorPower->f4 = control->normalizedForces[3] * maxThrust; } -void powerDistribution(motors_thrust_t* motorPower, const control_t *control) +void powerDistribution(motors_thrust_t* motorPower, const control_t *control, float maxThrust) { - if (motorSetEnable) + switch (control->controlMode) { - motorsSetRatio(MOTOR_M1, motorPowerSet.m1); - motorsSetRatio(MOTOR_M2, motorPowerSet.m2); - motorsSetRatio(MOTOR_M3, motorPowerSet.m3); - motorsSetRatio(MOTOR_M4, motorPowerSet.m4); - } else { - - switch (control->controlMode) - { - case controlModeLegacy: - powerDistributionLegacy(motorPower, control); - break; - case controlModeForceTorque: - powerDistributionForceTorque(motorPower, control); - break; - case controlModeForce: - powerDistributionForce(motorPower, control); - break; - } - + case controlModeLegacy: + powerDistributionLegacy(motorPower, control); + break; + case controlModeForceTorque: + powerDistributionForceTorque(motorPower, control, maxThrust); + break; + case controlModeForce: + powerDistributionForce(motorPower, control, maxThrust); + break; } } diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 683e526c54..c1dcebd01f 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -286,11 +286,19 @@ static void stabilizerTask(void* param) if (emergencyStop || (systemIsArmed() == false)) { motorsStop(); } else { - powerDistribution(&motorPower, &control); - motorsSetRatio(MOTOR_M1, motorPower.m1); - motorsSetRatio(MOTOR_M2, motorPower.m2); - motorsSetRatio(MOTOR_M3, motorPower.m3); - motorsSetRatio(MOTOR_M4, motorPower.m4); + float maxThrust = motorsGetMaxThrust(); + powerDistribution(&motorPower, &control, maxThrust); + if (motorPower.mode == motorsThrustModePWM) { + motorsSetRatio(MOTOR_M1, motorPower.m1); + motorsSetRatio(MOTOR_M2, motorPower.m2); + motorsSetRatio(MOTOR_M3, motorPower.m3); + motorsSetRatio(MOTOR_M4, motorPower.m4); + } else if (motorPower.mode == motorsThrustModeForce) { + motorsSetThrust(MOTOR_M1, motorPower.f1); + motorsSetThrust(MOTOR_M2, motorPower.f2); + motorsSetThrust(MOTOR_M3, motorPower.f3); + motorsSetThrust(MOTOR_M4, motorPower.f4); + } } #ifdef CONFIG_DECK_USD diff --git a/test_python/test_power_distribution.py b/test_python/test_power_distribution.py index 802392c717..584b3c65b5 100644 --- a/test_python/test_power_distribution.py +++ b/test_python/test_power_distribution.py @@ -13,7 +13,7 @@ def test_controller_mellinger(): control.pitch = 0 control.yaw = 0 - cffirmware.powerDistribution(motorPower, control) + cffirmware.powerDistribution(motorPower, control, 0) # control.thrust will be at a (tuned) hover-state assert motorPower.m1 == control.thrust assert motorPower.m2 == control.thrust From 23f8a99a2bca174a9bafaf8df90fa140b9e275ad Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 9 Jun 2022 10:01:05 +0200 Subject: [PATCH 48/85] add Lee controller to the bindings --- bindings/cffirmware.i | 2 ++ bindings/setup.py | 1 + src/modules/src/controller_lee.c | 13 +++------ test_python/test_controller_lee.py | 43 ++++++++++++++++++++++++++++++ 4 files changed, 50 insertions(+), 9 deletions(-) create mode 100644 test_python/test_controller_lee.py diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i index 26fde9ce58..2b92347f2d 100755 --- a/bindings/cffirmware.i +++ b/bindings/cffirmware.i @@ -60,6 +60,7 @@ #include "controller_mellinger.h" #include "power_distribution.h" #include "controller_sjc.h" +#include "controller_lee.h" %} %include "math3d.h" @@ -72,6 +73,7 @@ %include "controller_mellinger.h" %include "power_distribution.h" %include "controller_sjc.h" +%include "controller_lee.h" %inline %{ struct poly4d* piecewise_get(struct piecewise_traj *pp, int i) diff --git a/bindings/setup.py b/bindings/setup.py index 96c1a4bfcf..3c67449840 100644 --- a/bindings/setup.py +++ b/bindings/setup.py @@ -28,6 +28,7 @@ "src/modules/src/controller_mellinger.c", "src/modules/src/power_distribution_stock.c", "src/modules/src/controller_sjc.c", + "src/modules/src/controller_lee.c", ] cffirmware = Extension( diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index ae9b496d9c..5a7a1c2446 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -46,11 +46,6 @@ CDC 2010 #include "math3d.h" #include "controller_lee.h" -#include "usec_time.h" - -#include "FreeRTOS.h" -#include "task.h" - #define GRAVITY_MAGNITUDE (9.81f) static float g_vehicleMass = 0.033; // TODO: should be CF global for other modules @@ -82,7 +77,7 @@ static struct vec omega; static struct vec omega_r; static struct vec u; -static uint32_t ticks; +// static uint32_t ticks; static inline struct vec vclampscl(struct vec value, float min, float max) { return mkvec( @@ -117,7 +112,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, return; } - uint64_t startTime = usecTimestamp(); + // uint64_t startTime = usecTimestamp(); float dt = (float)(1.0f/ATTITUDE_RATE); @@ -244,7 +239,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, control->torque[1] = u.y; control->torque[2] = u.z; - ticks = usecTimestamp() - startTime; + // ticks = usecTimestamp() - startTime; } PARAM_GROUP_START(ctrlLee) @@ -310,6 +305,6 @@ LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x) LOG_ADD(LOG_FLOAT, omegary, &omega_r.y) LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) -LOG_ADD(LOG_UINT32, ticks, &ticks) +// LOG_ADD(LOG_UINT32, ticks, &ticks) LOG_GROUP_STOP(ctrlLee) diff --git a/test_python/test_controller_lee.py b/test_python/test_controller_lee.py new file mode 100644 index 0000000000..0dcd9ae46b --- /dev/null +++ b/test_python/test_controller_lee.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +import cffirmware + +def test_controller_lee(): + + cffirmware.controllerLeeInit() + + control = cffirmware.control_t() + setpoint = cffirmware.setpoint_t() + setpoint.mode.z = cffirmware.modeAbs + setpoint.position.z = 0 + setpoint.mode.x = cffirmware.modeVelocity + setpoint.velocity.x = 0 + setpoint.mode.y = cffirmware.modeVelocity + setpoint.velocity.y = 0 + setpoint.mode.yaw = cffirmware.modeVelocity + setpoint.attitudeRate.yaw = 0 + + state = cffirmware.state_t() + state.attitude.roll = 0 + state.attitude.pitch = -0 # WARNING: This needs to be negated + state.attitude.yaw = 0 + state.position.x = 0 + state.position.y = 0 + state.position.z = 0 + state.velocity.x = 0 + state.velocity.y = 0 + state.velocity.z = 0 + + sensors = cffirmware.sensorData_t() + sensors.gyro.x = 0 + sensors.gyro.y = 0 + sensors.gyro.z = 0 + + tick = 100 + + cffirmware.controllerLee(control, setpoint,sensors,state,tick) + # control.thrust will be at a (tuned) hover-state + assert control.controlMode == cffirmware.controlModeForceTorque + assert control.torque[0] == 0 + assert control.torque[1] == 0 + assert control.torque[2] == 0 From bbd3682cd70018adc9db56fcce6c166b9e6f6b9a Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 13 Jun 2022 10:49:44 +0200 Subject: [PATCH 49/85] ability to change exposure settings via params --- src/modules/src/cvmrs.c | 81 +++++++++++++++++++++++++++++++++++------ 1 file changed, 69 insertions(+), 12 deletions(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index f0d19619db..f083e1db8a 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -6,9 +6,19 @@ #include "uart1.h" #include "usec_time.h" #include "debug.h" +#include "param.h" #define TASK_FREQ 100 +// GAP8 register settings + +static uint8_t aeg; +static uint8_t aGain; +static uint8_t dGain; +static uint16_t exposure; +static uint8_t trigger = 0; + + typedef struct { uint8_t cmd; @@ -19,8 +29,17 @@ typedef struct uint32_t quat; // compressed, see quatcompress.h } __attribute__((packed)) StatePacket_t; // static StatePacket_t cf_state; - static CPXPacket_t cpx_packet; +static StatePacket_t state_packet; + +typedef struct +{ + uint8_t cmd; + uint8_t aeg; + uint8_t aGain; + uint8_t dGain; + uint16_t exposure; +} __attribute__((packed)) RegisterPacket_t; void appMain() { @@ -36,9 +55,10 @@ void appMain() logVarId_t quat_id = logGetVarId("stateEstimateZ", "quat"); cpxInitRoute(CPX_T_STM32, CPX_T_GAP8, CPX_F_APP, &cpx_packet.route); - cpx_packet.dataLength = sizeof(StatePacket_t); - StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; - state_packet->cmd = 0; + // cpx_packet.dataLength = sizeof(StatePacket_t); + + // StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; + state_packet.cmd = 0; // Delay is only needed for CPX // vTaskDelay(60000); @@ -51,11 +71,11 @@ void appMain() vTaskDelayUntil(&lastWakeTime, F2T(TASK_FREQ)); // Sending current state information to GAP8 - state_packet->timestamp = usecTimestamp(); - state_packet->x = logGetInt(x_id); - state_packet->y = logGetInt(y_id); - state_packet->z = logGetInt(z_id); - state_packet->quat = logGetInt(quat_id); + state_packet.timestamp = usecTimestamp(); + state_packet.x = logGetInt(x_id); + state_packet.y = logGetInt(y_id); + state_packet.z = logGetInt(z_id); + state_packet.quat = logGetInt(quat_id); // cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */); @@ -63,11 +83,11 @@ void appMain() uint8_t length = sizeof(StatePacket_t); uart1SendData(1, &magic); uart1SendData(1, &length); - uart1SendData(length, (uint8_t*)state_packet); + uart1SendData(length, (uint8_t*)&state_packet); // compute crc uint8_t crc = 0; - for (const uint8_t* p = (uint8_t*)state_packet; p < (uint8_t*)state_packet + length; p++) { + for (const uint8_t* p = (uint8_t*)&state_packet; p < (uint8_t*)&state_packet + length; p++) { crc ^= *p; } uart1SendData(1, &crc); @@ -77,5 +97,42 @@ void appMain() } // DEBUG_PRINT("cpxSendPacket\n"); + + if (trigger == 1) { + cpx_packet.dataLength = sizeof(RegisterPacket_t); + RegisterPacket_t* reg_packet = (RegisterPacket_t*)&cpx_packet.data; + reg_packet->cmd = 0; + reg_packet->aeg = aeg; + reg_packet->aGain = aGain; + reg_packet->dGain = dGain; + reg_packet->exposure = exposure; + + cpxSendPacketBlocking(&cpx_packet); + trigger = 0; + } } -} \ No newline at end of file +} + + +PARAM_GROUP_START(cvmrs) +/** + * @brief Automatic Exposure Gain (0: disabled; 1: enabled) + */ +PARAM_ADD(PARAM_UINT8, aeg, &aeg) +/** + * @brief Analog gain (1, 2, 4, or 8) [If AEG is disabled] + */ +PARAM_ADD(PARAM_UINT8, aGain, &aGain) +/** + * @brief Digital gain in 2.6 format (2-bit integer, 6-bit floating) [If AEG is disabled] + */ +PARAM_ADD(PARAM_UINT8, dGain, &dGain) +/** + * @brief Exposure 2 to (frame_length_lines - 2) [If AEG is disabled] + */ +PARAM_ADD(PARAM_UINT16, exposure, &exposure) +/** + * @brief Triggers an update of all settings/registers on the GAP8 + */ +PARAM_ADD(PARAM_UINT8, trigger, &trigger) +PARAM_GROUP_STOP(cvmrs) \ No newline at end of file From b5e8d3714a6c1887ae55438ede0d6659115a8ab4 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 13 Jun 2022 12:27:09 +0200 Subject: [PATCH 50/85] improved controller gains --- src/modules/src/controller_lee.c | 10 +++++----- src/modules/src/controller_sjc.c | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index 5a7a1c2446..a21e6c261d 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -48,7 +48,7 @@ CDC 2010 #define GRAVITY_MAGNITUDE (9.81f) -static float g_vehicleMass = 0.033; // TODO: should be CF global for other modules +static float g_vehicleMass = 0.028; // TODO: should be CF global for other modules // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter @@ -57,17 +57,17 @@ static float g_vehicleMass = 0.033; // TODO: should be CF global for other modul static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 // Position PID -static struct vec Kpos_P = {6, 6, 6}; // Kp in paper +static struct vec Kpos_P = {10, 10, 10}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {4, 4, 4}; // Kv in paper +static struct vec Kpos_D = {5, 5, 5}; // Kv in paper static float Kpos_D_limit = 100; static struct vec Kpos_I = {0, 0, 0}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; // Attitude PID -static struct vec KR = {10, 10, 10}; -static struct vec Komega = {0.0005, 0.0005, 0.001}; +static struct vec KR = {0.05, 0.05, 0.05}; +static struct vec Komega = {0.004, 0.004, 0.004}; // Logging variables static struct vec rpy; diff --git a/src/modules/src/controller_sjc.c b/src/modules/src/controller_sjc.c index 31d1da589a..8b67f050e4 100644 --- a/src/modules/src/controller_sjc.c +++ b/src/modules/src/controller_sjc.c @@ -59,7 +59,7 @@ static uint32_t ticks; static uint8_t trigReset; -static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules +static float g_vehicleMass = 0.028; // TODO: should be CF global for other modules // Attitude P on omega static struct vec K = {0.0005, 0.0005, 0.001}; From 09dd8d931ec4027dec301e93313f01580a49e7ba Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 13 Jun 2022 18:26:57 +0200 Subject: [PATCH 51/85] the update of states should be from rpy and not quaternions --- src/modules/src/controller_lee.c | 84 +++++++++++++++++++++----------- src/modules/src/controller_sjc.c | 2 +- 2 files changed, 57 insertions(+), 29 deletions(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index a21e6c261d..2c7642ee09 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -36,7 +36,6 @@ CDC 2010 TODO: * Switch position controller * consider Omega_d dot (currently assumes this is zero) - */ #include @@ -48,7 +47,7 @@ CDC 2010 #define GRAVITY_MAGNITUDE (9.81f) -static float g_vehicleMass = 0.028; // TODO: should be CF global for other modules +static float g_vehicleMass = 0.031; // TODO: should be CF global for other modules // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter @@ -73,6 +72,7 @@ static struct vec Komega = {0.004, 0.004, 0.004}; static struct vec rpy; static struct vec rpy_des; static struct vec qr; +static struct mat33 R_des; static struct vec omega; static struct vec omega_r; static struct vec u; @@ -115,7 +115,9 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // uint64_t startTime = usecTimestamp(); float dt = (float)(1.0f/ATTITUDE_RATE); - + // Desired Jerk and snap for now are zeros vector + struct vec desJerk = vzero(); + // struct vec dessnap = vzero(); // Address inconsistency in firmware where we need to compute our own desired yaw angle // Rate-controlled YAW is moving YAW angle setpoint float desiredYaw = 0; //rad @@ -129,9 +131,6 @@ void controllerLee(control_t *control, setpoint_t *setpoint, desiredYaw = rpy_des.z; } - // qr: Desired/reference angles in rad - // struct vec qr; - // Position controller if ( setpoint->mode.x == modeAbs || setpoint->mode.y == modeAbs @@ -145,26 +144,44 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // errors struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); - i_error_pos = vclampscl(vadd(i_error_pos, vscl(dt, pos_e)), -Kpos_I_limit, Kpos_I_limit); - struct vec F_d = vscl(g_vehicleMass, vadd4( + struct vec F_d = vadd3( acc_d, veltmul(Kpos_D, vel_e), - veltmul(Kpos_P, pos_e), - veltmul(Kpos_I, i_error_pos))); + veltmul(Kpos_P, pos_e)); + + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + struct mat33 R = quat2rotmat(q); - control->thrustSI = vmag(F_d); + control->thrustSI = g_vehicleMass*vdot(F_d , mcolumn(R, 2)); // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { controllerLeeReset(); } // Use current yaw instead of desired yaw for roll/pitch - float yaw = radians(state->attitude.yaw); - qr = mkvec( - asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI), - atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z), - desiredYaw); + // float yaw = radians(state->attitude.yaw); + // Compute Desired Rotation matrix + float normFd = vmag(F_d); + + struct vec xdes = vbasis(0); + struct vec ydes = vbasis(1); + struct vec zdes = vbasis(2); + + if (normFd > 0) { + zdes = vnormalize(F_d); + } + struct vec xcdes = mkvec(cosf(desiredYaw), sinf(desiredYaw), 0); + struct vec zcrossx = vcross(zdes, xcdes); + float normZX = vmag(zcrossx); + + if (normZX > 0) { + ydes = vnormalize(zcrossx); + } + xdes = vcross(ydes, zdes); + + R_des = mcolumns(xdes, ydes, zdes); + } else { if (setpoint->mode.z == modeDisable) { if (setpoint->thrust < 1000) { @@ -190,15 +207,18 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // Attitude controller // current rotation [R] - struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + // struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + rpy = mkvec( + radians(state->attitude.roll), + radians(-state->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + radians(state->attitude.yaw)); + struct quat q = rpy2quat(rpy); struct mat33 R = quat2rotmat(q); rpy = quat2rpy(q); // desired rotation [Rdes] - struct quat q_des = rpy2quat(qr); - struct mat33 R_des = quat2rotmat(q_des); - + struct quat q_des = mat2quat(R_des); rpy_des = quat2rpy(q_des); // rotation error @@ -212,23 +232,31 @@ void controllerLee(control_t *control, setpoint_t *setpoint, radians(sensors->gyro.y), radians(sensors->gyro.z)); - struct vec omega_des = mkvec( - radians(setpoint->attitudeRate.roll), - radians(setpoint->attitudeRate.pitch), - radians(setpoint->attitudeRate.yaw)); + // Compute desired omega + struct vec xdes = mcolumn(R_des, 0); + struct vec ydes = mcolumn(R_des, 1); + struct vec zdes = mcolumn(R_des, 2); + struct vec hw = vzero(); + + if (control->thrustSI != 0) { + struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); + hw = vscl(g_vehicleMass/control->thrustSI, tmp); + } + + struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), 0); omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); struct vec omega_error = vsub(omega, omega_r); + + // compute moments // M = -kR eR - kw ew + w x Jw - J(w x wr) - u = vadd4( + u = vadd3( vneg(veltmul(KR, eR)), vneg(veltmul(Komega, omega_error)), - vcross(omega, veltmul(J, omega)), - vneg( veltmul(J, vcross(omega, omega_r))) - ); + vcross(omega, veltmul(J, omega))); // if (enableNN > 1) { // u = vsub(u, tau_a); diff --git a/src/modules/src/controller_sjc.c b/src/modules/src/controller_sjc.c index 8b67f050e4..4a7f141442 100644 --- a/src/modules/src/controller_sjc.c +++ b/src/modules/src/controller_sjc.c @@ -59,7 +59,7 @@ static uint32_t ticks; static uint8_t trigReset; -static float g_vehicleMass = 0.028; // TODO: should be CF global for other modules +static float g_vehicleMass = 0.032; // TODO: should be CF global for other modules // Attitude P on omega static struct vec K = {0.0005, 0.0005, 0.001}; From da0ce52924052e34f61727142d5c813d2f17fae7 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Tue, 14 Jun 2022 18:24:06 +0200 Subject: [PATCH 52/85] fixed bugs and found good gains --- src/modules/src/controller_lee.c | 35 ++++++++++++++++++-------------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index 2c7642ee09..82c92ce72e 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -56,17 +56,17 @@ static float g_vehicleMass = 0.031; // TODO: should be CF global for other modul static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 // Position PID -static struct vec Kpos_P = {10, 10, 10}; // Kp in paper +static struct vec Kpos_P = {5.5, 5.5, 5.5}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {5, 5, 5}; // Kv in paper +static struct vec Kpos_D = {3, 3, 3}; // Kv in paper static float Kpos_D_limit = 100; static struct vec Kpos_I = {0, 0, 0}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; // Attitude PID -static struct vec KR = {0.05, 0.05, 0.05}; -static struct vec Komega = {0.004, 0.004, 0.004}; +static struct vec KR = {0.005, 0.005, 0.006}; +static struct vec Komega = {0.0009, 0.0009, 0.0012}; // Logging variables static struct vec rpy; @@ -150,19 +150,24 @@ void controllerLee(control_t *control, setpoint_t *setpoint, veltmul(Kpos_D, vel_e), veltmul(Kpos_P, pos_e)); - struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); - struct mat33 R = quat2rotmat(q); - control->thrustSI = g_vehicleMass*vdot(F_d , mcolumn(R, 2)); + rpy = mkvec( + radians(state->attitude.roll), + radians(-state->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + radians(state->attitude.yaw)); + + struct quat q = rpy2quat(rpy); + struct mat33 R = quat2rotmat(q); + struct vec z = vbasis(2); + control->thrustSI = g_vehicleMass*vdot(F_d , mvmul(R, z)); + // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { controllerLeeReset(); } - // Use current yaw instead of desired yaw for roll/pitch - // float yaw = radians(state->attitude.yaw); // Compute Desired Rotation matrix - float normFd = vmag(F_d); + float normFd = control->thrustSI; struct vec xdes = vbasis(0); struct vec ydes = vbasis(1); @@ -207,16 +212,14 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // Attitude controller // current rotation [R] - // struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); - rpy = mkvec( + rpy = mkvec( radians(state->attitude.roll), radians(-state->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted radians(state->attitude.yaw)); + struct quat q = rpy2quat(rpy); struct mat33 R = quat2rotmat(q); - rpy = quat2rpy(q); - // desired rotation [Rdes] struct quat q_des = mat2quat(R_des); rpy_des = quat2rpy(q_des); @@ -244,7 +247,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, } struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), 0); - + omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); struct vec omega_error = vsub(omega, omega_r); @@ -300,6 +303,8 @@ PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x) PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) + +PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) PARAM_GROUP_STOP(ctrlLee) From 30e5a448ac412fdfcec42ae9b9e7f50afaaaded0 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Tue, 14 Jun 2022 20:14:44 +0200 Subject: [PATCH 53/85] attitude feedback from quaternions --- src/modules/src/controller_lee.c | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index 82c92ce72e..a2278c3632 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -150,13 +150,8 @@ void controllerLee(control_t *control, setpoint_t *setpoint, veltmul(Kpos_D, vel_e), veltmul(Kpos_P, pos_e)); - - rpy = mkvec( - radians(state->attitude.roll), - radians(-state->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted - radians(state->attitude.yaw)); - struct quat q = rpy2quat(rpy); + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); struct mat33 R = quat2rotmat(q); struct vec z = vbasis(2); control->thrustSI = g_vehicleMass*vdot(F_d , mvmul(R, z)); @@ -212,12 +207,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // Attitude controller // current rotation [R] - rpy = mkvec( - radians(state->attitude.roll), - radians(-state->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted - radians(state->attitude.yaw)); - - struct quat q = rpy2quat(rpy); + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); struct mat33 R = quat2rotmat(q); // desired rotation [Rdes] From 860db28d27b8f50e39caf5e3996fc48ed3016d56 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 14 Jun 2022 20:20:54 +0200 Subject: [PATCH 54/85] add jerk to setpoint --- src/modules/interface/pptraj.h | 1 + src/modules/interface/stabilizer_types.h | 2 ++ src/modules/src/crtp_commander_generic.c | 1 + src/modules/src/crtp_commander_high_level.c | 3 +++ src/modules/src/pptraj.c | 6 ++++-- 5 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h index 568b299be4..22616ff93a 100644 --- a/src/modules/interface/pptraj.h +++ b/src/modules/interface/pptraj.h @@ -120,6 +120,7 @@ struct traj_eval struct vec pos; struct vec vel; struct vec acc; + struct vec jerk; struct vec omega; float yaw; }; diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 54a4ab91b5..3e64f84c22 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -62,6 +62,7 @@ typedef struct vec3_s vector_t; typedef struct vec3_s point_t; typedef struct vec3_s velocity_t; typedef struct vec3_s acc_t; +typedef struct vec3_s jerk_t; /* Orientation as a quaternion */ typedef struct quaternion_s { @@ -230,6 +231,7 @@ typedef struct setpoint_s { point_t position; // m velocity_t velocity; // m/s acc_t acceleration; // m/s^2 + jerk_t jerk; // m/s^3 bool velocity_body; // true if velocity is given in body frame; false if velocity is given in world frame struct { diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c index aaf24a1ebf..62b3540270 100644 --- a/src/modules/src/crtp_commander_generic.c +++ b/src/modules/src/crtp_commander_generic.c @@ -327,6 +327,7 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat setpoint->position.x = values->x / 1000.0f; \ setpoint->velocity.x = (values->v ## x) / 1000.0f; \ setpoint->acceleration.x = (values->a ## x) / 1000.0f; \ + setpoint->jerk.x = 0.0f; \ UNPACK(x) UNPACK(y) diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 9375e788a4..e160ae2bfa 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -349,6 +349,9 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat setpoint->acceleration.x = ev.acc.x; setpoint->acceleration.y = ev.acc.y; setpoint->acceleration.z = ev.acc.z; + setpoint->jerk.x = ev.jerk.x; + setpoint->jerk.y = ev.jerk.y; + setpoint->jerk.z = ev.jerk.z; // store the last setpoint pos = ev.pos; diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c index d24dc2f93c..ea7c4be493 100644 --- a/src/modules/src/pptraj.c +++ b/src/modules/src/pptraj.c @@ -309,7 +309,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t) // 3rd derivative polyder4d(deriv); - struct vec jerk = polyval_xyz(deriv, t); + out.jerk = polyval_xyz(deriv, t); struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV)); // float thrust_mag = mass * vmag(thrust); @@ -319,7 +319,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t) struct vec y_body = vnormalize(vcross(z_body, x_world)); struct vec x_body = vcross(y_body, z_body); - struct vec jerk_orth_zbody = vorthunit(jerk, z_body); + struct vec jerk_orth_zbody = vorthunit(out.jerk, z_body); struct vec h_w = vscl(1.0f / vmag(thrust), jerk_orth_zbody); out.omega.x = -vdot(h_w, y_body); @@ -356,6 +356,7 @@ struct traj_eval piecewise_eval( ev.pos = vadd(ev.pos, traj->shift); ev.vel = vzero(); ev.acc = vzero(); + ev.jerk = vzero(); ev.omega = vzero(); return ev; } @@ -386,6 +387,7 @@ struct traj_eval piecewise_eval_reversed( ev.pos = vadd(ev.pos, traj->shift); ev.vel = vzero(); ev.acc = vzero(); + ev.jerk = vzero(); ev.omega = vzero(); return ev; } From 762f3a8459d49301bf245a049450bd4a953d7045 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Fri, 17 Jun 2022 12:20:28 +0200 Subject: [PATCH 55/85] final tuning and changing of logging parameters for lee controller --- src/deck/drivers/src/usddeck.c | 4 +-- src/modules/src/controller_lee.c | 55 +++++++++++++++++++++++--------- src/modules/src/controller_sjc.c | 5 +-- tools/usdlog/plot_events.py | 5 ++- 4 files changed, 49 insertions(+), 20 deletions(-) diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c index 35506e3071..9f625efd5a 100644 --- a/src/deck/drivers/src/usddeck.c +++ b/src/deck/drivers/src/usddeck.c @@ -90,8 +90,8 @@ #define SPI_END_TRANSACTION spiEndTransaction #endif -#define MAX_USD_LOG_VARIABLES_PER_EVENT (20) -#define MAX_USD_LOG_EVENTS (20) +#define MAX_USD_LOG_VARIABLES_PER_EVENT (54) +#define MAX_USD_LOG_EVENTS (5) #define FIXED_FREQUENCY_EVENT_ID (0xFFFF) #define FIXED_FREQUENCY_EVENT_NAME "fixedFrequency" diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index a2278c3632..83131c5a5e 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -47,8 +47,8 @@ CDC 2010 #define GRAVITY_MAGNITUDE (9.81f) -static float g_vehicleMass = 0.031; // TODO: should be CF global for other modules - +static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules +static float thrustSI; // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter // BA theses, Julian Foerster, ETHZ @@ -56,17 +56,18 @@ static float g_vehicleMass = 0.031; // TODO: should be CF global for other modul static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 // Position PID -static struct vec Kpos_P = {5.5, 5.5, 5.5}; // Kp in paper +static struct vec Kpos_P = {20, 20, 20}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {3, 3, 3}; // Kv in paper +static struct vec Kpos_D = {18, 18,18}; // Kv in paper static float Kpos_D_limit = 100; static struct vec Kpos_I = {0, 0, 0}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; - +static struct vec p_error; +static struct vec v_error; // Attitude PID -static struct vec KR = {0.005, 0.005, 0.006}; -static struct vec Komega = {0.0009, 0.0009, 0.0012}; +static struct vec KR = {0.0055, 0.0055, 0.0055}; +static struct vec Komega = {0.0013, 0.0013, 0.0016}; // Logging variables static struct vec rpy; @@ -115,8 +116,6 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // uint64_t startTime = usecTimestamp(); float dt = (float)(1.0f/ATTITUDE_RATE); - // Desired Jerk and snap for now are zeros vector - struct vec desJerk = vzero(); // struct vec dessnap = vzero(); // Address inconsistency in firmware where we need to compute our own desired yaw angle // Rate-controlled YAW is moving YAW angle setpoint @@ -145,6 +144,9 @@ void controllerLee(control_t *control, setpoint_t *setpoint, struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); + p_error = pos_e; + v_error = vel_e; + struct vec F_d = vadd3( acc_d, veltmul(Kpos_D, vel_e), @@ -155,7 +157,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, struct mat33 R = quat2rotmat(q); struct vec z = vbasis(2); control->thrustSI = g_vehicleMass*vdot(F_d , mvmul(R, z)); - + thrustSI = control->thrustSI; // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { controllerLeeReset(); @@ -186,7 +188,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, if (setpoint->mode.z == modeDisable) { if (setpoint->thrust < 1000) { control->controlMode = controlModeForceTorque; - control->thrustSI = 0; + control->thrustSI = 0; control->torque[0] = 0; control->torque[1] = 0; control->torque[2] = 0; @@ -208,6 +210,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // current rotation [R] struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + rpy = quat2rpy(q); struct mat33 R = quat2rotmat(q); // desired rotation [Rdes] @@ -230,6 +233,8 @@ void controllerLee(control_t *control, setpoint_t *setpoint, struct vec ydes = mcolumn(R_des, 1); struct vec zdes = mcolumn(R_des, 2); struct vec hw = vzero(); + // Desired Jerk and snap for now are zeros vector + struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z); if (control->thrustSI != 0) { struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); @@ -264,7 +269,6 @@ void controllerLee(control_t *control, setpoint_t *setpoint, } PARAM_GROUP_START(ctrlLee) -// Attitude P PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x) PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y) PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z) @@ -299,6 +303,23 @@ PARAM_GROUP_STOP(ctrlLee) LOG_GROUP_START(ctrlLee) + +LOG_ADD(LOG_FLOAT, KR_x, &KR.x) +LOG_ADD(LOG_FLOAT, KR_y, &KR.y) +LOG_ADD(LOG_FLOAT, KR_z, &KR.z) +LOG_ADD(LOG_FLOAT, Kw_x, &Komega.x) +LOG_ADD(LOG_FLOAT, Kw_y, &Komega.y) +LOG_ADD(LOG_FLOAT, Kw_z, &Komega.z) + +LOG_ADD(LOG_FLOAT,Kpos_Px, &Kpos_P.x) +LOG_ADD(LOG_FLOAT,Kpos_Py, &Kpos_P.y) +LOG_ADD(LOG_FLOAT,Kpos_Pz, &Kpos_P.z) +LOG_ADD(LOG_FLOAT,Kpos_Dx, &Kpos_D.x) +LOG_ADD(LOG_FLOAT,Kpos_Dy, &Kpos_D.y) +LOG_ADD(LOG_FLOAT,Kpos_Dz, &Kpos_D.z) + + +LOG_ADD(LOG_FLOAT, thrustSI, &thrustSI) LOG_ADD(LOG_FLOAT, torquex, &u.x) LOG_ADD(LOG_FLOAT, torquey, &u.y) LOG_ADD(LOG_FLOAT, torquez, &u.z) @@ -314,9 +335,13 @@ LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) // errors -LOG_ADD(LOG_FLOAT, i_error_posx, &i_error_pos.x) -LOG_ADD(LOG_FLOAT, i_error_posy, &i_error_pos.y) -LOG_ADD(LOG_FLOAT, i_error_posz, &i_error_pos.z) +LOG_ADD(LOG_FLOAT, error_posx, &p_error.x) +LOG_ADD(LOG_FLOAT, error_posy, &p_error.y) +LOG_ADD(LOG_FLOAT, error_posz, &p_error.z) + +LOG_ADD(LOG_FLOAT, error_velx, &v_error.x) +LOG_ADD(LOG_FLOAT, error_vely, &v_error.y) +LOG_ADD(LOG_FLOAT, error_velz, &v_error.z) // omega LOG_ADD(LOG_FLOAT, omegax, &omega.x) diff --git a/src/modules/src/controller_sjc.c b/src/modules/src/controller_sjc.c index 4a7f141442..cb5c4c4e6e 100644 --- a/src/modules/src/controller_sjc.c +++ b/src/modules/src/controller_sjc.c @@ -113,7 +113,7 @@ static struct vec qr; static struct vec q; static struct vec omega; static struct vec omega_r; - +static float thrustSI; static struct vec qrp; static struct vec qr_dot; @@ -213,7 +213,7 @@ void controllerSJC(control_t *control, setpoint_t *setpoint, veltmul(Kpos_I, i_error_pos))); control->thrustSI = vmag(F_d); - + thrustSI = control->thrustSI; // This is for delay compensation (idea #1) // replace by T_d by T = T_d + T_d dot / lambda float T_d_dot = (control->thrustSI - T_d_last) * POSITION_RATE; @@ -404,6 +404,7 @@ PARAM_ADD(PARAM_UINT8, T_d_lambda, &T_d_lambda) PARAM_GROUP_STOP(ctrlSJC) LOG_GROUP_START(ctrlSJC) +LOG_ADD(LOG_FLOAT, thrustSI, &thrustSI) LOG_ADD(LOG_FLOAT, torquex, &u.x) LOG_ADD(LOG_FLOAT, torquey, &u.y) LOG_ADD(LOG_FLOAT, torquez, &u.z) diff --git a/tools/usdlog/plot_events.py b/tools/usdlog/plot_events.py index d707d5d8e3..64e0b5904e 100644 --- a/tools/usdlog/plot_events.py +++ b/tools/usdlog/plot_events.py @@ -21,6 +21,8 @@ def showAnnotation(data, sel): # decode binary log data data_usd = cfusdlog.decode(args.file_usd) + # print(data_usd) + # exit() # find start time start_time = None @@ -32,9 +34,10 @@ def showAnnotation(data, sel): # new figure fig, ax = plt.subplots(len(data_usd.keys()),1,sharex=True) + ax = [ax] for k, (event_name, data) in enumerate(data_usd.items()): - # print(k, event_name) + print(k, event_name) t = (data['timestamp'] - start_time) / 1000 ax[k].scatter(t, t*0) ax[k].set_title(event_name) From 28a0731fa48c95aa4fda80ccbf7257777f980b16 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 20 Jun 2022 15:28:35 +0200 Subject: [PATCH 56/85] revert some unintended changes --- Kconfig | 2 +- src/deck/drivers/src/Kbuild | 3 --- src/deck/drivers/src/zranger2.c | 3 +-- src/drivers/src/i2c_drv.c | 17 +++++++++-------- src/drivers/src/motors.c | 2 +- 5 files changed, 12 insertions(+), 15 deletions(-) diff --git a/Kconfig b/Kconfig index 0a57a692e6..a3d9b12cc7 100644 --- a/Kconfig +++ b/Kconfig @@ -173,7 +173,7 @@ menu "Expansion deck configuration" config DECK_FORCE string "Force load specified custom deck driver" - default "bcLoadcell:bcRpm:bcACS37800" + default "none" help A colon seperated list of custom drivers to force load or "none". diff --git a/src/deck/drivers/src/Kbuild b/src/deck/drivers/src/Kbuild index ab0eea7953..77ab03bd09 100644 --- a/src/deck/drivers/src/Kbuild +++ b/src/deck/drivers/src/Kbuild @@ -18,6 +18,3 @@ obj-$(CONFIG_DECK_OA) += oa.o obj-$(CONFIG_DECK_USD) += usddeck.o obj-$(CONFIG_DECK_ZRANGER) += zranger.o obj-$(CONFIG_DECK_ZRANGER2) += zranger2.o -obj-y += loadcell_nau7802.o -obj-y += rpm.o -obj-y += acs37800.o \ No newline at end of file diff --git a/src/deck/drivers/src/zranger2.c b/src/deck/drivers/src/zranger2.c index 230e450805..d7ea5f4884 100644 --- a/src/deck/drivers/src/zranger2.c +++ b/src/deck/drivers/src/zranger2.c @@ -55,7 +55,6 @@ static float expCoeff; static uint16_t range_last = 0; -static bool enableEKF = false; static bool isInit; NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t dev; @@ -138,7 +137,7 @@ void zRanger2Task(void* arg) // check if range is feasible and push into the estimator // the sensor should not be able to measure >5 [m], and outliers typically // occur as >8 [m] measurements - if (enableEKF && range_last < RANGE_OUTLIER_LIMIT) { + if (range_last < RANGE_OUTLIER_LIMIT) { float distance = (float)range_last * 0.001f; // Scale from [mm] to [m] float stdDev = expStdA * (1.0f + expf( expCoeff * (distance - expPointA))); rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount()); diff --git a/src/drivers/src/i2c_drv.c b/src/drivers/src/i2c_drv.c index f886a1f2e4..f9f4369a67 100644 --- a/src/drivers/src/i2c_drv.c +++ b/src/drivers/src/i2c_drv.c @@ -623,20 +623,21 @@ static void i2cdrvClearDMA(I2cDrv* i2c) static void i2cdrvDmaIsrHandler(I2cDrv* i2c) { + if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Transfer complete + { + i2cdrvClearDMA(i2c); + i2cNotifyClient(i2c); + // Are there any other messages to transact? + i2cTryNextMessage(i2c); + } if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag)) // Transfer error { DMA_ClearITPendingBit(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag); //TODO: Best thing we could do? i2c->txMessage.status = i2cNack; + i2cNotifyClient(i2c); + i2cTryNextMessage(i2c); } - if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Tranasfer complete - { - i2c->txMessage.status = i2cAck; - } - i2cdrvClearDMA(i2c); - i2cNotifyClient(i2c); - // Are there any other messages to transact? - i2cTryNextMessage(i2c); } diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 0e099f9e50..26ce86adc4 100644 --- a/src/drivers/src/motors.c +++ b/src/drivers/src/motors.c @@ -827,7 +827,7 @@ LOG_ADD(LOG_UINT16, m3_pwm, &motor_ratios[2]) /** * @brief Current motor 4 PWM output */ -LOG_ADD(LOG_UINT32, m4_pwm, &motor_ratios[3]) +LOG_ADD(LOG_UINT16, m4_pwm, &motor_ratios[3]) /** * @brief Cycle time of M1 output in microseconds */ From 519f023496e3934cadc5d9430d2cf73cc606e4ac Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 20 Jun 2022 17:00:46 +0200 Subject: [PATCH 57/85] add yawrate desired --- src/modules/src/controller_lee.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index 83131c5a5e..79b1fc3bfe 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -240,8 +240,9 @@ void controllerLee(control_t *control, setpoint_t *setpoint, struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); hw = vscl(g_vehicleMass/control->thrustSI, tmp); } - - struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), 0); + struct vec z_w = mkvec(0,0,1); + desiredYaw = setpoint->attitudeRate.yaw * vdot(mcolumn(R, 2),z_w); + struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYaw); omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); From 1c238bf1d291405517865854733ba39205823cac Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 20 Jun 2022 17:03:13 +0200 Subject: [PATCH 58/85] yawrate edit --- src/modules/src/controller_lee.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index 79b1fc3bfe..e595e3e6f1 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -241,7 +241,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, hw = vscl(g_vehicleMass/control->thrustSI, tmp); } struct vec z_w = mkvec(0,0,1); - desiredYaw = setpoint->attitudeRate.yaw * vdot(mcolumn(R, 2),z_w); + desiredYaw = setpoint->attitudeRate.yaw * vdot(zdes,z_w); struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYaw); omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); From dadd9e7fe7c745c8d63dc6a2b6b65bb4217377b2 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 20 Jun 2022 17:15:12 +0200 Subject: [PATCH 59/85] switch to radians --- src/modules/src/controller_lee.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index e595e3e6f1..4daaca5efa 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -240,9 +240,9 @@ void controllerLee(control_t *control, setpoint_t *setpoint, struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); hw = vscl(g_vehicleMass/control->thrustSI, tmp); } - struct vec z_w = mkvec(0,0,1); - desiredYaw = setpoint->attitudeRate.yaw * vdot(zdes,z_w); - struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYaw); + struct vec z_w = mkvec(0,0,1); + float desiredYawRate = radians(setpoint->attitudeRate.yaw) * vdot(zdes,z_w); + struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYawRate); omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); From fe6a99f013e9dd4e62caf3976b7075ca74bbd1fc Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Fri, 24 Jun 2022 16:51:51 +0200 Subject: [PATCH 60/85] add integral gains for attitude and position --- src/modules/src/controller_lee.c | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index 4daaca5efa..e36402d5ab 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -60,7 +60,7 @@ static struct vec Kpos_P = {20, 20, 20}; // Kp in paper static float Kpos_P_limit = 100; static struct vec Kpos_D = {18, 18,18}; // Kv in paper static float Kpos_D_limit = 100; -static struct vec Kpos_I = {0, 0, 0}; // not in paper +static struct vec Kpos_I = {8, 8, 8}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; static struct vec p_error; @@ -68,7 +68,8 @@ static struct vec v_error; // Attitude PID static struct vec KR = {0.0055, 0.0055, 0.0055}; static struct vec Komega = {0.0013, 0.0013, 0.0016}; - +static struct vec KI = {0.005, 0.005, 0.005}; +static struct vec i_error_att; // Logging variables static struct vec rpy; static struct vec rpy_des; @@ -90,6 +91,7 @@ static inline struct vec vclampscl(struct vec value, float min, float max) { void controllerLeeReset(void) { i_error_pos = vzero(); + i_error_att = vzero(); } void controllerLeeInit(void) @@ -143,14 +145,15 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // errors struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); - + i_error_pos = vadd(i_error_pos, vscl(dt, pos_e)); p_error = pos_e; v_error = vel_e; - struct vec F_d = vadd3( + struct vec F_d = vadd4( acc_d, veltmul(Kpos_D, vel_e), - veltmul(Kpos_P, pos_e)); + veltmul(Kpos_P, pos_e), + veltmul(Kpos_I, i_error_pos)); struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); @@ -248,13 +251,16 @@ void controllerLee(control_t *control, setpoint_t *setpoint, struct vec omega_error = vsub(omega, omega_r); + // Integral part on angle + i_error_att = vadd(i_error_att, vscl(dt, eR)); // compute moments // M = -kR eR - kw ew + w x Jw - J(w x wr) - u = vadd3( + u = vadd4( vneg(veltmul(KR, eR)), vneg(veltmul(Komega, omega_error)), + vneg(veltmul(KI, i_error_att)), vcross(omega, veltmul(J, omega))); // if (enableNN > 1) { @@ -273,6 +279,10 @@ PARAM_GROUP_START(ctrlLee) PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x) PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y) PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z) +// Attitude I +PARAM_ADD(PARAM_FLOAT, KI_x, &KI.x) +PARAM_ADD(PARAM_FLOAT, KI_y, &KI.y) +PARAM_ADD(PARAM_FLOAT, KI_z, &KI.z) // Attitude D PARAM_ADD(PARAM_FLOAT, Kw_x, &Komega.x) PARAM_ADD(PARAM_FLOAT, Kw_y, &Komega.y) From e39187a067769ffd38a85f38b89b5d5757967df1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 27 Jun 2022 10:28:50 +0200 Subject: [PATCH 61/85] add powerDist logging variables to detect motor saturation --- .../src/power_distribution_quadrotor.c | 53 ++++++++++++++++--- 1 file changed, 45 insertions(+), 8 deletions(-) diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c index 29a6605729..bab74a4208 100644 --- a/src/modules/src/power_distribution_quadrotor.c +++ b/src/modules/src/power_distribution_quadrotor.c @@ -53,10 +53,12 @@ static uint32_t idleThrust = DEFAULT_IDLE_THRUST; float motorForce[4]; -// static float g_thrustpart; -static float g_rollpart; -static float g_pitchpart; -static float g_yawpart; +// Logging +static int16_t log_thrustpart; // in 1/100th of a gram +static int16_t log_rollpart; // in 1/100th of a gram +static int16_t log_pitchpart; // in 1/100th of a gram +static int16_t log_yawpart; // in 1/100th of a gram +static int16_t log_maxThrust; // in 1/100th of a gram static float thrust; static struct vec torque; @@ -128,10 +130,11 @@ static void powerDistributionForceTorque(motors_thrust_t* motorPower, const cont motorForce[2] = thrustpart + rollpart + pitchpart + yawpart; motorForce[3] = thrustpart + rollpart - pitchpart - yawpart; - // g_thrustpart = thrustpart; - g_rollpart = rollpart; - g_pitchpart = pitchpart; - g_yawpart = yawpart; + log_thrustpart = thrustpart / 9.81f * 1000.0f * 100.0f; // convert to 1/100th of a gram + log_rollpart = rollpart / 9.81f * 1000.0f * 100.0f; + log_pitchpart = pitchpart / 9.81f * 1000.0f * 100.0f; + log_yawpart = yawpart / 9.81f * 1000.0f * 100.0f; + log_maxThrust = maxThrust * 100.0f; #else // Thrust mixing similar to PX4 (see https://px4.github.io/Firmware-Doxygen/d7/d2a/mixer__multirotor_8cpp_source.html) // 1. Mix thrust, roll, pitch without yaw @@ -266,6 +269,40 @@ void powerDistribution(motors_thrust_t* motorPower, const control_t *control, fl } } +/** + * Power distribution related log variables. + * Use the following to compute desired motor forces: + * + * M1 = thrustPart - rollPart - pitchPart + yawPart; + * M2 = thrustPart - rollPart + pitchPart - yawPart; + * M3 = thrustPart + rollPart + pitchPart + yawPart; + * M4 = thrustPart + rollPart - pitchPart - yawPart; + * + * Note that the actual commanded motor forces will be clipped between 0 and maxThrust + */ +LOG_GROUP_START(powerDist) +/** + * @brief Thrust part of power distribution; divide by 100 to get grams + */ +LOG_ADD_CORE(LOG_INT16, thrustPart, &log_thrustpart) +/** + * @brief Roll part of power distribution; divide by 100 to get grams + */ +LOG_ADD_CORE(LOG_INT16, rollPart, &log_rollpart) +/** + * @brief Pitch part of power distribution; divide by 100 to get grams + */ +LOG_ADD_CORE(LOG_INT16, pitchPart, &log_pitchpart) +/** + * @brief Yaw part of power distribution; divide by 100 to get grams + */ +LOG_ADD_CORE(LOG_INT16, yawPart, &log_yawpart) +/** + * @brief Maximum thrust for one motor; divide by 100 to get grams + */ +LOG_ADD_CORE(LOG_INT16, maxThrust, &log_maxThrust) +LOG_GROUP_STOP(powerDist) + /** * Power distribution parameters */ From 77be530bb190c3d50e0c62145cca278925aa2f2e Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 27 Jun 2022 10:56:07 +0200 Subject: [PATCH 62/85] edit yaw polynomial final condition --- src/modules/src/planner.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index 7b19a8338e..90b2dfe590 100644 --- a/src/modules/src/planner.c +++ b/src/modules/src/planner.c @@ -45,9 +45,14 @@ static void plan_takeoff_or_landing(struct planner *p, struct vec curr_pos, floa struct vec hover_pos = curr_pos; hover_pos.z = hover_height; + // compute the shortest possible rotation towards 0 + hover_yaw = normalize_radians(hover_yaw); + curr_yaw = normalize_radians(curr_yaw); + float goal_yaw = hover_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw); + piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration, curr_pos, curr_yaw, vzero(), 0, vzero(), - hover_pos, hover_yaw, vzero(), 0, vzero()); + hover_pos, goal_yaw, vzero(), 0, vzero()); } // ----------------- // @@ -179,12 +184,13 @@ int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool r } // compute the shortest possible rotation towards 0 + float curr_yaw = normalize_radians(curr_eval->yaw); hover_yaw = normalize_radians(hover_yaw); - float goal_yaw = hover_yaw + shortest_signed_angle_radians(hover_yaw, 0); + float goal_yaw = hover_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw); piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration, - curr_eval->pos, curr_eval->yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc, - hover_pos, hover_yaw, vzero(), goal_yaw, vzero()); + curr_eval->pos, curr_yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc, + hover_pos, goal_yaw, vzero(), 0, vzero()); p->reversed = false; p->state = TRAJECTORY_STATE_FLYING; From bb4b7a1ce2a57387bccf8d2ac53a6c93b8831322 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 27 Jun 2022 12:32:22 +0200 Subject: [PATCH 63/85] add framework for lee_payload controller --- bindings/cffirmware.i | 2 + bindings/setup.py | 1 + src/modules/interface/controller.h | 1 + .../interface/controller_lee_payload.h | 37 ++ src/modules/interface/stabilizer_types.h | 4 + src/modules/src/Kbuild | 1 + src/modules/src/controller.c | 2 + src/modules/src/controller_lee_payload.c | 348 ++++++++++++++++++ src/modules/src/stabilizer.c | 92 +++++ test_python/test_controller_lee_payload.py | 43 +++ 10 files changed, 531 insertions(+) create mode 100644 src/modules/interface/controller_lee_payload.h create mode 100644 src/modules/src/controller_lee_payload.c create mode 100644 test_python/test_controller_lee_payload.py diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i index 2b92347f2d..743307ace4 100755 --- a/bindings/cffirmware.i +++ b/bindings/cffirmware.i @@ -61,6 +61,7 @@ #include "power_distribution.h" #include "controller_sjc.h" #include "controller_lee.h" +#include "controller_lee_payload.h" %} %include "math3d.h" @@ -74,6 +75,7 @@ %include "power_distribution.h" %include "controller_sjc.h" %include "controller_lee.h" +%include "controller_lee_payload.h" %inline %{ struct poly4d* piecewise_get(struct piecewise_traj *pp, int i) diff --git a/bindings/setup.py b/bindings/setup.py index 2868955345..26ededbd84 100644 --- a/bindings/setup.py +++ b/bindings/setup.py @@ -29,6 +29,7 @@ "src/modules/src/power_distribution_quadrotor.c", "src/modules/src/controller_sjc.c", "src/modules/src/controller_lee.c", + "src/modules/src/controller_lee_payload.c", ] cffirmware = Extension( diff --git a/src/modules/interface/controller.h b/src/modules/interface/controller.h index 3a8b57ac8b..8fec622140 100644 --- a/src/modules/interface/controller.h +++ b/src/modules/interface/controller.h @@ -36,6 +36,7 @@ typedef enum { ControllerTypeSJC, ControllerTypeMellingerSI, ControllerTypeLee, + ControllerTypeLeePayload, ControllerType_COUNT, } ControllerType; diff --git a/src/modules/interface/controller_lee_payload.h b/src/modules/interface/controller_lee_payload.h new file mode 100644 index 0000000000..f862c91df5 --- /dev/null +++ b/src/modules/interface/controller_lee_payload.h @@ -0,0 +1,37 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2016 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#ifndef __CONTROLLER_LEE_PAYLOAD_H__ +#define __CONTROLLER_LEE_PAYLOAD_H__ + +#include "stabilizer_types.h" + +void controllerLeePayloadInit(void); +bool controllerLeePayloadTest(void); +void controllerLeePayload(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); + +#endif //__CONTROLLER_LEE_PAYLOAD_H__ diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 3e64f84c22..3c7a1fe07c 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -166,6 +166,10 @@ typedef struct state_s { point_t position; // m velocity_t velocity; // m/s acc_t acc; // Gs (but acc.z without considering gravity) + + // Measured state of the payload + point_t payload_pos; // m (world frame) + velocity_t payload_vel; // m/s (world frame) } state_t; typedef enum control_mode_e { diff --git a/src/modules/src/Kbuild b/src/modules/src/Kbuild index 0e33feed17..c56ae5b20a 100644 --- a/src/modules/src/Kbuild +++ b/src/modules/src/Kbuild @@ -9,6 +9,7 @@ obj-y += comm.o obj-y += console.o obj-y += controller_indi.o obj-y += controller_lee.o +obj-y += controller_lee_payload.o obj-y += controller_mellinger.o obj-y += controller_mellingerSI.o obj-y += controller.o diff --git a/src/modules/src/controller.c b/src/modules/src/controller.c index 44f664b24a..6c3886d600 100644 --- a/src/modules/src/controller.c +++ b/src/modules/src/controller.c @@ -9,6 +9,7 @@ #include "controller_sjc.h" #include "controller_mellingerSI.h" #include "controller_lee.h" +#include "controller_lee_payload.h" #include "autoconf.h" @@ -32,6 +33,7 @@ static ControllerFcns controllerFunctions[] = { {.init = controllerSJCInit, .test = controllerSJCTest, .update = controllerSJC, .name = "SJC"}, {.init = controllerMellingerSIInit, .test = controllerMellingerSITest, .update = controllerMellingerSI, .name = "MellingerSI"}, {.init = controllerLeeInit, .test = controllerLeeTest, .update = controllerLee, .name = "Lee"}, + {.init = controllerLeePayloadInit, .test = controllerLeePayloadTest, .update = controllerLeePayload, .name = "LeePayload"}, }; diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c new file mode 100644 index 0000000000..06d52cf6da --- /dev/null +++ b/src/modules/src/controller_lee_payload.c @@ -0,0 +1,348 @@ +/* +The MIT License (MIT) + +Copyright (c) 2022 Khaled Wahba + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +This controller is based on the following publication: + +TODO +*/ + +#include + +#include "param.h" +#include "log.h" +#include "math3d.h" +#include "controller_lee_payload.h" + +#define GRAVITY_MAGNITUDE (9.81f) + +static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules +static float thrustSI; +// Inertia matrix (diagonal matrix), see +// System Identification of the Crazyflie 2.0 Nano Quadrocopter +// BA theses, Julian Foerster, ETHZ +// https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 +static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 + +// Position PID +static struct vec Kpos_P = {20, 20, 20}; // Kp in paper +static float Kpos_P_limit = 100; +static struct vec Kpos_D = {18, 18,18}; // Kv in paper +static float Kpos_D_limit = 100; +static struct vec Kpos_I = {0, 0, 0}; // not in paper +static float Kpos_I_limit = 2; +static struct vec i_error_pos; +static struct vec p_error; +static struct vec v_error; +// Attitude PID +static struct vec KR = {0.0055, 0.0055, 0.0055}; +static struct vec Komega = {0.0013, 0.0013, 0.0016}; + +// Logging variables +static struct vec rpy; +static struct vec rpy_des; +static struct vec qr; +static struct mat33 R_des; +static struct vec omega; +static struct vec omega_r; +static struct vec u; + +// static uint32_t ticks; + +static inline struct vec vclampscl(struct vec value, float min, float max) { + return mkvec( + clamp(value.x, min, max), + clamp(value.y, min, max), + clamp(value.z, min, max)); +} + +void controllerLeePayloadReset(void) +{ + i_error_pos = vzero(); +} + +void controllerLeePayloadInit(void) +{ + controllerLeePayloadReset(); +} + +bool controllerLeePayloadTest(void) +{ + return true; +} + +void controllerLeePayload(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + + + if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { + return; + } + + // uint64_t startTime = usecTimestamp(); + + float dt = (float)(1.0f/ATTITUDE_RATE); + // struct vec dessnap = vzero(); + // Address inconsistency in firmware where we need to compute our own desired yaw angle + // Rate-controlled YAW is moving YAW angle setpoint + float desiredYaw = 0; //rad + if (setpoint->mode.yaw == modeVelocity) { + desiredYaw = radians(state->attitude.yaw + setpoint->attitudeRate.yaw * dt); + } else if (setpoint->mode.yaw == modeAbs) { + desiredYaw = radians(setpoint->attitude.yaw); + } else if (setpoint->mode.quat == modeAbs) { + struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); + rpy_des = quat2rpy(setpoint_quat); + desiredYaw = rpy_des.z; + } + + // Position controller + if ( setpoint->mode.x == modeAbs + || setpoint->mode.y == modeAbs + || setpoint->mode.z == modeAbs) { + struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); + struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); + struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); + struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + + // errors + struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); + struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); + + p_error = pos_e; + v_error = vel_e; + + struct vec F_d = vadd3( + acc_d, + veltmul(Kpos_D, vel_e), + veltmul(Kpos_P, pos_e)); + + + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + struct mat33 R = quat2rotmat(q); + struct vec z = vbasis(2); + control->thrustSI = g_vehicleMass*vdot(F_d , mvmul(R, z)); + thrustSI = control->thrustSI; + // Reset the accumulated error while on the ground + if (control->thrustSI < 0.01f) { + controllerLeePayloadReset(); + } + + // Compute Desired Rotation matrix + float normFd = control->thrustSI; + + struct vec xdes = vbasis(0); + struct vec ydes = vbasis(1); + struct vec zdes = vbasis(2); + + if (normFd > 0) { + zdes = vnormalize(F_d); + } + struct vec xcdes = mkvec(cosf(desiredYaw), sinf(desiredYaw), 0); + struct vec zcrossx = vcross(zdes, xcdes); + float normZX = vmag(zcrossx); + + if (normZX > 0) { + ydes = vnormalize(zcrossx); + } + xdes = vcross(ydes, zdes); + + R_des = mcolumns(xdes, ydes, zdes); + + } else { + if (setpoint->mode.z == modeDisable) { + if (setpoint->thrust < 1000) { + control->controlMode = controlModeForceTorque; + control->thrustSI = 0; + control->torque[0] = 0; + control->torque[1] = 0; + control->torque[2] = 0; + controllerLeePayloadReset(); + return; + } + } + // On CF2, thrust is mapped 65536 <==> 4 * 12 grams + const float max_thrust = 70.0f / 1000.0f * 9.81f; // N + control->thrustSI = setpoint->thrust / UINT16_MAX * max_thrust; + + qr = mkvec( + radians(setpoint->attitude.roll), + -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + desiredYaw); + } + + // Attitude controller + + // current rotation [R] + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + rpy = quat2rpy(q); + struct mat33 R = quat2rotmat(q); + + // desired rotation [Rdes] + struct quat q_des = mat2quat(R_des); + rpy_des = quat2rpy(q_des); + + // rotation error + struct mat33 eRM = msub(mmul(mtranspose(R_des), R), mmul(mtranspose(R), R_des)); + + struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0])); + + // angular velocity + omega = mkvec( + radians(sensors->gyro.x), + radians(sensors->gyro.y), + radians(sensors->gyro.z)); + + // Compute desired omega + struct vec xdes = mcolumn(R_des, 0); + struct vec ydes = mcolumn(R_des, 1); + struct vec zdes = mcolumn(R_des, 2); + struct vec hw = vzero(); + // Desired Jerk and snap for now are zeros vector + struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z); + + if (control->thrustSI != 0) { + struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); + hw = vscl(g_vehicleMass/control->thrustSI, tmp); + } + + struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), 0); + + omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); + + struct vec omega_error = vsub(omega, omega_r); + + + + // compute moments + // M = -kR eR - kw ew + w x Jw - J(w x wr) + u = vadd3( + vneg(veltmul(KR, eR)), + vneg(veltmul(Komega, omega_error)), + vcross(omega, veltmul(J, omega))); + + // if (enableNN > 1) { + // u = vsub(u, tau_a); + // } + + control->controlMode = controlModeForceTorque; + control->torque[0] = u.x; + control->torque[1] = u.y; + control->torque[2] = u.z; + + // ticks = usecTimestamp() - startTime; +} + +PARAM_GROUP_START(ctrlLeeP) +PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x) +PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y) +PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z) +// Attitude D +PARAM_ADD(PARAM_FLOAT, Kw_x, &Komega.x) +PARAM_ADD(PARAM_FLOAT, Kw_y, &Komega.y) +PARAM_ADD(PARAM_FLOAT, Kw_z, &Komega.z) + +// J +PARAM_ADD(PARAM_FLOAT, J_x, &J.x) +PARAM_ADD(PARAM_FLOAT, J_y, &J.y) +PARAM_ADD(PARAM_FLOAT, J_z, &J.z) + +// Position P +PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z) +PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit) +// Position D +PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z) +PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit) +// Position I +PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) +PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) + +PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) +PARAM_GROUP_STOP(ctrlLeeP) + + +LOG_GROUP_START(ctrlLeeP) + +LOG_ADD(LOG_FLOAT, KR_x, &KR.x) +LOG_ADD(LOG_FLOAT, KR_y, &KR.y) +LOG_ADD(LOG_FLOAT, KR_z, &KR.z) +LOG_ADD(LOG_FLOAT, Kw_x, &Komega.x) +LOG_ADD(LOG_FLOAT, Kw_y, &Komega.y) +LOG_ADD(LOG_FLOAT, Kw_z, &Komega.z) + +LOG_ADD(LOG_FLOAT,Kpos_Px, &Kpos_P.x) +LOG_ADD(LOG_FLOAT,Kpos_Py, &Kpos_P.y) +LOG_ADD(LOG_FLOAT,Kpos_Pz, &Kpos_P.z) +LOG_ADD(LOG_FLOAT,Kpos_Dx, &Kpos_D.x) +LOG_ADD(LOG_FLOAT,Kpos_Dy, &Kpos_D.y) +LOG_ADD(LOG_FLOAT,Kpos_Dz, &Kpos_D.z) + + +LOG_ADD(LOG_FLOAT, thrustSI, &thrustSI) +LOG_ADD(LOG_FLOAT, torquex, &u.x) +LOG_ADD(LOG_FLOAT, torquey, &u.y) +LOG_ADD(LOG_FLOAT, torquez, &u.z) + +// current angles +LOG_ADD(LOG_FLOAT, rpyx, &rpy.x) +LOG_ADD(LOG_FLOAT, rpyy, &rpy.y) +LOG_ADD(LOG_FLOAT, rpyz, &rpy.z) + +// desired angles +LOG_ADD(LOG_FLOAT, rpydx, &rpy_des.x) +LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) +LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) + +// errors +LOG_ADD(LOG_FLOAT, error_posx, &p_error.x) +LOG_ADD(LOG_FLOAT, error_posy, &p_error.y) +LOG_ADD(LOG_FLOAT, error_posz, &p_error.z) + +LOG_ADD(LOG_FLOAT, error_velx, &v_error.x) +LOG_ADD(LOG_FLOAT, error_vely, &v_error.y) +LOG_ADD(LOG_FLOAT, error_velz, &v_error.z) + +// omega +LOG_ADD(LOG_FLOAT, omegax, &omega.x) +LOG_ADD(LOG_FLOAT, omegay, &omega.y) +LOG_ADD(LOG_FLOAT, omegaz, &omega.z) + +// omega_r +LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x) +LOG_ADD(LOG_FLOAT, omegary, &omega_r.y) +LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) + +// LOG_ADD(LOG_UINT32, ticks, &ticks) + +LOG_GROUP_STOP(ctrlLeeP) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index be5f5b9488..0a24b1fd42 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -56,6 +56,8 @@ #include "statsCnt.h" #include "static_mem.h" #include "rateSupervisor.h" +#include "peer_localization.h" +#include "math3d.h" static bool isInit; static bool emergencyStop = false; @@ -98,6 +100,16 @@ static struct { int16_t rateRoll; int16_t ratePitch; int16_t rateYaw; + + // payload position - mm + int16_t px; + int16_t py; + int16_t pz; + + // payload velocity - mm / sec + int16_t pvx; + int16_t pvy; + int16_t pvz; } stateCompressed; static struct { @@ -115,6 +127,10 @@ static struct { int16_t az; } setpointCompressed; +// for payloads +point_t payload_last_pos; +float payload_alpha = 0.9; // between 0...1; 1: no filter + STATIC_MEM_TASK_ALLOC(stabilizerTask, STABILIZER_TASK_STACKSIZE); static void stabilizerTask(void* param); @@ -150,6 +166,15 @@ static void compressState() stateCompressed.rateRoll = sensorData.gyro.x * deg2millirad; stateCompressed.ratePitch = -sensorData.gyro.y * deg2millirad; stateCompressed.rateYaw = sensorData.gyro.z * deg2millirad; + + // payload + stateCompressed.px = state.payload_pos.x * 1000.0f; + stateCompressed.py = state.payload_pos.y * 1000.0f; + stateCompressed.pz = state.payload_pos.z * 1000.0f; + + stateCompressed.pvx = state.payload_vel.x * 1000.0f; + stateCompressed.pvy = state.payload_vel.y * 1000.0f; + stateCompressed.pvz = state.payload_vel.z * 1000.0f; } static void compressSetpoint() @@ -262,6 +287,40 @@ static void stabilizerTask(void* param) } stateEstimator(&state, tick); + + // add the payload state here + peerLocalizationOtherPosition_t* payloadPos = peerLocalizationGetPositionByID(255); + if (payloadPos != NULL) { + + // if we got a new state + if (payload_last_pos.timestamp < state.payload_pos.timestamp) { + // update the position + state.payload_pos.x = payloadPos->pos.x; + state.payload_pos.y = payloadPos->pos.y; + state.payload_pos.z = payloadPos->pos.z; + + // estimate the velocity numerically + const float dt = (state.payload_pos.timestamp - payload_last_pos.timestamp) / 1000.0f; //s + struct vec pos = mkvec(state.payload_pos.x, state.payload_pos.y, state.payload_pos.z); + struct vec last_pos = mkvec(payload_last_pos.x, payload_last_pos.y, payload_last_pos.z); + struct vec vel = vdiv(vsub(pos, last_pos), dt); + + // apply a simple complementary filter + struct vec vel_old = mkvec(state.payload_vel.x, state.payload_vel.y, state.payload_vel.z); + struct vec vel_filtered = vadd(vscl(1 - payload_alpha, vel_old), vscl(payload_alpha, vel)); + + state.payload_vel.x = vel_filtered.x; + state.payload_vel.y = vel_filtered.y; + state.payload_vel.z = vel_filtered.z; + + payload_last_pos = state.payload_pos; + } + } else { + state.payload_pos.x = NAN; + state.payload_pos.y = NAN; + state.payload_pos.z = NAN; + } + compressState(); if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, tick)) { @@ -801,4 +860,37 @@ LOG_ADD(LOG_INT16, ratePitch, &stateCompressed.ratePitch) * @brief Yaw rate (angular velocity) [milliradians / sec] */ LOG_ADD(LOG_INT16, rateYaw, &stateCompressed.rateYaw) + + +/** + * @brief The position of the payload in the global reference frame, X [mm] + */ +LOG_ADD(LOG_INT16, x, &stateCompressed.px) + +/** + * @brief The position of the payload in the global reference frame, Y [mm] + */ +LOG_ADD(LOG_INT16, y, &stateCompressed.py) + +/** + * @brief The position of the payload in the global reference frame, Z [mm] + */ +LOG_ADD(LOG_INT16, z, &stateCompressed.pz) + +/** + * @brief The velocity of the payload in the global reference frame, X [mm/s] + */ +LOG_ADD(LOG_INT16, vx, &stateCompressed.pvx) + +/** + * @brief The velocity of the payload in the global reference frame, Y [mm/s] + */ +LOG_ADD(LOG_INT16, vy, &stateCompressed.pvy) + +/** + * @brief The velocity of the payload in the global reference frame, Z [mm/s] + */ +LOG_ADD(LOG_INT16, vz, &stateCompressed.pvz) + + LOG_GROUP_STOP(stateEstimateZ) diff --git a/test_python/test_controller_lee_payload.py b/test_python/test_controller_lee_payload.py new file mode 100644 index 0000000000..8f8a91e1e2 --- /dev/null +++ b/test_python/test_controller_lee_payload.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +import cffirmware + +def test_controller_lee_payload(): + + cffirmware.controllerLeePayloadInit() + + control = cffirmware.control_t() + setpoint = cffirmware.setpoint_t() + setpoint.mode.z = cffirmware.modeAbs + setpoint.position.z = 0 + setpoint.mode.x = cffirmware.modeVelocity + setpoint.velocity.x = 0 + setpoint.mode.y = cffirmware.modeVelocity + setpoint.velocity.y = 0 + setpoint.mode.yaw = cffirmware.modeVelocity + setpoint.attitudeRate.yaw = 0 + + state = cffirmware.state_t() + state.attitude.roll = 0 + state.attitude.pitch = -0 # WARNING: This needs to be negated + state.attitude.yaw = 0 + state.position.x = 0 + state.position.y = 0 + state.position.z = 0 + state.velocity.x = 0 + state.velocity.y = 0 + state.velocity.z = 0 + + sensors = cffirmware.sensorData_t() + sensors.gyro.x = 0 + sensors.gyro.y = 0 + sensors.gyro.z = 0 + + tick = 100 + + cffirmware.controllerLeePayload(control, setpoint,sensors,state,tick) + # control.thrust will be at a (tuned) hover-state + assert control.controlMode == cffirmware.controlModeForceTorque + assert control.torque[0] == 0 + assert control.torque[1] == 0 + assert control.torque[2] == 0 From db5c6a33d9319cb28c680167bd263cbb8e953aa2 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Fri, 1 Jul 2022 13:53:47 +0200 Subject: [PATCH 64/85] add lee payload controller --- src/modules/interface/math3d.h | 18 ++++ src/modules/src/controller_lee_payload.c | 118 ++++++++++++++++------- 2 files changed, 103 insertions(+), 33 deletions(-) diff --git a/src/modules/interface/math3d.h b/src/modules/interface/math3d.h index 80d9ed6faf..3aa96872dc 100644 --- a/src/modules/interface/math3d.h +++ b/src/modules/interface/math3d.h @@ -382,6 +382,24 @@ static inline struct mat33 mcolumns(struct vec a, struct vec b, struct vec c) { return m; } +// construct a matrix from two vectors A = q*q.T +static inline struct mat33 vecmult(struct vec a) { + struct mat33 m; + m.m[0][0] = a.x*a.x; + m.m[1][0] = a.y*a.x; + m.m[2][0] = a.z*a.x; + + m.m[0][1] = a.x*a.y; + m.m[1][1] = a.y*a.y; + m.m[2][1] = a.z*a.y; + + m.m[0][2] = a.x*a.x; + m.m[1][2] = a.y*a.z; + m.m[2][2] = a.z*a.z; + + return m; +} + // construct a matrix from three row vectors. static inline struct mat33 mrows(struct vec a, struct vec b, struct vec c) { struct mat33 m; diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index 06d52cf6da..48282ea620 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -38,6 +38,8 @@ TODO #define GRAVITY_MAGNITUDE (9.81f) static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules +static float mp = 0.01; +static float l = 0.5; //length of the cable static float thrustSI; // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter @@ -45,16 +47,25 @@ static float thrustSI; // https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 +// initValues: +static struct vec qi_prev; +static struct vec payload_vel_prev; // Position PID -static struct vec Kpos_P = {20, 20, 20}; // Kp in paper +static struct vec Kpos_P = {4, 4, 4}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {18, 18,18}; // Kv in paper +static struct vec Kpos_D = {3, 3, 3}; // Kv in paper static float Kpos_D_limit = 100; static struct vec Kpos_I = {0, 0, 0}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; -static struct vec p_error; -static struct vec v_error; + +// Cable gains +static struct vec K_q = {25, 25, 25}; +static struct vec K_w = {3, 3, 3}; + + +static struct vec plp_error; +static struct vec plv_error; // Attitude PID static struct vec KR = {0.0055, 0.0055, 0.0055}; static struct vec Komega = {0.0013, 0.0013, 0.0016}; @@ -80,6 +91,8 @@ static inline struct vec vclampscl(struct vec value, float min, float max) { void controllerLeePayloadReset(void) { i_error_pos = vzero(); + qi_prev = mkvec(0,0,-1); + payload_vel_prev = mkvec(0,0,0); } void controllerLeePayloadInit(void) @@ -92,6 +105,8 @@ bool controllerLeePayloadTest(void) return true; } +// static inline struct vec u_parallel() + void controllerLeePayload(control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, @@ -124,44 +139,74 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, if ( setpoint->mode.x == modeAbs || setpoint->mode.y == modeAbs || setpoint->mode.z == modeAbs) { - struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); - struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); - struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); + + struct vec plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); + struct vec plVel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); + struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); - struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); - - // errors - struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); - struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); - - p_error = pos_e; - v_error = vel_e; - - struct vec F_d = vadd3( - acc_d, - veltmul(Kpos_D, vel_e), - veltmul(Kpos_P, pos_e)); + // struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + + struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); + struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); + // errors + struct vec plpos_e = vclampscl(vsub(plPos_d, plStPos), -Kpos_P_limit, Kpos_P_limit); + struct vec plvel_e = vclampscl(vsub(plVel_d, plStVel), -Kpos_D_limit, Kpos_D_limit); + + plp_error = plpos_e; + plv_error = plvel_e; + + struct vec F_d =vscl(mp ,vadd3( + plAcc_d, + veltmul(Kpos_P, plpos_e), + veltmul(Kpos_D, plvel_e))); + + struct vec desVirtInp = F_d; + //directional unit vector qi and angular velocity wi pointing from UAV to payload + struct vec qi = vnormalize(vsub(statePos, plStPos)); + struct vec qidot = vdiv(vsub(qi , qi_prev), dt); + struct vec wi = vcross(qi, qidot); + + struct mat33 qiqiT = vecmult(qi); + struct vec virtualInp = mvmul(qiqiT, desVirtInp); + struct vec grav = mkvec(0,0,-GRAVITY_MAGNITUDE); + struct vec acc0 = vsub(vdiv(vsub(plStVel, payload_vel_prev),dt), grav); + payload_vel_prev = plStVel; + + struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc0))); + + struct vec qdi = vnormalize(desVirtInp); + struct vec eq = vcross(qdi, qi); + struct mat33 skewqi = mcrossmat(qi); + struct mat33 skewqi2 = mmul(skewqi,skewqi); + struct vec wdi = vzero(); + struct vec ew = vadd(wi, mvmul(skewqi2, wdi)); + + struct vec u_perpind = vadd( + vscl(g_vehicleMass*l, mvmul(skewqi, vadd(vneg(veltmul(K_q, eq)), vneg(veltmul(K_w, ew))))) + , vneg(vscl(g_vehicleMass, mvmul(skewqi2, acc0)))); + struct vec u = vadd(u_parallel, u_perpind); + control->thrustSI = vmag(u); + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); struct mat33 R = quat2rotmat(q); - struct vec z = vbasis(2); - control->thrustSI = g_vehicleMass*vdot(F_d , mvmul(R, z)); + struct vec z = vbasis(2); thrustSI = control->thrustSI; - // Reset the accumulated error while on the ground + // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { controllerLeePayloadReset(); } // Compute Desired Rotation matrix - float normFd = control->thrustSI; - + float thrustSI = control->thrustSI; + struct vec Fd_ = vscl(control->thrustSI, mvmul(R, z)); struct vec xdes = vbasis(0); struct vec ydes = vbasis(1); struct vec zdes = vbasis(2); - if (normFd > 0) { - zdes = vnormalize(F_d); + if (thrustSI > 0) { + zdes = vnormalize(Fd_); } struct vec xcdes = mkvec(cosf(desiredYaw), sinf(desiredYaw), 0); struct vec zcrossx = vcross(zdes, xcdes); @@ -325,13 +370,13 @@ LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) // errors -LOG_ADD(LOG_FLOAT, error_posx, &p_error.x) -LOG_ADD(LOG_FLOAT, error_posy, &p_error.y) -LOG_ADD(LOG_FLOAT, error_posz, &p_error.z) +// LOG_ADD(LOG_FLOAT, error_posx, &p_error.x) +// LOG_ADD(LOG_FLOAT, error_posy, &p_error.y) +// LOG_ADD(LOG_FLOAT, error_posz, &p_error.z) -LOG_ADD(LOG_FLOAT, error_velx, &v_error.x) -LOG_ADD(LOG_FLOAT, error_vely, &v_error.y) -LOG_ADD(LOG_FLOAT, error_velz, &v_error.z) +// LOG_ADD(LOG_FLOAT, error_velx, &v_error.x) +// LOG_ADD(LOG_FLOAT, error_vely, &v_error.y) +// LOG_ADD(LOG_FLOAT, error_velz, &v_error.z) // omega LOG_ADD(LOG_FLOAT, omegax, &omega.x) @@ -346,3 +391,10 @@ LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) // LOG_ADD(LOG_UINT32, ticks, &ticks) LOG_GROUP_STOP(ctrlLeeP) + + + + + + + \ No newline at end of file From b594720cdafedf32ef2b9ceec24b953393ec9d87 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Fri, 1 Jul 2022 19:33:02 +0200 Subject: [PATCH 65/85] added the lee payload controller and tested in simulation --- src/modules/interface/stabilizer_types.h | 1 + src/modules/src/controller_lee_payload.c | 51 +++++++++++++----------- 2 files changed, 28 insertions(+), 24 deletions(-) diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 3c7a1fe07c..cb1714dd24 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -190,6 +190,7 @@ typedef struct control_s { struct { float thrustSI; // N float torque[3]; // Nm + float u_all[3]; }; float normalizedForces[4]; // 0 ... 1 }; diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index 48282ea620..8b536fce09 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -39,7 +39,7 @@ TODO static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules static float mp = 0.01; -static float l = 0.5; //length of the cable +static float l = 1.0; //length of the cable static float thrustSI; // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter @@ -51,24 +51,24 @@ static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 static struct vec qi_prev; static struct vec payload_vel_prev; // Position PID -static struct vec Kpos_P = {4, 4, 4}; // Kp in paper +static struct vec Kpos_P = {5, 5, 5}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {3, 3, 3}; // Kv in paper +static struct vec Kpos_D = {4.5, 4.5, 4.5}; // Kv in paper static float Kpos_D_limit = 100; static struct vec Kpos_I = {0, 0, 0}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; // Cable gains -static struct vec K_q = {25, 25, 25}; -static struct vec K_w = {3, 3, 3}; +static struct vec K_q = {20, 20, 18}; +static struct vec K_w = {7, 7, 7}; static struct vec plp_error; static struct vec plv_error; // Attitude PID -static struct vec KR = {0.0055, 0.0055, 0.0055}; -static struct vec Komega = {0.0013, 0.0013, 0.0016}; +static struct vec KR = {0.005, 0.005, 0.005}; +static struct vec Komega = {0.0009, 0.0009, 0.0009}; // Logging variables static struct vec rpy; @@ -121,7 +121,6 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // uint64_t startTime = usecTimestamp(); float dt = (float)(1.0f/ATTITUDE_RATE); - // struct vec dessnap = vzero(); // Address inconsistency in firmware where we need to compute our own desired yaw angle // Rate-controlled YAW is moving YAW angle setpoint float desiredYaw = 0; //rad @@ -143,9 +142,9 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); struct vec plVel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); - struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + // struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); - + struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); // errors @@ -162,36 +161,41 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec desVirtInp = F_d; //directional unit vector qi and angular velocity wi pointing from UAV to payload - struct vec qi = vnormalize(vsub(statePos, plStPos)); + struct vec qi = vnormalize(vsub(plStPos, statePos)); struct vec qidot = vdiv(vsub(qi , qi_prev), dt); + qi_prev = qi; + struct vec wi = vcross(qi, qidot); struct mat33 qiqiT = vecmult(qi); struct vec virtualInp = mvmul(qiqiT, desVirtInp); + // Compute parallel component struct vec grav = mkvec(0,0,-GRAVITY_MAGNITUDE); struct vec acc0 = vsub(vdiv(vsub(plStVel, payload_vel_prev),dt), grav); payload_vel_prev = plStVel; struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc0))); - - struct vec qdi = vnormalize(desVirtInp); + // Compute Perpindicular Component + struct vec qdi = vnormalize(vneg(desVirtInp)); struct vec eq = vcross(qdi, qi); struct mat33 skewqi = mcrossmat(qi); struct mat33 skewqi2 = mmul(skewqi,skewqi); struct vec wdi = vzero(); struct vec ew = vadd(wi, mvmul(skewqi2, wdi)); - struct vec u_perpind = vadd( - vscl(g_vehicleMass*l, mvmul(skewqi, vadd(vneg(veltmul(K_q, eq)), vneg(veltmul(K_w, ew))))) - , vneg(vscl(g_vehicleMass, mvmul(skewqi2, acc0)))); + struct vec u_perpind = vsub( + vscl(g_vehicleMass*l, mvmul(skewqi, vsub(vneg(veltmul(K_q, eq)), veltmul(K_w, ew)))), + vscl(g_vehicleMass, mvmul(skewqi2,acc0)) + ); + + struct vec u_i = vadd(u_parallel, u_perpind); - struct vec u = vadd(u_parallel, u_perpind); - control->thrustSI = vmag(u); + control->thrustSI = vmag(u_i); + control->u_all[0] = u_i.x; + control->u_all[1] = u_i.y; + control->u_all[2] = u_i.z; + - - struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); - struct mat33 R = quat2rotmat(q); - struct vec z = vbasis(2); thrustSI = control->thrustSI; // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { @@ -200,7 +204,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // Compute Desired Rotation matrix float thrustSI = control->thrustSI; - struct vec Fd_ = vscl(control->thrustSI, mvmul(R, z)); + struct vec Fd_ = u_i; //vscl(control->thrustSI, mvmul(R, z)); struct vec xdes = vbasis(0); struct vec ydes = vbasis(1); struct vec zdes = vbasis(2); @@ -299,7 +303,6 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, control->torque[0] = u.x; control->torque[1] = u.y; control->torque[2] = u.z; - // ticks = usecTimestamp() - startTime; } From e5584022c0c2b39d4682f45465089a049173abd7 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 4 Jul 2022 14:47:41 +0200 Subject: [PATCH 66/85] add the desired omega for cable and to the cable controller, with some gain tuning --- src/modules/src/controller_lee_payload.c | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index 8b536fce09..a35c00c153 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -49,6 +49,7 @@ static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 // initValues: static struct vec qi_prev; +static struct vec qdi_prev; static struct vec payload_vel_prev; // Position PID static struct vec Kpos_P = {5, 5, 5}; // Kp in paper @@ -60,8 +61,8 @@ static float Kpos_I_limit = 2; static struct vec i_error_pos; // Cable gains -static struct vec K_q = {20, 20, 18}; -static struct vec K_w = {7, 7, 7}; +static struct vec K_q = {28, 28, 20}; +static struct vec K_w = {10, 10, 7}; static struct vec plp_error; @@ -75,6 +76,8 @@ static struct vec rpy; static struct vec rpy_des; static struct vec qr; static struct mat33 R_des; +static struct quat q; +static struct mat33 R; static struct vec omega; static struct vec omega_r; static struct vec u; @@ -93,6 +96,7 @@ void controllerLeePayloadReset(void) i_error_pos = vzero(); qi_prev = mkvec(0,0,-1); payload_vel_prev = mkvec(0,0,0); + qdi_prev = vzero(); } void controllerLeePayloadInit(void) @@ -175,12 +179,16 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, payload_vel_prev = plStVel; struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc0))); + // Compute Perpindicular Component struct vec qdi = vnormalize(vneg(desVirtInp)); struct vec eq = vcross(qdi, qi); struct mat33 skewqi = mcrossmat(qi); struct mat33 skewqi2 = mmul(skewqi,skewqi); - struct vec wdi = vzero(); + + struct vec qdidot = vdiv(vsub(qdi, qdi_prev), dt); + qdi_prev = qdi; + struct vec wdi = vcross(qdi, qdidot); struct vec ew = vadd(wi, mvmul(skewqi2, wdi)); struct vec u_perpind = vsub( @@ -201,10 +209,14 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, if (control->thrustSI < 0.01f) { controllerLeePayloadReset(); } - + + q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + rpy = quat2rpy(q); + R = quat2rotmat(q); + struct vec z = vbasis(2); // Compute Desired Rotation matrix float thrustSI = control->thrustSI; - struct vec Fd_ = u_i; //vscl(control->thrustSI, mvmul(R, z)); + struct vec Fd_ = vscl(control->thrustSI, mvmul(R, z)); struct vec xdes = vbasis(0); struct vec ydes = vbasis(1); struct vec zdes = vbasis(2); @@ -248,7 +260,6 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // Attitude controller // current rotation [R] - struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); rpy = quat2rpy(q); struct mat33 R = quat2rotmat(q); From 2b0ae80331f211fe8b033a60f78c5df918f25c21 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 4 Jul 2022 17:28:09 +0200 Subject: [PATCH 67/85] fix goal yaw when landing and taking off --- src/modules/src/planner.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index 90b2dfe590..1f02a27256 100644 --- a/src/modules/src/planner.c +++ b/src/modules/src/planner.c @@ -48,7 +48,7 @@ static void plan_takeoff_or_landing(struct planner *p, struct vec curr_pos, floa // compute the shortest possible rotation towards 0 hover_yaw = normalize_radians(hover_yaw); curr_yaw = normalize_radians(curr_yaw); - float goal_yaw = hover_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw); + float goal_yaw = curr_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw); piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration, curr_pos, curr_yaw, vzero(), 0, vzero(), @@ -186,7 +186,7 @@ int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool r // compute the shortest possible rotation towards 0 float curr_yaw = normalize_radians(curr_eval->yaw); hover_yaw = normalize_radians(hover_yaw); - float goal_yaw = hover_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw); + float goal_yaw = curr_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw); piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration, curr_eval->pos, curr_yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc, From 63447b0810c08913a1f5b9a9aef763bb327c611d Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Tue, 5 Jul 2022 14:36:50 +0200 Subject: [PATCH 68/85] add parameters to change from crazyswarm --- src/modules/src/controller_lee_payload.c | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index a35c00c153..e6a84c5040 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -347,7 +347,20 @@ PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) +// Cable P +PARAM_ADD(PARAM_FLOAT, Kqx, &K_q.x) +PARAM_ADD(PARAM_FLOAT, Kqy, &K_q.y) +PARAM_ADD(PARAM_FLOAT, Kqz, &K_q.z) + +// Cable P +PARAM_ADD(PARAM_FLOAT, Kwx, &K_w.x) +PARAM_ADD(PARAM_FLOAT, Kwy, &K_w.y) +PARAM_ADD(PARAM_FLOAT, Kwz, &K_w.z) + PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) +PARAM_ADD(PARAM_FLOAT, massP, &mp) +PARAM_ADD(PARAM_FLOAT, length, &l) + PARAM_GROUP_STOP(ctrlLeeP) @@ -383,15 +396,6 @@ LOG_ADD(LOG_FLOAT, rpydx, &rpy_des.x) LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) -// errors -// LOG_ADD(LOG_FLOAT, error_posx, &p_error.x) -// LOG_ADD(LOG_FLOAT, error_posy, &p_error.y) -// LOG_ADD(LOG_FLOAT, error_posz, &p_error.z) - -// LOG_ADD(LOG_FLOAT, error_velx, &v_error.x) -// LOG_ADD(LOG_FLOAT, error_vely, &v_error.y) -// LOG_ADD(LOG_FLOAT, error_velz, &v_error.z) - // omega LOG_ADD(LOG_FLOAT, omegax, &omega.x) LOG_ADD(LOG_FLOAT, omegay, &omega.y) From 6022da073003f36fa2fd2c547a2c5114c89cb23b Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Fri, 8 Jul 2022 13:22:40 +0200 Subject: [PATCH 69/85] testing hovering case --- src/modules/src/controller_lee_payload.c | 67 ++++++++++++++++------- src/modules/src/stabilizer.c | 69 ++++++++++++++---------- 2 files changed, 88 insertions(+), 48 deletions(-) diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index e6a84c5040..3c36043dfd 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -39,8 +39,10 @@ TODO static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules static float mp = 0.01; -static float l = 1.0; //length of the cable +static float l = 0.4; //length of the cable static float thrustSI; +static float alpha_qidot = 0.6; +static float alpha_acc = 0.6; // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter // BA theses, Julian Foerster, ETHZ @@ -52,17 +54,17 @@ static struct vec qi_prev; static struct vec qdi_prev; static struct vec payload_vel_prev; // Position PID -static struct vec Kpos_P = {5, 5, 5}; // Kp in paper +static struct vec Kpos_P = {4, 4, 4}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {4.5, 4.5, 4.5}; // Kv in paper +static struct vec Kpos_D = {2, 2, 2}; // Kv in paper static float Kpos_D_limit = 100; static struct vec Kpos_I = {0, 0, 0}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; // Cable gains -static struct vec K_q = {28, 28, 20}; -static struct vec K_w = {10, 10, 7}; +static struct vec K_q = {10, 10, 10}; +static struct vec K_w = {8, 8, 8}; static struct vec plp_error; @@ -81,7 +83,9 @@ static struct mat33 R; static struct vec omega; static struct vec omega_r; static struct vec u; - +static struct vec u_i; +static struct vec qidot_prev; +static struct vec acc_prev; // static uint32_t ticks; static inline struct vec vclampscl(struct vec value, float min, float max) { @@ -95,7 +99,9 @@ void controllerLeePayloadReset(void) { i_error_pos = vzero(); qi_prev = mkvec(0,0,-1); - payload_vel_prev = mkvec(0,0,0); + qidot_prev = vzero(); + acc_prev = vzero(); + payload_vel_prev = vzero(); qdi_prev = vzero(); } @@ -147,11 +153,14 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec plVel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); - // struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); - // errors + + // struct vec plStPos = mkvec(state->position.x, state->position.y, state->position.z-l); + // struct vec plStVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + + // errors struct vec plpos_e = vclampscl(vsub(plPos_d, plStPos), -Kpos_P_limit, Kpos_P_limit); struct vec plvel_e = vclampscl(vsub(plVel_d, plStVel), -Kpos_D_limit, Kpos_D_limit); @@ -168,16 +177,28 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec qi = vnormalize(vsub(plStPos, statePos)); struct vec qidot = vdiv(vsub(qi , qi_prev), dt); qi_prev = qi; + struct vec qidot_filtered = vadd(vscl(1.0f - alpha_qidot, qidot_prev), vscl(alpha_qidot, qidot)); + qidot_prev = qidot_filtered; - struct vec wi = vcross(qi, qidot); + // Testing qidot_filtered to be vzero() + qidot_filtered = vzero(); + + struct vec wi = vcross(qi, qidot_filtered); struct mat33 qiqiT = vecmult(qi); struct vec virtualInp = mvmul(qiqiT, desVirtInp); + // Compute parallel component struct vec grav = mkvec(0,0,-GRAVITY_MAGNITUDE); - struct vec acc0 = vsub(vdiv(vsub(plStVel, payload_vel_prev),dt), grav); + struct vec acc_ = vdiv(vsub(plStVel, payload_vel_prev),dt); payload_vel_prev = plStVel; + struct vec acc_filtered = vadd(vscl(1.0f - alpha_acc, acc_prev), vscl(alpha_acc, acc_)); + acc_prev = acc_filtered; + + // Testing acceleration of payload to be zero + struct vec acc0 = vneg(grav); //vsub(acc_filtered, grav); + struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc0))); // Compute Perpindicular Component @@ -196,7 +217,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, vscl(g_vehicleMass, mvmul(skewqi2,acc0)) ); - struct vec u_i = vadd(u_parallel, u_perpind); + u_i = vadd(u_parallel, u_perpind); control->thrustSI = vmag(u_i); control->u_all[0] = u_i.x; @@ -352,7 +373,7 @@ PARAM_ADD(PARAM_FLOAT, Kqx, &K_q.x) PARAM_ADD(PARAM_FLOAT, Kqy, &K_q.y) PARAM_ADD(PARAM_FLOAT, Kqz, &K_q.z) -// Cable P +// Cable D PARAM_ADD(PARAM_FLOAT, Kwx, &K_w.x) PARAM_ADD(PARAM_FLOAT, Kwy, &K_w.y) PARAM_ADD(PARAM_FLOAT, Kwz, &K_w.z) @@ -366,13 +387,6 @@ PARAM_GROUP_STOP(ctrlLeeP) LOG_GROUP_START(ctrlLeeP) -LOG_ADD(LOG_FLOAT, KR_x, &KR.x) -LOG_ADD(LOG_FLOAT, KR_y, &KR.y) -LOG_ADD(LOG_FLOAT, KR_z, &KR.z) -LOG_ADD(LOG_FLOAT, Kw_x, &Komega.x) -LOG_ADD(LOG_FLOAT, Kw_y, &Komega.y) -LOG_ADD(LOG_FLOAT, Kw_z, &Komega.z) - LOG_ADD(LOG_FLOAT,Kpos_Px, &Kpos_P.x) LOG_ADD(LOG_FLOAT,Kpos_Py, &Kpos_P.y) LOG_ADD(LOG_FLOAT,Kpos_Pz, &Kpos_P.z) @@ -381,6 +395,14 @@ LOG_ADD(LOG_FLOAT,Kpos_Dy, &Kpos_D.y) LOG_ADD(LOG_FLOAT,Kpos_Dz, &Kpos_D.z) +LOG_ADD(LOG_FLOAT, Kqx, &K_q.x) +LOG_ADD(LOG_FLOAT, Kqy, &K_q.y) +LOG_ADD(LOG_FLOAT, Kqz, &K_q.z) + +LOG_ADD(LOG_FLOAT, Kwx, &K_w.x) +LOG_ADD(LOG_FLOAT, Kwy, &K_w.y) +LOG_ADD(LOG_FLOAT, Kwz, &K_w.z) + LOG_ADD(LOG_FLOAT, thrustSI, &thrustSI) LOG_ADD(LOG_FLOAT, torquex, &u.x) LOG_ADD(LOG_FLOAT, torquey, &u.y) @@ -406,6 +428,11 @@ LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x) LOG_ADD(LOG_FLOAT, omegary, &omega_r.y) LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) +LOG_ADD(LOG_FLOAT, ux, &u_i.x) +LOG_ADD(LOG_FLOAT, uy, &u_i.y) +LOG_ADD(LOG_FLOAT, uz, &u_i.z) + + // LOG_ADD(LOG_UINT32, ticks, &ticks) LOG_GROUP_STOP(ctrlLeeP) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 0a24b1fd42..b2f5f96b98 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -128,8 +128,9 @@ static struct { } setpointCompressed; // for payloads -point_t payload_last_pos; -float payload_alpha = 0.9; // between 0...1; 1: no filter +static float payload_alpha = 0.1; // between 0...1; 1: no filter +static point_t payload_pos_last; // m (world frame) +static velocity_t payload_vel_last; // m/s (world frame) STATIC_MEM_TASK_ALLOC(stabilizerTask, STABILIZER_TASK_STACKSIZE); @@ -262,7 +263,8 @@ static void stabilizerTask(void* param) rateSupervisorInit(&rateSupervisorContext, xTaskGetTickCount(), M2T(1000), 997, 1003, 1); - DEBUG_PRINT("Ready to fly.\n"); + payload_pos_last.timestamp = 0; + DEBUG_PRINT("Ready to fly.\n"); while(1) { // The sensor should unlock at 1kHz @@ -291,36 +293,43 @@ static void stabilizerTask(void* param) // add the payload state here peerLocalizationOtherPosition_t* payloadPos = peerLocalizationGetPositionByID(255); if (payloadPos != NULL) { - + // if we got a new state - if (payload_last_pos.timestamp < state.payload_pos.timestamp) { + if (payload_pos_last.timestamp < payloadPos->pos.timestamp) { + struct vec vel_filtered = vzero(); + // in the beginning, estimate the velocity to be zero, otherwise use + // numeric estimation with filter + if (payload_pos_last.timestamp != 0) { + // estimate the velocity numerically + const float dt = (payloadPos->pos.timestamp - payload_pos_last.timestamp) / 1000.0f; //s + struct vec pos = mkvec(payloadPos->pos.x, payloadPos->pos.y, payloadPos->pos.z); + struct vec last_pos = mkvec(payload_pos_last.x, payload_pos_last.y, payload_pos_last.z); + struct vec vel = vdiv(vsub(pos, last_pos), dt); + + // apply a simple complementary filter + struct vec vel_old = mkvec(payload_vel_last.x, payload_vel_last.y, payload_vel_last.z); + vel_filtered = vadd(vscl(1.0f - payload_alpha, vel_old), vscl(payload_alpha, vel)); + } // update the position state.payload_pos.x = payloadPos->pos.x; state.payload_pos.y = payloadPos->pos.y; state.payload_pos.z = payloadPos->pos.z; + state.payload_pos.timestamp = payloadPos->pos.timestamp; - // estimate the velocity numerically - const float dt = (state.payload_pos.timestamp - payload_last_pos.timestamp) / 1000.0f; //s - struct vec pos = mkvec(state.payload_pos.x, state.payload_pos.y, state.payload_pos.z); - struct vec last_pos = mkvec(payload_last_pos.x, payload_last_pos.y, payload_last_pos.z); - struct vec vel = vdiv(vsub(pos, last_pos), dt); - - // apply a simple complementary filter - struct vec vel_old = mkvec(state.payload_vel.x, state.payload_vel.y, state.payload_vel.z); - struct vec vel_filtered = vadd(vscl(1 - payload_alpha, vel_old), vscl(payload_alpha, vel)); - + // update the velocity state.payload_vel.x = vel_filtered.x; state.payload_vel.y = vel_filtered.y; state.payload_vel.z = vel_filtered.z; - - payload_last_pos = state.payload_pos; + state.payload_vel.timestamp = payloadPos->pos.timestamp; + + // update state + payload_pos_last = state.payload_pos; + payload_vel_last = state.payload_vel; + } else { + state.payload_pos = payload_vel_last; + state.payload_vel = payload_vel_last; } - } else { - state.payload_pos.x = NAN; - state.payload_pos.y = NAN; - state.payload_pos.z = NAN; } - compressState(); if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, tick)) { @@ -418,6 +427,10 @@ PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType) * @brief If set to nonzero will turn off power */ PARAM_ADD_CORE(PARAM_UINT8, stop, &emergencyStop) + + +PARAM_ADD_CORE(PARAM_FLOAT, pAlpha, &payload_alpha) + PARAM_GROUP_STOP(stabilizer) @@ -865,32 +878,32 @@ LOG_ADD(LOG_INT16, rateYaw, &stateCompressed.rateYaw) /** * @brief The position of the payload in the global reference frame, X [mm] */ -LOG_ADD(LOG_INT16, x, &stateCompressed.px) +LOG_ADD(LOG_INT16, px, &stateCompressed.px) /** * @brief The position of the payload in the global reference frame, Y [mm] */ -LOG_ADD(LOG_INT16, y, &stateCompressed.py) +LOG_ADD(LOG_INT16, py, &stateCompressed.py) /** * @brief The position of the payload in the global reference frame, Z [mm] */ -LOG_ADD(LOG_INT16, z, &stateCompressed.pz) +LOG_ADD(LOG_INT16, pz, &stateCompressed.pz) /** * @brief The velocity of the payload in the global reference frame, X [mm/s] */ -LOG_ADD(LOG_INT16, vx, &stateCompressed.pvx) +LOG_ADD(LOG_INT16, pvx, &stateCompressed.pvx) /** * @brief The velocity of the payload in the global reference frame, Y [mm/s] */ -LOG_ADD(LOG_INT16, vy, &stateCompressed.pvy) +LOG_ADD(LOG_INT16, pvy, &stateCompressed.pvy) /** * @brief The velocity of the payload in the global reference frame, Z [mm/s] */ -LOG_ADD(LOG_INT16, vz, &stateCompressed.pvz) +LOG_ADD(LOG_INT16, pvz, &stateCompressed.pvz) LOG_GROUP_STOP(stateEstimateZ) From 539a26240675f41cb98bbae3d92af5ffc4255c41 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Fri, 15 Jul 2022 17:25:50 +0200 Subject: [PATCH 70/85] add lee Payload controller --- .vscode/settings.json | 3 +- src/modules/src/controller_lee.c | 12 ++-- src/modules/src/controller_lee_payload.c | 77 +++++++++++++++--------- src/modules/src/stabilizer.c | 4 +- 4 files changed, 59 insertions(+), 37 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 605386e7e4..76cc761fa0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,5 +6,6 @@ }, "files.associations": { "bootloader.h": "c" - } + }, + "C_Cpp.errorSquiggles": "Disabled" } diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index e36402d5ab..bc749ec1cf 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -56,19 +56,19 @@ static float thrustSI; static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 // Position PID -static struct vec Kpos_P = {20, 20, 20}; // Kp in paper +static struct vec Kpos_P = {9, 9, 9}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {18, 18,18}; // Kv in paper +static struct vec Kpos_D = {7, 7, 7}; // Kv in paper static float Kpos_D_limit = 100; -static struct vec Kpos_I = {8, 8, 8}; // not in paper +static struct vec Kpos_I = {5, 5, 5}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; static struct vec p_error; static struct vec v_error; // Attitude PID -static struct vec KR = {0.0055, 0.0055, 0.0055}; -static struct vec Komega = {0.0013, 0.0013, 0.0016}; -static struct vec KI = {0.005, 0.005, 0.005}; +static struct vec KR = {0.0055, 0.0055, 0.01}; +static struct vec Komega = {0.0013, 0.0013, 0.002}; +static struct vec KI = {0.012, 0.018, 0.015}; static struct vec i_error_att; // Logging variables static struct vec rpy; diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index 3c36043dfd..b55b31f1ed 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -38,11 +38,11 @@ TODO #define GRAVITY_MAGNITUDE (9.81f) static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules -static float mp = 0.01; -static float l = 0.4; //length of the cable +static float mp = 0.001; +static float l = 0.001; //length of the cable static float thrustSI; -static float alpha_qidot = 0.6; -static float alpha_acc = 0.6; +static float alpha_qidot = 0.9; +static float alpha_acc = 0.9; // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter // BA theses, Julian Foerster, ETHZ @@ -54,17 +54,17 @@ static struct vec qi_prev; static struct vec qdi_prev; static struct vec payload_vel_prev; // Position PID -static struct vec Kpos_P = {4, 4, 4}; // Kp in paper +static struct vec Kpos_P = {60, 60, 90}; //{4, 4, 4}; // Kp in paper static float Kpos_P_limit = 100; -static struct vec Kpos_D = {2, 2, 2}; // Kv in paper +static struct vec Kpos_D = {0, 0, 0}; //{2, 2, 2}; // Kv in paper static float Kpos_D_limit = 100; static struct vec Kpos_I = {0, 0, 0}; // not in paper static float Kpos_I_limit = 2; static struct vec i_error_pos; // Cable gains -static struct vec K_q = {10, 10, 10}; -static struct vec K_w = {8, 8, 8}; +static struct vec K_q = {160, 160, 160}; //{10, 10, 10}; +static struct vec K_w = {0, 0, 0}; //{8, 8, 8}; static struct vec plp_error; @@ -72,6 +72,9 @@ static struct vec plv_error; // Attitude PID static struct vec KR = {0.005, 0.005, 0.005}; static struct vec Komega = {0.0009, 0.0009, 0.0009}; +static struct vec KI = {0.012, 0.018, 0.015}; +static struct vec i_error_att; + // Logging variables static struct vec rpy; @@ -98,6 +101,7 @@ static inline struct vec vclampscl(struct vec value, float min, float max) { void controllerLeePayloadReset(void) { i_error_pos = vzero(); + i_error_att = vzero(); qi_prev = mkvec(0,0,-1); qidot_prev = vzero(); acc_prev = vzero(); @@ -149,16 +153,16 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, || setpoint->mode.y == modeAbs || setpoint->mode.z == modeAbs) { - struct vec plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); + struct vec plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z-l); struct vec plVel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); - struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); + struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z); struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); - struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); - struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); + // struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); + // struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); - // struct vec plStPos = mkvec(state->position.x, state->position.y, state->position.z-l); - // struct vec plStVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + struct vec plStPos = mkvec(state->position.x, state->position.y, state->position.z-l); + struct vec plStVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); // errors struct vec plpos_e = vclampscl(vsub(plPos_d, plStPos), -Kpos_P_limit, Kpos_P_limit); @@ -174,15 +178,15 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec desVirtInp = F_d; //directional unit vector qi and angular velocity wi pointing from UAV to payload - struct vec qi = vnormalize(vsub(plStPos, statePos)); + // struct vec qi = vnormalize(vsub(plStPos, statePos)); + struct vec qi = vnormalize(vsub(plStPos, statePos)); //mkvec(0,0,-1); struct vec qidot = vdiv(vsub(qi , qi_prev), dt); qi_prev = qi; struct vec qidot_filtered = vadd(vscl(1.0f - alpha_qidot, qidot_prev), vscl(alpha_qidot, qidot)); + // Testing qidot_filtered to be vzero() + // qidot_filtered = vzero(); qidot_prev = qidot_filtered; - // Testing qidot_filtered to be vzero() - qidot_filtered = vzero(); - struct vec wi = vcross(qi, qidot_filtered); struct mat33 qiqiT = vecmult(qi); @@ -190,16 +194,15 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // Compute parallel component struct vec grav = mkvec(0,0,-GRAVITY_MAGNITUDE); - struct vec acc_ = vdiv(vsub(plStVel, payload_vel_prev),dt); + struct vec acc_ = vsub(vdiv(vsub(plStVel, payload_vel_prev),dt), grav); payload_vel_prev = plStVel; struct vec acc_filtered = vadd(vscl(1.0f - alpha_acc, acc_prev), vscl(alpha_acc, acc_)); acc_prev = acc_filtered; // Testing acceleration of payload to be zero - struct vec acc0 = vneg(grav); //vsub(acc_filtered, grav); + acc_filtered = vneg(grav); - - struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc0))); + struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc_filtered))); // Compute Perpindicular Component struct vec qdi = vnormalize(vneg(desVirtInp)); @@ -214,7 +217,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec u_perpind = vsub( vscl(g_vehicleMass*l, mvmul(skewqi, vsub(vneg(veltmul(K_q, eq)), veltmul(K_w, ew)))), - vscl(g_vehicleMass, mvmul(skewqi2,acc0)) + vscl(g_vehicleMass, mvmul(skewqi2,acc_filtered)) ); u_i = vadd(u_parallel, u_perpind); @@ -226,7 +229,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, thrustSI = control->thrustSI; - // Reset the accumulated error while on the ground + // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { controllerLeePayloadReset(); } @@ -234,10 +237,10 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); rpy = quat2rpy(q); R = quat2rotmat(q); - struct vec z = vbasis(2); + // struct vec z = vbasis(2); // Compute Desired Rotation matrix float thrustSI = control->thrustSI; - struct vec Fd_ = vscl(control->thrustSI, mvmul(R, z)); + struct vec Fd_ = u_i; struct vec xdes = vbasis(0); struct vec ydes = vbasis(1); struct vec zdes = vbasis(2); @@ -281,6 +284,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // Attitude controller // current rotation [R] + q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); rpy = quat2rpy(q); struct mat33 R = quat2rotmat(q); @@ -318,13 +322,15 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec omega_error = vsub(omega, omega_r); - + // Integral part on angle + i_error_att = vadd(i_error_att, vscl(dt, eR)); // compute moments // M = -kR eR - kw ew + w x Jw - J(w x wr) - u = vadd3( + u = vadd4( vneg(veltmul(KR, eR)), vneg(veltmul(Komega, omega_error)), + vneg(veltmul(KI, i_error_att)), vcross(omega, veltmul(J, omega))); // if (enableNN > 1) { @@ -368,6 +374,21 @@ PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) +// Attitude P +PARAM_ADD(PARAM_FLOAT, KRx, &KR.x) +PARAM_ADD(PARAM_FLOAT, KRy, &KR.y) +PARAM_ADD(PARAM_FLOAT, KRz, &KR.z) + +// Attitude D +PARAM_ADD(PARAM_FLOAT, Komx, &Komega.x) +PARAM_ADD(PARAM_FLOAT, Komy, &Komega.y) +PARAM_ADD(PARAM_FLOAT, Komz, &Komega.z) + +// Attitude I +PARAM_ADD(PARAM_FLOAT, KI_x, &KI.x) +PARAM_ADD(PARAM_FLOAT, KI_y, &KI.y) +PARAM_ADD(PARAM_FLOAT, KI_z, &KI.z) + // Cable P PARAM_ADD(PARAM_FLOAT, Kqx, &K_q.x) PARAM_ADD(PARAM_FLOAT, Kqy, &K_q.y) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index b2f5f96b98..fdaa31f7b6 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -128,7 +128,7 @@ static struct { } setpointCompressed; // for payloads -static float payload_alpha = 0.1; // between 0...1; 1: no filter +static float payload_alpha = 0.9; // between 0...1; 1: no filter static point_t payload_pos_last; // m (world frame) static velocity_t payload_vel_last; // m/s (world frame) @@ -326,7 +326,7 @@ static void stabilizerTask(void* param) payload_pos_last = state.payload_pos; payload_vel_last = state.payload_vel; } else { - state.payload_pos = payload_vel_last; + state.payload_pos = payload_pos_last; state.payload_vel = payload_vel_last; } } From 646a9037b71db1dd65827c417cb5cfb70721a63c Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 18 Jul 2022 15:22:28 +0200 Subject: [PATCH 71/85] refactor controllerLee to use a state/param struct * This enables tuning of parameters and logging/plotting of internal states from a Python script --- src/modules/interface/controller_lee.h | 48 +++- src/modules/src/controller.c | 2 +- src/modules/src/controller_lee.c | 290 +++++++++++++------------ test_python/test_controller_lee.py | 8 +- 4 files changed, 205 insertions(+), 143 deletions(-) diff --git a/src/modules/interface/controller_lee.h b/src/modules/interface/controller_lee.h index 8d4b64588d..d61bd50409 100644 --- a/src/modules/interface/controller_lee.h +++ b/src/modules/interface/controller_lee.h @@ -27,11 +27,53 @@ #include "stabilizer_types.h" -void controllerLeeInit(void); -bool controllerLeeTest(void); -void controllerLee(control_t *control, setpoint_t *setpoint, +// This structure contains the mutable state and inmutable parameters +typedef struct controllerLee_s { + float mass; // TODO: should be CF global for other modules + float thrustSI; + struct vec J; // Inertia matrix (diagonal matrix); kg m^2 + + // Position PID + struct vec Kpos_P; // Kp in paper + float Kpos_P_limit; + struct vec Kpos_D; // Kv in paper + float Kpos_D_limit; + struct vec Kpos_I; // not in paper + float Kpos_I_limit; + struct vec i_error_pos; + struct vec p_error; + struct vec v_error; + // Attitude PID + struct vec KR; + struct vec Komega; + struct vec KI; + struct vec i_error_att; + // Logging variables + struct vec rpy; + struct vec rpy_des; + struct vec qr; + struct mat33 R_des; + struct vec omega; + struct vec omega_r; + struct vec u; +} controllerLee_t; + + + +void controllerLeeInit(controllerLee_t* self); +void controllerLeeReset(controllerLee_t* self); +void controllerLee(controllerLee_t* self, control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); + +#ifdef CRAZYFLIE_FW +void controllerLeeFirmwareInit(void); +bool controllerLeeFirmwareTest(void); +void controllerLeeFirmware(control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick); +#endif // CRAZYFLIE_FW defined #endif //__CONTROLLER_LEE_H__ diff --git a/src/modules/src/controller.c b/src/modules/src/controller.c index 6c3886d600..dc60f7d449 100644 --- a/src/modules/src/controller.c +++ b/src/modules/src/controller.c @@ -32,7 +32,7 @@ static ControllerFcns controllerFunctions[] = { {.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"}, {.init = controllerSJCInit, .test = controllerSJCTest, .update = controllerSJC, .name = "SJC"}, {.init = controllerMellingerSIInit, .test = controllerMellingerSITest, .update = controllerMellingerSI, .name = "MellingerSI"}, - {.init = controllerLeeInit, .test = controllerLeeTest, .update = controllerLee, .name = "Lee"}, + {.init = controllerLeeFirmwareInit, .test = controllerLeeFirmwareTest, .update = controllerLeeFirmware, .name = "Lee"}, {.init = controllerLeePayloadInit, .test = controllerLeePayloadTest, .update = controllerLeePayload, .name = "LeePayload"}, }; diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c index bc749ec1cf..a92036bed7 100644 --- a/src/modules/src/controller_lee.c +++ b/src/modules/src/controller_lee.c @@ -39,47 +39,35 @@ CDC 2010 */ #include +#include -#include "param.h" -#include "log.h" #include "math3d.h" #include "controller_lee.h" #define GRAVITY_MAGNITUDE (9.81f) -static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules -static float thrustSI; -// Inertia matrix (diagonal matrix), see -// System Identification of the Crazyflie 2.0 Nano Quadrocopter -// BA theses, Julian Foerster, ETHZ -// https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 -static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 - -// Position PID -static struct vec Kpos_P = {9, 9, 9}; // Kp in paper -static float Kpos_P_limit = 100; -static struct vec Kpos_D = {7, 7, 7}; // Kv in paper -static float Kpos_D_limit = 100; -static struct vec Kpos_I = {5, 5, 5}; // not in paper -static float Kpos_I_limit = 2; -static struct vec i_error_pos; -static struct vec p_error; -static struct vec v_error; -// Attitude PID -static struct vec KR = {0.0055, 0.0055, 0.01}; -static struct vec Komega = {0.0013, 0.0013, 0.002}; -static struct vec KI = {0.012, 0.018, 0.015}; -static struct vec i_error_att; -// Logging variables -static struct vec rpy; -static struct vec rpy_des; -static struct vec qr; -static struct mat33 R_des; -static struct vec omega; -static struct vec omega_r; -static struct vec u; - -// static uint32_t ticks; +static controllerLee_t g_self = { + .mass = 0.034, // TODO: should be CF global for other modules + + // Inertia matrix (diagonal matrix), see + // System Identification of the Crazyflie 2.0 Nano Quadrocopter + // BA theses, Julian Foerster, ETHZ + // https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 + .J = {16.571710e-6, 16.655602e-6, 29.261652e-6}, // kg m^2 + + // Position PID + .Kpos_P = {9, 9, 9}, // Kp in paper + .Kpos_P_limit = 100, + .Kpos_D = {7, 7, 7}, // Kv in paper + .Kpos_D_limit = 100, + .Kpos_I = {5, 5, 5}, // not in paper + .Kpos_I_limit = 2, + + // Attitude PID + .KR = {0.0055, 0.0055, 0.01}, + .Komega = {0.0013, 0.0013, 0.002}, + .KI = {0.012, 0.018, 0.015}, +}; static inline struct vec vclampscl(struct vec value, float min, float max) { return mkvec( @@ -88,23 +76,26 @@ static inline struct vec vclampscl(struct vec value, float min, float max) { clamp(value.z, min, max)); } -void controllerLeeReset(void) +void controllerLeeReset(controllerLee_t* self) { - i_error_pos = vzero(); - i_error_att = vzero(); + self->i_error_pos = vzero(); + self->i_error_att = vzero(); } -void controllerLeeInit(void) +void controllerLeeInit(controllerLee_t* self) { - controllerLeeReset(); + // copy default values (bindings), or NOP (firmware) + *self = g_self; + + controllerLeeReset(self); } -bool controllerLeeTest(void) +bool controllerLeeTest(controllerLee_t* self) { return true; } -void controllerLee(control_t *control, setpoint_t *setpoint, +void controllerLee(controllerLee_t* self, control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick) @@ -128,8 +119,8 @@ void controllerLee(control_t *control, setpoint_t *setpoint, desiredYaw = radians(setpoint->attitude.yaw); } else if (setpoint->mode.quat == modeAbs) { struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); - rpy_des = quat2rpy(setpoint_quat); - desiredYaw = rpy_des.z; + self->rpy_des = quat2rpy(setpoint_quat); + desiredYaw = self->rpy_des.z; } // Position controller @@ -143,27 +134,27 @@ void controllerLee(control_t *control, setpoint_t *setpoint, struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); // errors - struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit); - struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit); - i_error_pos = vadd(i_error_pos, vscl(dt, pos_e)); - p_error = pos_e; - v_error = vel_e; + struct vec pos_e = vclampscl(vsub(pos_d, statePos), -self->Kpos_P_limit, self->Kpos_P_limit); + struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -self->Kpos_D_limit, self->Kpos_D_limit); + self->i_error_pos = vadd(self->i_error_pos, vscl(dt, pos_e)); + self->p_error = pos_e; + self->v_error = vel_e; struct vec F_d = vadd4( acc_d, - veltmul(Kpos_D, vel_e), - veltmul(Kpos_P, pos_e), - veltmul(Kpos_I, i_error_pos)); + veltmul(self->Kpos_D, vel_e), + veltmul(self->Kpos_P, pos_e), + veltmul(self->Kpos_I, self->i_error_pos)); struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); struct mat33 R = quat2rotmat(q); struct vec z = vbasis(2); - control->thrustSI = g_vehicleMass*vdot(F_d , mvmul(R, z)); - thrustSI = control->thrustSI; + control->thrustSI = self->mass*vdot(F_d , mvmul(R, z)); + self->thrustSI = control->thrustSI; // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { - controllerLeeReset(); + controllerLeeReset(self); } // Compute Desired Rotation matrix @@ -185,7 +176,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, } xdes = vcross(ydes, zdes); - R_des = mcolumns(xdes, ydes, zdes); + self->R_des = mcolumns(xdes, ydes, zdes); } else { if (setpoint->mode.z == modeDisable) { @@ -195,7 +186,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, control->torque[0] = 0; control->torque[1] = 0; control->torque[2] = 0; - controllerLeeReset(); + controllerLeeReset(self); return; } } @@ -203,7 +194,7 @@ void controllerLee(control_t *control, setpoint_t *setpoint, const float max_thrust = 70.0f / 1000.0f * 9.81f; // N control->thrustSI = setpoint->thrust / UINT16_MAX * max_thrust; - qr = mkvec( + self->qr = mkvec( radians(setpoint->attitude.roll), -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted desiredYaw); @@ -213,157 +204,182 @@ void controllerLee(control_t *control, setpoint_t *setpoint, // current rotation [R] struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); - rpy = quat2rpy(q); + self->rpy = quat2rpy(q); struct mat33 R = quat2rotmat(q); // desired rotation [Rdes] - struct quat q_des = mat2quat(R_des); - rpy_des = quat2rpy(q_des); + struct quat q_des = mat2quat(self->R_des); + self->rpy_des = quat2rpy(q_des); // rotation error - struct mat33 eRM = msub(mmul(mtranspose(R_des), R), mmul(mtranspose(R), R_des)); + struct mat33 eRM = msub(mmul(mtranspose(self->R_des), R), mmul(mtranspose(R), self->R_des)); struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0])); // angular velocity - omega = mkvec( + self->omega = mkvec( radians(sensors->gyro.x), radians(sensors->gyro.y), radians(sensors->gyro.z)); // Compute desired omega - struct vec xdes = mcolumn(R_des, 0); - struct vec ydes = mcolumn(R_des, 1); - struct vec zdes = mcolumn(R_des, 2); + struct vec xdes = mcolumn(self->R_des, 0); + struct vec ydes = mcolumn(self->R_des, 1); + struct vec zdes = mcolumn(self->R_des, 2); struct vec hw = vzero(); // Desired Jerk and snap for now are zeros vector struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z); if (control->thrustSI != 0) { struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); - hw = vscl(g_vehicleMass/control->thrustSI, tmp); + hw = vscl(self->mass/control->thrustSI, tmp); } struct vec z_w = mkvec(0,0,1); float desiredYawRate = radians(setpoint->attitudeRate.yaw) * vdot(zdes,z_w); struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYawRate); - omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); + self->omega_r = mvmul(mmul(mtranspose(R), self->R_des), omega_des); - struct vec omega_error = vsub(omega, omega_r); + struct vec omega_error = vsub(self->omega, self->omega_r); // Integral part on angle - i_error_att = vadd(i_error_att, vscl(dt, eR)); + self->i_error_att = vadd(self->i_error_att, vscl(dt, eR)); // compute moments // M = -kR eR - kw ew + w x Jw - J(w x wr) - u = vadd4( - vneg(veltmul(KR, eR)), - vneg(veltmul(Komega, omega_error)), - vneg(veltmul(KI, i_error_att)), - vcross(omega, veltmul(J, omega))); + self->u = vadd4( + vneg(veltmul(self->KR, eR)), + vneg(veltmul(self->Komega, omega_error)), + vneg(veltmul(self->KI, self->i_error_att)), + vcross(self->omega, veltmul(self->J, self->omega))); // if (enableNN > 1) { // u = vsub(u, tau_a); // } control->controlMode = controlModeForceTorque; - control->torque[0] = u.x; - control->torque[1] = u.y; - control->torque[2] = u.z; + control->torque[0] = self->u.x; + control->torque[1] = self->u.y; + control->torque[2] = self->u.z; // ticks = usecTimestamp() - startTime; } +#ifdef CRAZYFLIE_FW + +#include "param.h" +#include "log.h" + +void controllerLeeFirmwareInit(void) +{ + controllerLeeInit(&g_self); +} + +bool controllerLeeFirmwareTest(void) +{ + return true; +} + +void controllerLeeFirmware(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + controllerLee(&g_self, control, setpoint, sensors, state, tick); +} + PARAM_GROUP_START(ctrlLee) -PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x) -PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y) -PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z) +PARAM_ADD(PARAM_FLOAT, KR_x, &g_self.KR.x) +PARAM_ADD(PARAM_FLOAT, KR_y, &g_self.KR.y) +PARAM_ADD(PARAM_FLOAT, KR_z, &g_self.KR.z) // Attitude I -PARAM_ADD(PARAM_FLOAT, KI_x, &KI.x) -PARAM_ADD(PARAM_FLOAT, KI_y, &KI.y) -PARAM_ADD(PARAM_FLOAT, KI_z, &KI.z) +PARAM_ADD(PARAM_FLOAT, KI_x, &g_self.KI.x) +PARAM_ADD(PARAM_FLOAT, KI_y, &g_self.KI.y) +PARAM_ADD(PARAM_FLOAT, KI_z, &g_self.KI.z) // Attitude D -PARAM_ADD(PARAM_FLOAT, Kw_x, &Komega.x) -PARAM_ADD(PARAM_FLOAT, Kw_y, &Komega.y) -PARAM_ADD(PARAM_FLOAT, Kw_z, &Komega.z) +PARAM_ADD(PARAM_FLOAT, Kw_x, &g_self.Komega.x) +PARAM_ADD(PARAM_FLOAT, Kw_y, &g_self.Komega.y) +PARAM_ADD(PARAM_FLOAT, Kw_z, &g_self.Komega.z) // J -PARAM_ADD(PARAM_FLOAT, J_x, &J.x) -PARAM_ADD(PARAM_FLOAT, J_y, &J.y) -PARAM_ADD(PARAM_FLOAT, J_z, &J.z) +PARAM_ADD(PARAM_FLOAT, J_x, &g_self.J.x) +PARAM_ADD(PARAM_FLOAT, J_y, &g_self.J.y) +PARAM_ADD(PARAM_FLOAT, J_z, &g_self.J.z) // Position P -PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x) -PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y) -PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z) -PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit) +PARAM_ADD(PARAM_FLOAT, Kpos_Px, &g_self.Kpos_P.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Py, &g_self.Kpos_P.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &g_self.Kpos_P.z) +PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &g_self.Kpos_P_limit) // Position D -PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x) -PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y) -PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z) -PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit) +PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &g_self.Kpos_D.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &g_self.Kpos_D.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &g_self.Kpos_D.z) +PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &g_self.Kpos_D_limit) // Position I -PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x) -PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) -PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) -PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) +PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &g_self.Kpos_I.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &g_self.Kpos_I.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &g_self.Kpos_I.z) +PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &g_self.Kpos_I_limit) -PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) +PARAM_ADD(PARAM_FLOAT, mass, &g_self.mass) PARAM_GROUP_STOP(ctrlLee) LOG_GROUP_START(ctrlLee) -LOG_ADD(LOG_FLOAT, KR_x, &KR.x) -LOG_ADD(LOG_FLOAT, KR_y, &KR.y) -LOG_ADD(LOG_FLOAT, KR_z, &KR.z) -LOG_ADD(LOG_FLOAT, Kw_x, &Komega.x) -LOG_ADD(LOG_FLOAT, Kw_y, &Komega.y) -LOG_ADD(LOG_FLOAT, Kw_z, &Komega.z) +LOG_ADD(LOG_FLOAT, KR_x, &g_self.KR.x) +LOG_ADD(LOG_FLOAT, KR_y, &g_self.KR.y) +LOG_ADD(LOG_FLOAT, KR_z, &g_self.KR.z) +LOG_ADD(LOG_FLOAT, Kw_x, &g_self.Komega.x) +LOG_ADD(LOG_FLOAT, Kw_y, &g_self.Komega.y) +LOG_ADD(LOG_FLOAT, Kw_z, &g_self.Komega.z) -LOG_ADD(LOG_FLOAT,Kpos_Px, &Kpos_P.x) -LOG_ADD(LOG_FLOAT,Kpos_Py, &Kpos_P.y) -LOG_ADD(LOG_FLOAT,Kpos_Pz, &Kpos_P.z) -LOG_ADD(LOG_FLOAT,Kpos_Dx, &Kpos_D.x) -LOG_ADD(LOG_FLOAT,Kpos_Dy, &Kpos_D.y) -LOG_ADD(LOG_FLOAT,Kpos_Dz, &Kpos_D.z) +LOG_ADD(LOG_FLOAT,Kpos_Px, &g_self.Kpos_P.x) +LOG_ADD(LOG_FLOAT,Kpos_Py, &g_self.Kpos_P.y) +LOG_ADD(LOG_FLOAT,Kpos_Pz, &g_self.Kpos_P.z) +LOG_ADD(LOG_FLOAT,Kpos_Dx, &g_self.Kpos_D.x) +LOG_ADD(LOG_FLOAT,Kpos_Dy, &g_self.Kpos_D.y) +LOG_ADD(LOG_FLOAT,Kpos_Dz, &g_self.Kpos_D.z) -LOG_ADD(LOG_FLOAT, thrustSI, &thrustSI) -LOG_ADD(LOG_FLOAT, torquex, &u.x) -LOG_ADD(LOG_FLOAT, torquey, &u.y) -LOG_ADD(LOG_FLOAT, torquez, &u.z) +LOG_ADD(LOG_FLOAT, thrustSI, &g_self.thrustSI) +LOG_ADD(LOG_FLOAT, torquex, &g_self.u.x) +LOG_ADD(LOG_FLOAT, torquey, &g_self.u.y) +LOG_ADD(LOG_FLOAT, torquez, &g_self.u.z) // current angles -LOG_ADD(LOG_FLOAT, rpyx, &rpy.x) -LOG_ADD(LOG_FLOAT, rpyy, &rpy.y) -LOG_ADD(LOG_FLOAT, rpyz, &rpy.z) +LOG_ADD(LOG_FLOAT, rpyx, &g_self.rpy.x) +LOG_ADD(LOG_FLOAT, rpyy, &g_self.rpy.y) +LOG_ADD(LOG_FLOAT, rpyz, &g_self.rpy.z) // desired angles -LOG_ADD(LOG_FLOAT, rpydx, &rpy_des.x) -LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) -LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) +LOG_ADD(LOG_FLOAT, rpydx, &g_self.rpy_des.x) +LOG_ADD(LOG_FLOAT, rpydy, &g_self.rpy_des.y) +LOG_ADD(LOG_FLOAT, rpydz, &g_self.rpy_des.z) // errors -LOG_ADD(LOG_FLOAT, error_posx, &p_error.x) -LOG_ADD(LOG_FLOAT, error_posy, &p_error.y) -LOG_ADD(LOG_FLOAT, error_posz, &p_error.z) +LOG_ADD(LOG_FLOAT, error_posx, &g_self.p_error.x) +LOG_ADD(LOG_FLOAT, error_posy, &g_self.p_error.y) +LOG_ADD(LOG_FLOAT, error_posz, &g_self.p_error.z) -LOG_ADD(LOG_FLOAT, error_velx, &v_error.x) -LOG_ADD(LOG_FLOAT, error_vely, &v_error.y) -LOG_ADD(LOG_FLOAT, error_velz, &v_error.z) +LOG_ADD(LOG_FLOAT, error_velx, &g_self.v_error.x) +LOG_ADD(LOG_FLOAT, error_vely, &g_self.v_error.y) +LOG_ADD(LOG_FLOAT, error_velz, &g_self.v_error.z) // omega -LOG_ADD(LOG_FLOAT, omegax, &omega.x) -LOG_ADD(LOG_FLOAT, omegay, &omega.y) -LOG_ADD(LOG_FLOAT, omegaz, &omega.z) +LOG_ADD(LOG_FLOAT, omegax, &g_self.omega.x) +LOG_ADD(LOG_FLOAT, omegay, &g_self.omega.y) +LOG_ADD(LOG_FLOAT, omegaz, &g_self.omega.z) // omega_r -LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x) -LOG_ADD(LOG_FLOAT, omegary, &omega_r.y) -LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) +LOG_ADD(LOG_FLOAT, omegarx, &g_self.omega_r.x) +LOG_ADD(LOG_FLOAT, omegary, &g_self.omega_r.y) +LOG_ADD(LOG_FLOAT, omegarz, &g_self.omega_r.z) // LOG_ADD(LOG_UINT32, ticks, &ticks) LOG_GROUP_STOP(ctrlLee) + +#endif // CRAZYFLIE_FW defined diff --git a/test_python/test_controller_lee.py b/test_python/test_controller_lee.py index 0dcd9ae46b..1b299b3777 100644 --- a/test_python/test_controller_lee.py +++ b/test_python/test_controller_lee.py @@ -4,7 +4,11 @@ def test_controller_lee(): - cffirmware.controllerLeeInit() + lee = cffirmware.controllerLee_t() + + cffirmware.controllerLeeInit(lee) + # now we can change some controller parameters + lee.Kpos_D.z = 5 control = cffirmware.control_t() setpoint = cffirmware.setpoint_t() @@ -35,7 +39,7 @@ def test_controller_lee(): tick = 100 - cffirmware.controllerLee(control, setpoint,sensors,state,tick) + cffirmware.controllerLee(lee,control,setpoint,sensors,state,tick) # control.thrust will be at a (tuned) hover-state assert control.controlMode == cffirmware.controlModeForceTorque assert control.torque[0] == 0 From c44369c115423f01696f80da4476509f2385839e Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 25 Jul 2022 18:43:10 +0200 Subject: [PATCH 72/85] pass all tests (states of payload should be added), edits in payload controller --- src/modules/src/controller_lee_payload.c | 26 +++++++++++----------- test_python/test_controller_lee_payload.py | 5 ++++- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index b55b31f1ed..5f735fa431 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -38,10 +38,10 @@ TODO #define GRAVITY_MAGNITUDE (9.81f) static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules -static float mp = 0.001; -static float l = 0.001; //length of the cable +static float mp = 0.01; +static float l = 0.663; //length of the cable static float thrustSI; -static float alpha_qidot = 0.9; +static float alpha_qidot = 0.7; static float alpha_acc = 0.9; // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter @@ -155,15 +155,13 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z-l); struct vec plVel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); - struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z); - - struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); - // struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); - // struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); + struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); - struct vec plStPos = mkvec(state->position.x, state->position.y, state->position.z-l); - struct vec plStVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); + struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); + // errors struct vec plpos_e = vclampscl(vsub(plPos_d, plStPos), -Kpos_P_limit, Kpos_P_limit); struct vec plvel_e = vclampscl(vsub(plVel_d, plStVel), -Kpos_D_limit, Kpos_D_limit); @@ -199,18 +197,19 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec acc_filtered = vadd(vscl(1.0f - alpha_acc, acc_prev), vscl(alpha_acc, acc_)); acc_prev = acc_filtered; - // Testing acceleration of payload to be zero + // Testing acceleration of payload to be zero (only gravity) acc_filtered = vneg(grav); struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc_filtered))); // Compute Perpindicular Component - struct vec qdi = vnormalize(vneg(desVirtInp)); + struct vec qdi = vneg(vnormalize(desVirtInp)); struct vec eq = vcross(qdi, qi); struct mat33 skewqi = mcrossmat(qi); struct mat33 skewqi2 = mmul(skewqi,skewqi); struct vec qdidot = vdiv(vsub(qdi, qdi_prev), dt); + qdidot = vzero(); qdi_prev = qdi; struct vec wdi = vcross(qdi, qdidot); struct vec ew = vadd(wi, mvmul(skewqi2, wdi)); @@ -402,6 +401,7 @@ PARAM_ADD(PARAM_FLOAT, Kwz, &K_w.z) PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) PARAM_ADD(PARAM_FLOAT, massP, &mp) PARAM_ADD(PARAM_FLOAT, length, &l) +PARAM_ADD(PARAM_FLOAT, alpha_qidot, &alpha_qidot) PARAM_GROUP_STOP(ctrlLeeP) @@ -463,4 +463,4 @@ LOG_GROUP_STOP(ctrlLeeP) - \ No newline at end of file + diff --git a/test_python/test_controller_lee_payload.py b/test_python/test_controller_lee_payload.py index 8f8a91e1e2..ef74addb37 100644 --- a/test_python/test_controller_lee_payload.py +++ b/test_python/test_controller_lee_payload.py @@ -28,12 +28,15 @@ def test_controller_lee_payload(): state.velocity.y = 0 state.velocity.z = 0 + state.payload_pos.x = 0 + state.payload_pos.y = 0 + state.payload_pos.z = -0.784 sensors = cffirmware.sensorData_t() sensors.gyro.x = 0 sensors.gyro.y = 0 sensors.gyro.z = 0 - tick = 100 + tick = 500 cffirmware.controllerLeePayload(control, setpoint,sensors,state,tick) # control.thrust will be at a (tuned) hover-state From d4ae638b39ea783731ff8f1988af92aad8632070 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Tue, 26 Jul 2022 17:04:42 +0200 Subject: [PATCH 73/85] final edits for payload lee controller --- src/modules/src/controller_lee_payload.c | 36 ++++++---------------- test_python/test_controller_lee_payload.py | 4 +++ 2 files changed, 14 insertions(+), 26 deletions(-) diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index 5f735fa431..7bdb3cae87 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -38,11 +38,9 @@ TODO #define GRAVITY_MAGNITUDE (9.81f) static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules -static float mp = 0.01; +static float mp = 0.007; static float l = 0.663; //length of the cable static float thrustSI; -static float alpha_qidot = 0.7; -static float alpha_acc = 0.9; // Inertia matrix (diagonal matrix), see // System Identification of the Crazyflie 2.0 Nano Quadrocopter // BA theses, Julian Foerster, ETHZ @@ -158,6 +156,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); @@ -176,31 +175,17 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec desVirtInp = F_d; //directional unit vector qi and angular velocity wi pointing from UAV to payload - // struct vec qi = vnormalize(vsub(plStPos, statePos)); - struct vec qi = vnormalize(vsub(plStPos, statePos)); //mkvec(0,0,-1); - struct vec qidot = vdiv(vsub(qi , qi_prev), dt); - qi_prev = qi; - struct vec qidot_filtered = vadd(vscl(1.0f - alpha_qidot, qidot_prev), vscl(alpha_qidot, qidot)); - // Testing qidot_filtered to be vzero() - // qidot_filtered = vzero(); - qidot_prev = qidot_filtered; - - struct vec wi = vcross(qi, qidot_filtered); + struct vec qi = vnormalize(vsub(plStPos, statePos)); + struct vec qidot = vdiv(vsub(plStVel, stateVel), vmag(vsub(plStPos, statePos))); + struct vec wi = vcross(qi, qidot); struct mat33 qiqiT = vecmult(qi); struct vec virtualInp = mvmul(qiqiT, desVirtInp); // Compute parallel component - struct vec grav = mkvec(0,0,-GRAVITY_MAGNITUDE); - struct vec acc_ = vsub(vdiv(vsub(plStVel, payload_vel_prev),dt), grav); - payload_vel_prev = plStVel; - struct vec acc_filtered = vadd(vscl(1.0f - alpha_acc, acc_prev), vscl(alpha_acc, acc_)); - acc_prev = acc_filtered; - - // Testing acceleration of payload to be zero (only gravity) - acc_filtered = vneg(grav); + struct vec acc_ = plAcc_d; - struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc_filtered))); + struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc_))); // Compute Perpindicular Component struct vec qdi = vneg(vnormalize(desVirtInp)); @@ -208,15 +193,15 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct mat33 skewqi = mcrossmat(qi); struct mat33 skewqi2 = mmul(skewqi,skewqi); - struct vec qdidot = vdiv(vsub(qdi, qdi_prev), dt); - qdidot = vzero(); + // struct vec qdidot = vdiv(vsub(qdi, qdi_prev), dt); + struct vec qdidot = vzero(); qdi_prev = qdi; struct vec wdi = vcross(qdi, qdidot); struct vec ew = vadd(wi, mvmul(skewqi2, wdi)); struct vec u_perpind = vsub( vscl(g_vehicleMass*l, mvmul(skewqi, vsub(vneg(veltmul(K_q, eq)), veltmul(K_w, ew)))), - vscl(g_vehicleMass, mvmul(skewqi2,acc_filtered)) + vscl(g_vehicleMass, mvmul(skewqi2,acc_)) ); u_i = vadd(u_parallel, u_perpind); @@ -401,7 +386,6 @@ PARAM_ADD(PARAM_FLOAT, Kwz, &K_w.z) PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) PARAM_ADD(PARAM_FLOAT, massP, &mp) PARAM_ADD(PARAM_FLOAT, length, &l) -PARAM_ADD(PARAM_FLOAT, alpha_qidot, &alpha_qidot) PARAM_GROUP_STOP(ctrlLeeP) diff --git a/test_python/test_controller_lee_payload.py b/test_python/test_controller_lee_payload.py index ef74addb37..aacd08e99e 100644 --- a/test_python/test_controller_lee_payload.py +++ b/test_python/test_controller_lee_payload.py @@ -31,6 +31,10 @@ def test_controller_lee_payload(): state.payload_pos.x = 0 state.payload_pos.y = 0 state.payload_pos.z = -0.784 + state.payload_vel.x = 0 + state.payload_vel.y = 0 + state.payload_vel.z = 0 + sensors = cffirmware.sensorData_t() sensors.gyro.x = 0 sensors.gyro.y = 0 From 2141106bdeb3b2371a2462df4a5e0d3b18c77b67 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Thu, 28 Jul 2022 17:28:13 +0200 Subject: [PATCH 74/85] refactor controllerLeePayload to a use a state/param struct --- .vscode/settings.json | 3 +- .../interface/controller_lee_payload.h | 67 ++- src/modules/src/controller.c | 2 +- src/modules/src/controller_lee_payload.c | 397 +++++++++--------- 4 files changed, 266 insertions(+), 203 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 76cc761fa0..34f4c41133 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -5,7 +5,8 @@ "**/bower_components": true }, "files.associations": { - "bootloader.h": "c" + "bootloader.h": "c", + "controller_lee.h": "c" }, "C_Cpp.errorSquiggles": "Disabled" } diff --git a/src/modules/interface/controller_lee_payload.h b/src/modules/interface/controller_lee_payload.h index f862c91df5..1f664b0dfe 100644 --- a/src/modules/interface/controller_lee_payload.h +++ b/src/modules/interface/controller_lee_payload.h @@ -27,11 +27,72 @@ #include "stabilizer_types.h" -void controllerLeePayloadInit(void); -bool controllerLeePayloadTest(void); -void controllerLeePayload(control_t *control, setpoint_t *setpoint, +// This structure contains the mutable state and inmutable parameters +typedef struct controllerLeePayload_s { + float mass; // TODO: should be CF global for other modules + float mp; // mass payload + float l; // length of cable; + float thrustSI; + struct vec J; // Inertia matrix (diagonal matrix); kg m^2 + float offset; // offset for reference + //Position PID + struct vec Kpos_P; + float Kpos_P_limit; + struct vec Kpos_D; + float Kpos_D_limit; + struct vec Kpos_I; + float Kpos_I_limit; + + struct vec i_error_pos; + struct vec i_error_att; + + // Cable PD + struct vec K_q; + struct vec K_w; + + // Cable errors + struct vec plp_error; + struct vec plv_error; + + //Attitude PID + struct vec KR; + struct vec Komega; + struct vec KI; + + // Logging variables + struct vec qi_prev; + struct vec qdi_prev; + struct vec payload_vel_prev; + struct vec rpy; + struct vec rpy_des; + struct vec qr; + struct mat33 R_des; + struct quat q; + struct mat33 R; + struct vec omega; + struct vec omega_r; + struct vec u; + struct vec u_i; + struct vec qidot_prev; + struct vec acc_prev; + +} controllerLeePayload_t; + + +void controllerLeePayloadInit(controllerLeePayload_t* self); +void controllerLeePayloadReset(controllerLeePayload_t* self); +void controllerLeePayload(controllerLeePayload_t* self, control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); + +#ifdef CRAZYFLIE_FW +void controllerLeePayloadFirmwareInit(void); +bool controllerLeePayloadFirmwareTest(void); +void controllerLeePayloadFirmware(control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick); +#endif // CRAZYFLIE_FW defined #endif //__CONTROLLER_LEE_PAYLOAD_H__ diff --git a/src/modules/src/controller.c b/src/modules/src/controller.c index dc60f7d449..9f73f61c96 100644 --- a/src/modules/src/controller.c +++ b/src/modules/src/controller.c @@ -33,7 +33,7 @@ static ControllerFcns controllerFunctions[] = { {.init = controllerSJCInit, .test = controllerSJCTest, .update = controllerSJC, .name = "SJC"}, {.init = controllerMellingerSIInit, .test = controllerMellingerSITest, .update = controllerMellingerSI, .name = "MellingerSI"}, {.init = controllerLeeFirmwareInit, .test = controllerLeeFirmwareTest, .update = controllerLeeFirmware, .name = "Lee"}, - {.init = controllerLeePayloadInit, .test = controllerLeePayloadTest, .update = controllerLeePayload, .name = "LeePayload"}, + {.init = controllerLeePayloadFirmwareInit, .test = controllerLeePayloadFirmwareTest, .update = controllerLeePayloadFirmware, .name = "LeePayload"}, }; diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index 7bdb3cae87..10a6ee61b2 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -29,65 +29,41 @@ TODO */ #include - -#include "param.h" -#include "log.h" +#include #include "math3d.h" #include "controller_lee_payload.h" #define GRAVITY_MAGNITUDE (9.81f) -static float g_vehicleMass = 0.034; // TODO: should be CF global for other modules -static float mp = 0.007; -static float l = 0.663; //length of the cable -static float thrustSI; -// Inertia matrix (diagonal matrix), see -// System Identification of the Crazyflie 2.0 Nano Quadrocopter -// BA theses, Julian Foerster, ETHZ -// https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 -static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2 - -// initValues: -static struct vec qi_prev; -static struct vec qdi_prev; -static struct vec payload_vel_prev; -// Position PID -static struct vec Kpos_P = {60, 60, 90}; //{4, 4, 4}; // Kp in paper -static float Kpos_P_limit = 100; -static struct vec Kpos_D = {0, 0, 0}; //{2, 2, 2}; // Kv in paper -static float Kpos_D_limit = 100; -static struct vec Kpos_I = {0, 0, 0}; // not in paper -static float Kpos_I_limit = 2; -static struct vec i_error_pos; - -// Cable gains -static struct vec K_q = {160, 160, 160}; //{10, 10, 10}; -static struct vec K_w = {0, 0, 0}; //{8, 8, 8}; - - -static struct vec plp_error; -static struct vec plv_error; -// Attitude PID -static struct vec KR = {0.005, 0.005, 0.005}; -static struct vec Komega = {0.0009, 0.0009, 0.0009}; -static struct vec KI = {0.012, 0.018, 0.015}; -static struct vec i_error_att; - - -// Logging variables -static struct vec rpy; -static struct vec rpy_des; -static struct vec qr; -static struct mat33 R_des; -static struct quat q; -static struct mat33 R; -static struct vec omega; -static struct vec omega_r; -static struct vec u; -static struct vec u_i; -static struct vec qidot_prev; -static struct vec acc_prev; -// static uint32_t ticks; +static controllerLeePayload_t g_self = { + .mass = 0.034, + .mp = 0.01, + .l = 0.6, + .offset = 0.0, + // Inertia matrix (diagonal matrix), see + // System Identification of the Crazyflie 2.0 Nano Quadrocopter + // BA theses, Julian Foerster, ETHZ + // https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 + .J = {16.571710e-6, 16.655602e-6, 29.261652e-6}, // kg m^2 + + // Position PID + .Kpos_P = {18, 18, 18}, + .Kpos_P_limit = 100, + .Kpos_D = {15, 15, 15}, + .Kpos_D_limit = 100, + .Kpos_I ={0, 0, 0}, + .Kpos_I_limit = 100, + + // Cables PD + .K_q = {25, 25, 25}, + .K_w = {24, 24, 24}, + + //Attitude PID + .KR = {0.008, 0.008, 0.01}, + .Komega = {0.0013, 0.0013, 0.002}, + .KI = {0.02, 0.02, 0.05}, + +}; static inline struct vec vclampscl(struct vec value, float min, float max) { return mkvec( @@ -96,36 +72,35 @@ static inline struct vec vclampscl(struct vec value, float min, float max) { clamp(value.z, min, max)); } -void controllerLeePayloadReset(void) +void controllerLeePayloadReset(controllerLeePayload_t* self) { - i_error_pos = vzero(); - i_error_att = vzero(); - qi_prev = mkvec(0,0,-1); - qidot_prev = vzero(); - acc_prev = vzero(); - payload_vel_prev = vzero(); - qdi_prev = vzero(); + self->i_error_pos = vzero(); + self->i_error_att = vzero(); + self->qi_prev = mkvec(0,0,-1); + self->qidot_prev = vzero(); + self->acc_prev = vzero(); + self->payload_vel_prev = vzero(); + self->qdi_prev = vzero(); } -void controllerLeePayloadInit(void) +void controllerLeePayloadInit(controllerLeePayload_t* self) { - controllerLeePayloadReset(); + // copy default values (bindings), or NOP (firmware) + *self = g_self; + + controllerLeePayloadReset(self); } -bool controllerLeePayloadTest(void) +bool controllerLeePayloadTest(controllerLeePayload_t* self) { return true; } -// static inline struct vec u_parallel() - -void controllerLeePayload(control_t *control, setpoint_t *setpoint, - const sensorData_t *sensors, - const state_t *state, - const uint32_t tick) +void controllerLeePayload(controllerLeePayload_t* self, control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) { - - if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { return; } @@ -142,8 +117,8 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, desiredYaw = radians(setpoint->attitude.yaw); } else if (setpoint->mode.quat == modeAbs) { struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); - rpy_des = quat2rpy(setpoint_quat); - desiredYaw = rpy_des.z; + self->rpy_des = quat2rpy(setpoint_quat); + desiredYaw = self->rpy_des.z; } // Position controller @@ -151,7 +126,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, || setpoint->mode.y == modeAbs || setpoint->mode.z == modeAbs) { - struct vec plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z-l); + struct vec plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z-self->offset); struct vec plVel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); struct vec plAcc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); @@ -160,18 +135,17 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); - // errors - struct vec plpos_e = vclampscl(vsub(plPos_d, plStPos), -Kpos_P_limit, Kpos_P_limit); - struct vec plvel_e = vclampscl(vsub(plVel_d, plStVel), -Kpos_D_limit, Kpos_D_limit); + struct vec plpos_e = vclampscl(vsub(plPos_d, plStPos), -self->Kpos_P_limit, self->Kpos_P_limit); + struct vec plvel_e = vclampscl(vsub(plVel_d, plStVel), -self->Kpos_D_limit, self->Kpos_D_limit); - plp_error = plpos_e; - plv_error = plvel_e; + self->plp_error = plpos_e; + self->plv_error = plvel_e; - struct vec F_d =vscl(mp ,vadd3( + struct vec F_d =vscl(self->mp ,vadd3( plAcc_d, - veltmul(Kpos_P, plpos_e), - veltmul(Kpos_D, plvel_e))); + veltmul(self->Kpos_P, plpos_e), + veltmul(self->Kpos_D, plvel_e))); struct vec desVirtInp = F_d; //directional unit vector qi and angular velocity wi pointing from UAV to payload @@ -185,7 +159,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // Compute parallel component struct vec acc_ = plAcc_d; - struct vec u_parallel = vadd3(virtualInp, vscl(g_vehicleMass*l*vmag2(wi), qi), vscl(g_vehicleMass, mvmul(qiqiT, acc_))); + struct vec u_parallel = vadd3(virtualInp, vscl(self->mass*self->l*vmag2(wi), qi), vscl(self->mass, mvmul(qiqiT, acc_))); // Compute Perpindicular Component struct vec qdi = vneg(vnormalize(desVirtInp)); @@ -195,41 +169,40 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // struct vec qdidot = vdiv(vsub(qdi, qdi_prev), dt); struct vec qdidot = vzero(); - qdi_prev = qdi; + self->qdi_prev = qdi; struct vec wdi = vcross(qdi, qdidot); struct vec ew = vadd(wi, mvmul(skewqi2, wdi)); struct vec u_perpind = vsub( - vscl(g_vehicleMass*l, mvmul(skewqi, vsub(vneg(veltmul(K_q, eq)), veltmul(K_w, ew)))), - vscl(g_vehicleMass, mvmul(skewqi2,acc_)) + vscl(self->mass*self->l, mvmul(skewqi, vsub(vneg(veltmul(self->K_q, eq)), veltmul(self->K_w, ew)))), + vscl(self->mass, mvmul(skewqi2, acc_)) ); - u_i = vadd(u_parallel, u_perpind); + self->u_i = vadd(u_parallel, u_perpind); - control->thrustSI = vmag(u_i); - control->u_all[0] = u_i.x; - control->u_all[1] = u_i.y; - control->u_all[2] = u_i.z; + control->thrustSI = vmag(self->u_i); + control->u_all[0] = self->u_i.x; + control->u_all[1] = self->u_i.y; + control->u_all[2] = self->u_i.z; - thrustSI = control->thrustSI; + self->thrustSI = control->thrustSI; // Reset the accumulated error while on the ground if (control->thrustSI < 0.01f) { - controllerLeePayloadReset(); + controllerLeePayloadReset(self); } - q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); - rpy = quat2rpy(q); - R = quat2rotmat(q); - // struct vec z = vbasis(2); + self->q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + self->rpy = quat2rpy(self->q); + self->R = quat2rotmat(self->q); + // Compute Desired Rotation matrix - float thrustSI = control->thrustSI; - struct vec Fd_ = u_i; + struct vec Fd_ = self->u_i; struct vec xdes = vbasis(0); struct vec ydes = vbasis(1); struct vec zdes = vbasis(2); - if (thrustSI > 0) { + if (self->thrustSI > 0) { zdes = vnormalize(Fd_); } struct vec xcdes = mkvec(cosf(desiredYaw), sinf(desiredYaw), 0); @@ -241,7 +214,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, } xdes = vcross(ydes, zdes); - R_des = mcolumns(xdes, ydes, zdes); + self->R_des = mcolumns(xdes, ydes, zdes); } else { if (setpoint->mode.z == modeDisable) { @@ -251,7 +224,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, control->torque[0] = 0; control->torque[1] = 0; control->torque[2] = 0; - controllerLeePayloadReset(); + controllerLeePayloadReset(self); return; } } @@ -259,7 +232,7 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, const float max_thrust = 70.0f / 1000.0f * 9.81f; // N control->thrustSI = setpoint->thrust / UINT16_MAX * max_thrust; - qr = mkvec( + self->qr = mkvec( radians(setpoint->attitude.roll), -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted desiredYaw); @@ -268,180 +241,208 @@ void controllerLeePayload(control_t *control, setpoint_t *setpoint, // Attitude controller // current rotation [R] - q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); - rpy = quat2rpy(q); - struct mat33 R = quat2rotmat(q); + self->q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + self->rpy = quat2rpy(self->q); + self->R = quat2rotmat(self->q); // desired rotation [Rdes] - struct quat q_des = mat2quat(R_des); - rpy_des = quat2rpy(q_des); + struct quat q_des = mat2quat(self->R_des); + self->rpy_des = quat2rpy(q_des); // rotation error - struct mat33 eRM = msub(mmul(mtranspose(R_des), R), mmul(mtranspose(R), R_des)); + struct mat33 eRM = msub(mmul(mtranspose(self->R_des), self->R), mmul(mtranspose(self->R), self->R_des)); struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0])); // angular velocity - omega = mkvec( + self->omega = mkvec( radians(sensors->gyro.x), radians(sensors->gyro.y), radians(sensors->gyro.z)); // Compute desired omega - struct vec xdes = mcolumn(R_des, 0); - struct vec ydes = mcolumn(R_des, 1); - struct vec zdes = mcolumn(R_des, 2); + struct vec xdes = mcolumn(self->R_des, 0); + struct vec ydes = mcolumn(self->R_des, 1); + struct vec zdes = mcolumn(self->R_des, 2); struct vec hw = vzero(); // Desired Jerk and snap for now are zeros vector struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z); if (control->thrustSI != 0) { struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); - hw = vscl(g_vehicleMass/control->thrustSI, tmp); + hw = vscl(self->mass/control->thrustSI, tmp); } - struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), 0); + struct vec z_w = mkvec(0, 0, 1); + float desiredYawRate = radians(setpoint->attitudeRate.yaw) * vdot(zdes, z_w); + struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYawRate); - omega_r = mvmul(mmul(mtranspose(R), R_des), omega_des); + self->omega_r = mvmul(mmul(mtranspose(self->R), self->R_des), omega_des); - struct vec omega_error = vsub(omega, omega_r); + struct vec omega_error = vsub(self->omega, self->omega_r); // Integral part on angle - i_error_att = vadd(i_error_att, vscl(dt, eR)); + self->i_error_att = vadd(self->i_error_att, vscl(dt, eR)); // compute moments // M = -kR eR - kw ew + w x Jw - J(w x wr) - u = vadd4( - vneg(veltmul(KR, eR)), - vneg(veltmul(Komega, omega_error)), - vneg(veltmul(KI, i_error_att)), - vcross(omega, veltmul(J, omega))); + self->u = vadd4( + vneg(veltmul(self->KR, eR)), + vneg(veltmul(self->Komega, omega_error)), + vneg(veltmul(self->KI, self->i_error_att)), + vcross(self->omega, veltmul(self->J, self->omega))); // if (enableNN > 1) { // u = vsub(u, tau_a); // } control->controlMode = controlModeForceTorque; - control->torque[0] = u.x; - control->torque[1] = u.y; - control->torque[2] = u.z; + control->torque[0] = self->u.x; + control->torque[1] = self->u.y; + control->torque[2] = self->u.z; + // ticks = usecTimestamp() - startTime; } +#ifdef CRAZYFLIE_FW + +#include "param.h" +#include "log.h" + +void controllerLeePayloadFirmwareInit(void) +{ + controllerLeePayloadInit(&g_self); +} + +bool controllerLeePayloadFirmwareTest(void) +{ + return true; +} + +void controllerLeePayloadFirmware(control_t *control, setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + controllerLeePayload(&g_self, control, setpoint, sensors, state, tick); +} + PARAM_GROUP_START(ctrlLeeP) -PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x) -PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y) -PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z) +PARAM_ADD(PARAM_FLOAT, KR_x, &g_self.KR.x) +PARAM_ADD(PARAM_FLOAT, KR_y, &g_self.KR.y) +PARAM_ADD(PARAM_FLOAT, KR_z, &g_self.KR.z) // Attitude D -PARAM_ADD(PARAM_FLOAT, Kw_x, &Komega.x) -PARAM_ADD(PARAM_FLOAT, Kw_y, &Komega.y) -PARAM_ADD(PARAM_FLOAT, Kw_z, &Komega.z) +PARAM_ADD(PARAM_FLOAT, Kw_x, &g_self.Komega.x) +PARAM_ADD(PARAM_FLOAT, Kw_y, &g_self.Komega.y) +PARAM_ADD(PARAM_FLOAT, Kw_z, &g_self.Komega.z) // J -PARAM_ADD(PARAM_FLOAT, J_x, &J.x) -PARAM_ADD(PARAM_FLOAT, J_y, &J.y) -PARAM_ADD(PARAM_FLOAT, J_z, &J.z) +PARAM_ADD(PARAM_FLOAT, J_x, &g_self.J.x) +PARAM_ADD(PARAM_FLOAT, J_y, &g_self.J.y) +PARAM_ADD(PARAM_FLOAT, J_z, &g_self.J.z) // Position P -PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x) -PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y) -PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z) -PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit) +PARAM_ADD(PARAM_FLOAT, Kpos_Px, &g_self.Kpos_P.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Py, &g_self.Kpos_P.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &g_self.Kpos_P.z) +PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &g_self.Kpos_P_limit) // Position D -PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x) -PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y) -PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z) -PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit) +PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &g_self.Kpos_D.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &g_self.Kpos_D.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &g_self.Kpos_D.z) +PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &g_self.Kpos_D_limit) // Position I -PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x) -PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y) -PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z) -PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit) +PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &g_self.Kpos_I.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &g_self.Kpos_I.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &g_self.Kpos_I.z) +PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &g_self.Kpos_I_limit) // Attitude P -PARAM_ADD(PARAM_FLOAT, KRx, &KR.x) -PARAM_ADD(PARAM_FLOAT, KRy, &KR.y) -PARAM_ADD(PARAM_FLOAT, KRz, &KR.z) +PARAM_ADD(PARAM_FLOAT, KRx, &g_self.KR.x) +PARAM_ADD(PARAM_FLOAT, KRy, &g_self.KR.y) +PARAM_ADD(PARAM_FLOAT, KRz, &g_self.KR.z) // Attitude D -PARAM_ADD(PARAM_FLOAT, Komx, &Komega.x) -PARAM_ADD(PARAM_FLOAT, Komy, &Komega.y) -PARAM_ADD(PARAM_FLOAT, Komz, &Komega.z) +PARAM_ADD(PARAM_FLOAT, Komx, &g_self.Komega.x) +PARAM_ADD(PARAM_FLOAT, Komy, &g_self.Komega.y) +PARAM_ADD(PARAM_FLOAT, Komz, &g_self.Komega.z) // Attitude I -PARAM_ADD(PARAM_FLOAT, KI_x, &KI.x) -PARAM_ADD(PARAM_FLOAT, KI_y, &KI.y) -PARAM_ADD(PARAM_FLOAT, KI_z, &KI.z) +PARAM_ADD(PARAM_FLOAT, KI_x, &g_self.KI.x) +PARAM_ADD(PARAM_FLOAT, KI_y, &g_self.KI.y) +PARAM_ADD(PARAM_FLOAT, KI_z, &g_self.KI.z) // Cable P -PARAM_ADD(PARAM_FLOAT, Kqx, &K_q.x) -PARAM_ADD(PARAM_FLOAT, Kqy, &K_q.y) -PARAM_ADD(PARAM_FLOAT, Kqz, &K_q.z) +PARAM_ADD(PARAM_FLOAT, Kqx, &g_self.K_q.x) +PARAM_ADD(PARAM_FLOAT, Kqy, &g_self.K_q.y) +PARAM_ADD(PARAM_FLOAT, Kqz, &g_self.K_q.z) // Cable D -PARAM_ADD(PARAM_FLOAT, Kwx, &K_w.x) -PARAM_ADD(PARAM_FLOAT, Kwy, &K_w.y) -PARAM_ADD(PARAM_FLOAT, Kwz, &K_w.z) +PARAM_ADD(PARAM_FLOAT, Kwx, &g_self.K_w.x) +PARAM_ADD(PARAM_FLOAT, Kwy, &g_self.K_w.y) +PARAM_ADD(PARAM_FLOAT, Kwz, &g_self.K_w.z) -PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass) -PARAM_ADD(PARAM_FLOAT, massP, &mp) -PARAM_ADD(PARAM_FLOAT, length, &l) +PARAM_ADD(PARAM_FLOAT, mass, &g_self.mass) +PARAM_ADD(PARAM_FLOAT, massP, &g_self.mp) +PARAM_ADD(PARAM_FLOAT, length, &g_self.l) +PARAM_ADD(PARAM_FLOAT, offset, &g_self.offset) PARAM_GROUP_STOP(ctrlLeeP) LOG_GROUP_START(ctrlLeeP) -LOG_ADD(LOG_FLOAT,Kpos_Px, &Kpos_P.x) -LOG_ADD(LOG_FLOAT,Kpos_Py, &Kpos_P.y) -LOG_ADD(LOG_FLOAT,Kpos_Pz, &Kpos_P.z) -LOG_ADD(LOG_FLOAT,Kpos_Dx, &Kpos_D.x) -LOG_ADD(LOG_FLOAT,Kpos_Dy, &Kpos_D.y) -LOG_ADD(LOG_FLOAT,Kpos_Dz, &Kpos_D.z) +LOG_ADD(LOG_FLOAT,Kpos_Px, &g_self.Kpos_P.x) +LOG_ADD(LOG_FLOAT,Kpos_Py, &g_self.Kpos_P.y) +LOG_ADD(LOG_FLOAT,Kpos_Pz, &g_self.Kpos_P.z) +LOG_ADD(LOG_FLOAT,Kpos_Dx, &g_self.Kpos_D.x) +LOG_ADD(LOG_FLOAT,Kpos_Dy, &g_self.Kpos_D.y) +LOG_ADD(LOG_FLOAT,Kpos_Dz, &g_self.Kpos_D.z) -LOG_ADD(LOG_FLOAT, Kqx, &K_q.x) -LOG_ADD(LOG_FLOAT, Kqy, &K_q.y) -LOG_ADD(LOG_FLOAT, Kqz, &K_q.z) +LOG_ADD(LOG_FLOAT, Kqx, &g_self.K_q.x) +LOG_ADD(LOG_FLOAT, Kqy, &g_self.K_q.y) +LOG_ADD(LOG_FLOAT, Kqz, &g_self.K_q.z) -LOG_ADD(LOG_FLOAT, Kwx, &K_w.x) -LOG_ADD(LOG_FLOAT, Kwy, &K_w.y) -LOG_ADD(LOG_FLOAT, Kwz, &K_w.z) +LOG_ADD(LOG_FLOAT, Kwx, &g_self.K_w.x) +LOG_ADD(LOG_FLOAT, Kwy, &g_self.K_w.y) +LOG_ADD(LOG_FLOAT, Kwz, &g_self.K_w.z) -LOG_ADD(LOG_FLOAT, thrustSI, &thrustSI) -LOG_ADD(LOG_FLOAT, torquex, &u.x) -LOG_ADD(LOG_FLOAT, torquey, &u.y) -LOG_ADD(LOG_FLOAT, torquez, &u.z) +LOG_ADD(LOG_FLOAT, thrustSI, &g_self.thrustSI) +LOG_ADD(LOG_FLOAT, torquex, &g_self.u.x) +LOG_ADD(LOG_FLOAT, torquey, &g_self.u.y) +LOG_ADD(LOG_FLOAT, torquez, &g_self.u.z) // current angles -LOG_ADD(LOG_FLOAT, rpyx, &rpy.x) -LOG_ADD(LOG_FLOAT, rpyy, &rpy.y) -LOG_ADD(LOG_FLOAT, rpyz, &rpy.z) +LOG_ADD(LOG_FLOAT, rpyx, &g_self.rpy.x) +LOG_ADD(LOG_FLOAT, rpyy, &g_self.rpy.y) +LOG_ADD(LOG_FLOAT, rpyz, &g_self.rpy.z) // desired angles -LOG_ADD(LOG_FLOAT, rpydx, &rpy_des.x) -LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y) -LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z) +LOG_ADD(LOG_FLOAT, rpydx, &g_self.rpy_des.x) +LOG_ADD(LOG_FLOAT, rpydy, &g_self.rpy_des.y) +LOG_ADD(LOG_FLOAT, rpydz, &g_self.rpy_des.z) // omega -LOG_ADD(LOG_FLOAT, omegax, &omega.x) -LOG_ADD(LOG_FLOAT, omegay, &omega.y) -LOG_ADD(LOG_FLOAT, omegaz, &omega.z) +LOG_ADD(LOG_FLOAT, omegax, &g_self.omega.x) +LOG_ADD(LOG_FLOAT, omegay, &g_self.omega.y) +LOG_ADD(LOG_FLOAT, omegaz, &g_self.omega.z) // omega_r -LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x) -LOG_ADD(LOG_FLOAT, omegary, &omega_r.y) -LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z) +LOG_ADD(LOG_FLOAT, omegarx, &g_self.omega_r.x) +LOG_ADD(LOG_FLOAT, omegary, &g_self.omega_r.y) +LOG_ADD(LOG_FLOAT, omegarz, &g_self.omega_r.z) -LOG_ADD(LOG_FLOAT, ux, &u_i.x) -LOG_ADD(LOG_FLOAT, uy, &u_i.y) -LOG_ADD(LOG_FLOAT, uz, &u_i.z) +LOG_ADD(LOG_FLOAT, ux, &g_self.u_i.x) +LOG_ADD(LOG_FLOAT, uy, &g_self.u_i.y) +LOG_ADD(LOG_FLOAT, uz, &g_self.u_i.z) // LOG_ADD(LOG_UINT32, ticks, &ticks) LOG_GROUP_STOP(ctrlLeeP) +#endif // CRAZYFLIE_FW defined From b06996317a50c3c177289f1ed1f7b791be014bb9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 16 Aug 2022 12:10:32 +0200 Subject: [PATCH 75/85] fix build error --- src/modules/src/cvmrs.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index f083e1db8a..6ed24fdd49 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -2,7 +2,7 @@ #include "task.h" #include "system.h" #include "log.h" -#include "aideck.h" +#include "cpx_internal_router.h" #include "uart1.h" #include "usec_time.h" #include "debug.h" From 874cf08a0fa5d22380d73a3995d5c7d3a085a5e7 Mon Sep 17 00:00:00 2001 From: akmaralAW Date: Wed, 2 Nov 2022 11:53:37 +0100 Subject: [PATCH 76/85] fixing Cf+aideck crashes while taking off --- src/drivers/src/uart1.c | 9 +++++---- src/modules/src/cvmrs.c | 5 ++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/src/uart1.c b/src/drivers/src/uart1.c index 51477ee4f5..d0663adaa0 100644 --- a/src/drivers/src/uart1.c +++ b/src/drivers/src/uart1.c @@ -42,6 +42,7 @@ #include "config.h" #include "nvicconf.h" #include "static_mem.h" +#include "debug.h" /** This uart is conflicting with SPI2 DMA used in sensors_bmi088_spi_bmp388.c * which is used in CF-Bolt. So for other products this can be enabled. @@ -205,10 +206,10 @@ bool uart1GetDataWithDefaultTimeout(uint8_t *c) void uart1SendData(uint32_t size, uint8_t* data) { uint32_t i; - - if (!isInit) - return; - + if (!isInit){ + DEBUG_PRINT("DO NOT WORK \n"); + return; + } for(i = 0; i < size; i++) { while (!(UART1_TYPE->SR & USART_FLAG_TXE)); diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 6ed24fdd49..10167bf149 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -7,7 +7,6 @@ #include "usec_time.h" #include "debug.h" #include "param.h" - #define TASK_FREQ 100 // GAP8 register settings @@ -17,7 +16,7 @@ static uint8_t aGain; static uint8_t dGain; static uint16_t exposure; static uint8_t trigger = 0; - +const uint32_t baudrate_esp32 = 115200; typedef struct { @@ -66,6 +65,7 @@ void appMain() DEBUG_PRINT("Starting CVMRS task\n"); lastWakeTime = xTaskGetTickCount(); + uart1Init(baudrate_esp32); while (1) { vTaskDelayUntil(&lastWakeTime, F2T(TASK_FREQ)); @@ -76,7 +76,6 @@ void appMain() state_packet.y = logGetInt(y_id); state_packet.z = logGetInt(z_id); state_packet.quat = logGetInt(quat_id); - // cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */); uint8_t magic = 0xBC; From 846b699fd7ea516132671692ade923e93dbe2c12 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 21 Nov 2022 14:27:18 +0100 Subject: [PATCH 77/85] cpx: switch state to uncompressed version --- src/modules/src/cvmrs.c | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 10167bf149..7f1d137c2e 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -22,10 +22,13 @@ typedef struct { uint8_t cmd; uint64_t timestamp; // usec timestamp from STM32 - int16_t x; // compressed [mm] - int16_t y; // compressed [mm] - int16_t z; // compressed [mm] - uint32_t quat; // compressed, see quatcompress.h + float x; // m + float y; // m + float z; // m + float qx; + float qy; + float qz; + float qw; } __attribute__((packed)) StatePacket_t; // static StatePacket_t cf_state; static CPXPacket_t cpx_packet; @@ -48,16 +51,19 @@ void appMain() systemWaitStart(); - logVarId_t x_id = logGetVarId("stateEstimateZ", "x"); - logVarId_t y_id = logGetVarId("stateEstimateZ", "y"); - logVarId_t z_id = logGetVarId("stateEstimateZ", "z"); - logVarId_t quat_id = logGetVarId("stateEstimateZ", "quat"); + logVarId_t x_id = logGetVarId("stateEstimate", "x"); + logVarId_t y_id = logGetVarId("stateEstimate", "y"); + logVarId_t z_id = logGetVarId("stateEstimate", "z"); + logVarId_t qx_id = logGetVarId("stateEstimate", "qx"); + logVarId_t qy_id = logGetVarId("stateEstimate", "qy"); + logVarId_t qz_id = logGetVarId("stateEstimate", "qz"); + logVarId_t qw_id = logGetVarId("stateEstimate", "qw"); cpxInitRoute(CPX_T_STM32, CPX_T_GAP8, CPX_F_APP, &cpx_packet.route); // cpx_packet.dataLength = sizeof(StatePacket_t); // StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data; - state_packet.cmd = 0; + state_packet.cmd = 1; // Delay is only needed for CPX // vTaskDelay(60000); @@ -75,7 +81,10 @@ void appMain() state_packet.x = logGetInt(x_id); state_packet.y = logGetInt(y_id); state_packet.z = logGetInt(z_id); - state_packet.quat = logGetInt(quat_id); + state_packet.qx = logGetInt(qx_id); + state_packet.qy = logGetInt(qy_id); + state_packet.qz = logGetInt(qz_id); + state_packet.qw = logGetInt(qw_id); // cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */); uint8_t magic = 0xBC; From 0a75ab917b29ee09012acede0f11f5a3ad2b570b Mon Sep 17 00:00:00 2001 From: akmaralAW Date: Tue, 22 Nov 2022 11:18:09 +0100 Subject: [PATCH 78/85] fixing zero state info for aideck --- src/modules/src/cvmrs.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 7f1d137c2e..b048e3d128 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -78,13 +78,13 @@ void appMain() // Sending current state information to GAP8 state_packet.timestamp = usecTimestamp(); - state_packet.x = logGetInt(x_id); - state_packet.y = logGetInt(y_id); - state_packet.z = logGetInt(z_id); - state_packet.qx = logGetInt(qx_id); - state_packet.qy = logGetInt(qy_id); - state_packet.qz = logGetInt(qz_id); - state_packet.qw = logGetInt(qw_id); + state_packet.x = logGetFloat(x_id); + state_packet.y = logGetFloat(y_id); + state_packet.z = logGetFloat(z_id); + state_packet.qx = logGetFloat(qx_id); + state_packet.qy = logGetFloat(qy_id); + state_packet.qz = logGetFloat(qz_id); + state_packet.qw = logGetFloat(qw_id); // cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */); uint8_t magic = 0xBC; From f1c85dd4ccd32a5ccec4f3d49d9700394c3bccc9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 19 Jan 2023 13:43:41 +0100 Subject: [PATCH 79/85] switch usddeck.c to IO3 --- src/deck/drivers/src/usddeck.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c index 9f625efd5a..9ad0ab963a 100644 --- a/src/deck/drivers/src/usddeck.c +++ b/src/deck/drivers/src/usddeck.c @@ -80,7 +80,7 @@ #else #include "deck_spi.h" -#define USD_CS_PIN DECK_GPIO_IO4 +#define USD_CS_PIN DECK_GPIO_IO3 #define SPI_BEGIN spiBegin #define USD_SPI_BAUDRATE_2MHZ SPI_BAUDRATE_2MHZ @@ -1067,7 +1067,7 @@ static const DeckDriver usd_deck = { .vid = 0xBC, .pid = 0x08, .name = "bcUSD", - .usedGpio = DECK_USING_IO_4, + .usedGpio = DECK_USING_IO_3, .usedPeriph = DECK_USING_SPI, .init = usdInit, .test = usdTest, From fa3ee9fb78e7f5ef2183db1b5158d96369b9554a Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 11 Jan 2023 14:20:09 +0100 Subject: [PATCH 80/85] Params: add missing callback for writeByName The callback function is used, e.g., for led.bitmask and currently only implemented for paramWriteProcess, not paramWriteByNameProcess --- src/modules/src/param_logic.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/src/param_logic.c b/src/modules/src/param_logic.c index 638e0ba1ee..37502e315a 100644 --- a/src/modules/src/param_logic.c +++ b/src/modules/src/param_logic.c @@ -407,6 +407,10 @@ static char paramWriteByNameProcess(char* group, char* name, int type, void *val paramSet(index, valptr); + if (params[index].callback) { + params[index].callback(); + } + return 0; } From cd07d7fdfacf6ab1ad472cdb9c376d6e9d6e5c0c Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 20 Jan 2023 14:17:47 +0100 Subject: [PATCH 81/85] usec: add support for usec.reset --- src/hal/src/usec_time.c | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/hal/src/usec_time.c b/src/hal/src/usec_time.c index 59d302880f..66215a13ee 100644 --- a/src/hal/src/usec_time.c +++ b/src/hal/src/usec_time.c @@ -27,11 +27,13 @@ #include #include "usec_time.h" #include "cfassert.h" +#include "param.h" #include "nvicconf.h" #include "stm32fxxx.h" static bool isInit = false; +static uint8_t reset = 0; static uint32_t usecTimerHighCount; void initUsecTimer(void) @@ -106,3 +108,20 @@ void __attribute__((used)) TIM7_IRQHandler(void) __sync_fetch_and_add(&usecTimerHighCount, 1); } + +/** + * Parameters to synchronize time + * */ + +static void resetParamCallback(void) +{ + if (reset) { + usecTimerReset(); + reset = 0; + } +} + + +PARAM_GROUP_START(usec) +PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, reset, &reset, &resetParamCallback) +PARAM_GROUP_STOP(usec) \ No newline at end of file From 4313e964bdc6f02c104c8e329d6467edd03343c2 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 24 Jan 2023 10:19:30 +0100 Subject: [PATCH 82/85] bugfix when not using ai deck --- src/deck/drivers/src/usddeck.c | 10 ++++++---- src/modules/src/cvmrs.c | 6 ++++++ 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c index 9ad0ab963a..ba59cc5ef8 100644 --- a/src/deck/drivers/src/usddeck.c +++ b/src/deck/drivers/src/usddeck.c @@ -80,7 +80,8 @@ #else #include "deck_spi.h" -#define USD_CS_PIN DECK_GPIO_IO3 +#define USD_CS_PIN DECK_GPIO_IO4 +#define USD_CS_PIN_USED DECK_USING_IO_4 #define SPI_BEGIN spiBegin #define USD_SPI_BAUDRATE_2MHZ SPI_BAUDRATE_2MHZ @@ -460,7 +461,8 @@ static void usdInit(DeckInfo *info) shutdownMutex = xSemaphoreCreateMutex(); /* try to mount drives before creating the tasks */ - if (f_mount(&FatFs, "", 1) == FR_OK) { + FRESULT r = f_mount(&FatFs, "", 1); + if (r == FR_OK) { DEBUG_PRINT("mount SD-Card [OK].\n"); /* create usd-log task */ @@ -468,7 +470,7 @@ static void usdInit(DeckInfo *info) USDLOG_TASK_STACKSIZE, NULL, USDLOG_TASK_PRI, NULL); } else { - DEBUG_PRINT("mount SD-Card [FAIL].\n"); + DEBUG_PRINT("mount SD-Card [FAIL] (%d).\n", r); } } isInit = true; @@ -1067,7 +1069,7 @@ static const DeckDriver usd_deck = { .vid = 0xBC, .pid = 0x08, .name = "bcUSD", - .usedGpio = DECK_USING_IO_3, + .usedGpio = USD_CS_PIN_USED, .usedPeriph = DECK_USING_SPI, .init = usdInit, .test = usdTest, diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index b048e3d128..66953906a7 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -50,6 +50,12 @@ void appMain() //Wait for the system to be fully started to start stabilization loop systemWaitStart(); + paramVarId_t deck_id = paramGetVarId("deck", "bcAI"); + uint8_t ai_deck_available = paramGetInt(deck_id); + if (!ai_deck_available) { + DEBUG_PRINT("No AI deck - do not start CVMRS task\n"); + return; + } logVarId_t x_id = logGetVarId("stateEstimate", "x"); logVarId_t y_id = logGetVarId("stateEstimate", "y"); From 80c5e63c2fae7bc15596a8dfb6db784427d56cdc Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 9 Feb 2023 22:33:02 +0100 Subject: [PATCH 83/85] disable motors; allow to set isQuadFlying param --- src/deck/drivers/src/usddeck.c | 4 ++-- src/drivers/src/motors.c | 38 +++++++++++++++--------------- src/modules/src/estimator_kalman.c | 10 ++++++-- 3 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c index ba59cc5ef8..3c9f8abbfe 100644 --- a/src/deck/drivers/src/usddeck.c +++ b/src/deck/drivers/src/usddeck.c @@ -80,8 +80,8 @@ #else #include "deck_spi.h" -#define USD_CS_PIN DECK_GPIO_IO4 -#define USD_CS_PIN_USED DECK_USING_IO_4 +#define USD_CS_PIN DECK_GPIO_IO3 +#define USD_CS_PIN_USED DECK_USING_IO_3 #define SPI_BEGIN spiBegin #define USD_SPI_BAUDRATE_2MHZ SPI_BAUDRATE_2MHZ diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 9816d61d6b..12beaedecf 100644 --- a/src/drivers/src/motors.c +++ b/src/drivers/src/motors.c @@ -339,25 +339,25 @@ void motorsDeInit(const MotorPerifDef** motorMapSelect) bool motorsTest(void) { - int i; - - for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++) - { - if (motorMap[i]->drvType == BRUSHED) - { -#ifdef ACTIVATE_STARTUP_SOUND - motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20); - vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); - motorsBeep(MOTORS[i], false, 0, 0); - vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); -#else - motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO); - vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); - motorsSetRatio(MOTORS[i], 0); - vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); -#endif - } - } + // int i; + +// for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++) +// { +// if (motorMap[i]->drvType == BRUSHED) +// { +// #ifdef ACTIVATE_STARTUP_SOUND +// motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20); +// vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); +// motorsBeep(MOTORS[i], false, 0, 0); +// vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); +// #else +// motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO); +// vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); +// motorsSetRatio(MOTORS[i], 0); +// vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); +// #endif +// } +// } return isInit; } diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index 0400a15302..bea26ebfca 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -146,6 +146,7 @@ static uint32_t gyroAccumulatorCount; static Axis3f accLatest; static Axis3f gyroLatest; static bool quadIsFlying = false; +static uint8_t quadIsFlyingOverwrite = 2; // 0 - quad is not flying, 1 - quad is flying, 2 - automatic (default) static OutlierFilterLhState_t sweepOutlierFilterState; @@ -335,7 +336,12 @@ static bool predictStateForward(uint32_t osTick, float dt) { gyroAccumulator = (Axis3f){.axis={0}}; gyroAccumulatorCount = 0; - quadIsFlying = supervisorIsFlying(); + if (quadIsFlyingOverwrite == 2) { + quadIsFlying = supervisorIsFlying(); + } else { + quadIsFlying = quadIsFlyingOverwrite; + } + kalmanCorePredict(&coreData, &accAverage, &gyroAverage, dt, quadIsFlying); return true; @@ -594,7 +600,7 @@ PARAM_GROUP_START(kalman) * @brief Reset the kalman estimator */ PARAM_ADD_CORE(PARAM_UINT8, resetEstimation, &resetEstimation) - PARAM_ADD(PARAM_UINT8, quadIsFlying, &quadIsFlying) + PARAM_ADD(PARAM_UINT8, quadIsFlying, &quadIsFlyingOverwrite) /** * @brief Nonzero to use robust TDOA method (default: 0) */ From cd4548ecaece356d6e1984815339c8344b231232 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 21 Feb 2023 10:08:37 +0100 Subject: [PATCH 84/85] improved auto-yaw --- src/modules/interface/stabilizer_types.h | 3 +- src/modules/src/crtp_commander_high_level.c | 39 ++++++++++++--------- src/modules/src/crtp_localization_service.c | 15 +++++--- src/modules/src/cvmrs.c | 9 +++++ src/modules/src/kalman_core/mm_pose.c | 6 ++-- 5 files changed, 47 insertions(+), 25 deletions(-) diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index a04c6693a4..87e80acd2b 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -126,7 +126,8 @@ typedef struct poseMeasurement_s { }; quaternion_t quat; float stdDevPos; - float stdDevQuat; + float stdDevRollPitch; + float stdDevYaw; } poseMeasurement_t; typedef struct distanceMeasurement_s { diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index c17544f0ae..d7bd4e8be7 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -111,7 +111,9 @@ static StaticSemaphore_t lockTrajBuffer; static float defaultTakeoffVelocity = 0.5f; static float defaultLandingVelocity = 0.5f; -static float yawrate = 0.0f; +static float yawrate_desired = 0.0f; +static float yawacc_limit = 1.0f; +static float yawrate_limit = 5.0f; static float yawrate_current = 0.0f; // Trajectory memory handling from the memory module @@ -306,6 +308,7 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat if (!RATE_DO_EXECUTE(RATE_HL_COMMANDER, tick)) { return false; } + const float dt = (float)(1.0f/RATE_HL_COMMANDER); xSemaphoreTake(lockTraj, portMAX_DELAY); float t = usecTimestamp() / 1e6; @@ -357,22 +360,22 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat // store the last setpoint pos = ev.pos; vel = ev.vel; - // if (yawrate != 0) { - float delta = yawrate - yawrate_current; - if (delta < -0.01f) { - delta = -0.01f; - } - if (delta > 0.01f) { - delta = 0.01f; - } - yawrate_current += delta; - yaw = normalize_radians(yaw + yawrate_current / ((float)(RATE_HL_COMMANDER))); - setpoint->attitude.yaw = degrees(yaw); - setpoint->attitudeRate.yaw = degrees(yawrate_current); - // } else { - // yaw = ev.yaw; - // } + float yawrate_desired_adjusted = yawrate_desired; + if (yawrate_desired == 0.0f) { + float signed_yaw = shortest_signed_angle_radians(yaw, 0.0f); + // attempt to go there in 1.0s + yawrate_desired_adjusted = copysignf(fabsf(signed_yaw/1.0f), signed_yaw); + } + + float yawacc_desired = (yawrate_desired_adjusted - yawrate_current)/dt; + float yawacc = clamp(yawacc_desired, -yawacc_limit, yawacc_limit); + yawrate_current += yawacc * dt; + yawrate_current = clamp(yawrate_current, -yawrate_limit, yawrate_limit); + + yaw = normalize_radians(yaw + yawrate_current * dt); + setpoint->attitude.yaw = degrees(yaw); + setpoint->attitudeRate.yaw = degrees(yawrate_current); return true; } @@ -859,6 +862,8 @@ PARAM_ADD_CORE(PARAM_FLOAT, vtoff, &defaultTakeoffVelocity) PARAM_ADD_CORE(PARAM_FLOAT, vland, &defaultLandingVelocity) -PARAM_ADD(PARAM_FLOAT, yawrate, &yawrate) +PARAM_ADD(PARAM_FLOAT, yawrate, &yawrate_desired) +PARAM_ADD(PARAM_FLOAT, yawacc, &yawacc_limit) +PARAM_ADD(PARAM_FLOAT, yawrlim, &yawrate_limit) PARAM_GROUP_STOP(hlCommander) diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c index a78de944f8..20fdd1f563 100644 --- a/src/modules/src/crtp_localization_service.c +++ b/src/modules/src/crtp_localization_service.c @@ -120,7 +120,8 @@ static CRTPPacket LhAngle; #endif static bool enableLighthouseAngleStream = false; static float extPosStdDev = 0.01; -static float extQuatStdDev = 4.5e-3; +static float extRollPitchStdDev = 4.5e-3; +static float extYawStdDev = 4.5e-3; static bool isInit = false; static bool isTimeSynced = false; static uint8_t my_id; @@ -218,7 +219,8 @@ static void extPoseHandler(const CRTPPacket* pk) { ext_pose.quat.z = data->qz; ext_pose.quat.w = data->qw; ext_pose.stdDevPos = extPosStdDev; - ext_pose.stdDevQuat = extQuatStdDev; + ext_pose.stdDevRollPitch = extRollPitchStdDev; + ext_pose.stdDevYaw = extYawStdDev; estimatorEnqueuePose(&ext_pose); tickOfLastPacket = xTaskGetTickCount(); @@ -236,7 +238,8 @@ static void extPosePackedHandler(const CRTPPacket* pk) { ext_pose.z = item->z / 1000.0f; quatdecompress(item->quat, (float *)&ext_pose.quat.q0); ext_pose.stdDevPos = extPosStdDev; - ext_pose.stdDevQuat = extQuatStdDev; + ext_pose.stdDevRollPitch = extRollPitchStdDev; + ext_pose.stdDevYaw = extYawStdDev; estimatorEnqueuePose(&ext_pose); tickOfLastPacket = xTaskGetTickCount(); } else { @@ -489,5 +492,9 @@ PARAM_GROUP_START(locSrv) /** * @brief Standard deviation of the quarternion data to kalman filter */ - PARAM_ADD_CORE(PARAM_FLOAT, extQuatStdDev, &extQuatStdDev) + PARAM_ADD_CORE(PARAM_FLOAT, extRPStdDev, &extRollPitchStdDev) + /** + * @brief Standard deviation of the quarternion data to kalman filter + */ + PARAM_ADD_CORE(PARAM_FLOAT, extYStdDev, &extYawStdDev) PARAM_GROUP_STOP(locSrv) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 66953906a7..14e623cf11 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -15,6 +15,7 @@ static uint8_t aeg; static uint8_t aGain; static uint8_t dGain; static uint16_t exposure; +static uint8_t num_consecutive_images = 3; static uint8_t trigger = 0; const uint32_t baudrate_esp32 = 115200; @@ -41,6 +42,7 @@ typedef struct uint8_t aGain; uint8_t dGain; uint16_t exposure; + uint8_t num_consecutive_images; } __attribute__((packed)) RegisterPacket_t; void appMain() @@ -120,6 +122,7 @@ void appMain() reg_packet->aGain = aGain; reg_packet->dGain = dGain; reg_packet->exposure = exposure; + reg_packet->num_consecutive_images = num_consecutive_images; cpxSendPacketBlocking(&cpx_packet); trigger = 0; @@ -145,6 +148,12 @@ PARAM_ADD(PARAM_UINT8, dGain, &dGain) * @brief Exposure 2 to (frame_length_lines - 2) [If AEG is disabled] */ PARAM_ADD(PARAM_UINT16, exposure, &exposure) + +/** + * @brief Number of consecutive images to record (1-3) + */ +PARAM_ADD(PARAM_UINT8, num_img, &num_consecutive_images) + /** * @brief Triggers an update of all settings/registers on the GAP8 */ diff --git a/src/modules/src/kalman_core/mm_pose.c b/src/modules/src/kalman_core/mm_pose.c index 4f1fc7fbb6..48b509fd42 100644 --- a/src/modules/src/kalman_core/mm_pose.c +++ b/src/modules/src/kalman_core/mm_pose.c @@ -49,14 +49,14 @@ void kalmanCoreUpdateWithPose(kalmanCoreData_t* this, poseMeasurement_t *pose) float h[KC_STATE_DIM] = {0}; arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h}; h[KC_STATE_D0] = 1; - kalmanCoreScalarUpdate(this, &H, err_quat.x, pose->stdDevQuat); + kalmanCoreScalarUpdate(this, &H, err_quat.x, pose->stdDevRollPitch); h[KC_STATE_D0] = 0; h[KC_STATE_D1] = 1; - kalmanCoreScalarUpdate(this, &H, err_quat.y, pose->stdDevQuat); + kalmanCoreScalarUpdate(this, &H, err_quat.y, pose->stdDevRollPitch); h[KC_STATE_D1] = 0; h[KC_STATE_D2] = 1; - kalmanCoreScalarUpdate(this, &H, err_quat.z, pose->stdDevQuat); + kalmanCoreScalarUpdate(this, &H, err_quat.z, pose->stdDevYaw); } } From 1c88053e32fa463d02ec22196e54e06ae558d7f8 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 27 Feb 2023 21:43:24 +0100 Subject: [PATCH 85/85] jpeg stream mode --- src/modules/src/cvmrs.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c index 14e623cf11..59e0137c37 100644 --- a/src/modules/src/cvmrs.c +++ b/src/modules/src/cvmrs.c @@ -16,6 +16,7 @@ static uint8_t aGain; static uint8_t dGain; static uint16_t exposure; static uint8_t num_consecutive_images = 3; +static uint8_t stream_mode = 0; static uint8_t trigger = 0; const uint32_t baudrate_esp32 = 115200; @@ -43,6 +44,7 @@ typedef struct uint8_t dGain; uint16_t exposure; uint8_t num_consecutive_images; + uint8_t stream_mode; } __attribute__((packed)) RegisterPacket_t; void appMain() @@ -123,6 +125,7 @@ void appMain() reg_packet->dGain = dGain; reg_packet->exposure = exposure; reg_packet->num_consecutive_images = num_consecutive_images; + reg_packet->stream_mode = stream_mode; cpxSendPacketBlocking(&cpx_packet); trigger = 0; @@ -154,6 +157,11 @@ PARAM_ADD(PARAM_UINT16, exposure, &exposure) */ PARAM_ADD(PARAM_UINT8, num_img, &num_consecutive_images) +/** + * @brief Streaming mode (0: raw; 1: jpeg) + */ +PARAM_ADD(PARAM_UINT8, stream_mode, &stream_mode) + /** * @brief Triggers an update of all settings/registers on the GAP8 */