diff --git a/.travis.yml b/.travis.yml index 5072324926..df472a95ec 100644 --- a/.travis.yml +++ b/.travis.yml @@ -82,7 +82,7 @@ install: - make arm_sdk_install before_script: - - tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version + - tools/gcc-arm-none-eabi-6_2-2016q4/bin/arm-none-eabi-gcc --version - clang --version - clang++ --version diff --git a/Makefile b/Makefile index 340dbcacb8..8b8183ba4a 100644 --- a/Makefile +++ b/Makefile @@ -561,6 +561,7 @@ COMMON_SRC = \ config/config_eeprom.c \ config/feature.c \ config/parameter_group.c \ + config/config_streamer.c \ drivers/adc.c \ drivers/buf_writer.c \ drivers/bus_i2c_soft.c \ @@ -593,6 +594,7 @@ COMMON_SRC = \ fc/fc_rc.c \ fc/fc_msp.c \ fc/fc_tasks.c \ + fc/rc_adjustments.c \ fc/rc_controls.c \ fc/runtime_config.c \ fc/cli.c \ @@ -648,6 +650,7 @@ HIGHEND_SRC = \ cms/cms_menu_osd.c \ cms/cms_menu_vtx.c \ common/colorconversion.c \ + common/gps_conversion.c \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ drivers/serial_escserial.c \ @@ -655,7 +658,6 @@ HIGHEND_SRC = \ drivers/sonar_hcsr04.c \ drivers/vtx_common.c \ flight/navigation.c \ - flight/gps_conversion.c \ io/dashboard.c \ io/displayport_max7456.c \ io/displayport_msp.c \ @@ -707,25 +709,20 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/serial.c \ drivers/serial_uart.c \ drivers/sound_beeper.c \ - drivers/stack_check.c \ drivers/system.c \ drivers/timer.c \ + fc/fc_core.c \ fc/fc_tasks.c \ fc/fc_rc.c \ fc/rc_controls.c \ fc/runtime_config.c \ - flight/altitudehold.c \ - flight/failsafe.c \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ flight/servos.c \ - io/beeper.c \ io/serial.c \ - io/statusindicator.c \ rx/ibus.c \ rx/jetiexbus.c \ - rx/msp.c \ rx/nrf24_cx10.c \ rx/nrf24_inav.c \ rx/nrf24_h8_3d.c \ @@ -746,25 +743,12 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ sensors/gyro.c \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ drivers/serial_softserial.c \ io/dashboard.c \ io/displayport_max7456.c \ - io/displayport_msp.c \ - io/displayport_oled.c \ - io/ledstrip.c \ io/osd.c \ - telemetry/telemetry.c \ - telemetry/crsf.c \ - telemetry/frsky.c \ - telemetry/hott.c \ - telemetry/smartport.c \ - telemetry/ltm.c \ - telemetry/mavlink.c \ - telemetry/esc_telemetry.c \ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/serial_escserial.c \ diff --git a/make/tools.mk b/make/tools.mk index 810716a57d..4ad98115d6 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -14,16 +14,16 @@ ############################## # Set up ARM (STM32) SDK -ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q3 +ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-6_2-2016q4 # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion) -GCC_REQUIRED_VERSION ?= 5.4.1 +GCC_REQUIRED_VERSION ?= 6.2.1 ## arm_sdk_install : Install Arm SDK .PHONY: arm_sdk_install -ARM_SDK_URL_BASE := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update/+download/gcc-arm-none-eabi-5_4-2016q3-20160926 +ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2016q4/gcc-arm-none-eabi-6_2-2016q4-20161216 -# source: https://launchpad.net/gcc-arm-embedded/5.0/ +# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads ifdef LINUX ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2 endif @@ -33,7 +33,7 @@ ifdef MACOSX endif ifdef WINDOWS - ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32-zip.zip endif ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 49ceee0848..eb9aee930d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -24,6 +24,9 @@ #ifdef BLACKBOX +#include "blackbox.h" +#include "blackbox_io.h" + #include "build/debug.h" #include "build/version.h" @@ -31,8 +34,10 @@ #include "common/encoding.h" #include "common/utils.h" -#include "blackbox.h" -#include "blackbox_io.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/sensor.h" #include "drivers/compass.h" @@ -43,18 +48,22 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "flight/failsafe.h" #include "flight/pid.h" #include "io/beeper.h" +#include "io/serial.h" -#include "sensors/sensors.h" +#include "rx/rx.h" + +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" #include "sensors/compass.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" #include "sensors/sonar.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #define BLACKBOX_I_INTERVAL 32 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #define SLOW_FRAME_INTERVAL 4096 @@ -232,21 +241,21 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { typedef enum BlackboxState { BLACKBOX_STATE_DISABLED = 0, - BLACKBOX_STATE_STOPPED, - BLACKBOX_STATE_PREPARE_LOG_FILE, - BLACKBOX_STATE_SEND_HEADER, - BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, - BLACKBOX_STATE_SEND_GPS_H_HEADER, - BLACKBOX_STATE_SEND_GPS_G_HEADER, - BLACKBOX_STATE_SEND_SLOW_HEADER, - BLACKBOX_STATE_SEND_SYSINFO, - BLACKBOX_STATE_PAUSED, - BLACKBOX_STATE_RUNNING, - BLACKBOX_STATE_SHUTTING_DOWN + BLACKBOX_STATE_STOPPED, //1 + BLACKBOX_STATE_PREPARE_LOG_FILE, //2 + BLACKBOX_STATE_SEND_HEADER, //3 + BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4 + BLACKBOX_STATE_SEND_GPS_H_HEADER, //5 + BLACKBOX_STATE_SEND_GPS_G_HEADER, //6 + BLACKBOX_STATE_SEND_SLOW_HEADER, //7 + BLACKBOX_STATE_SEND_SYSINFO, //8 + BLACKBOX_STATE_PAUSED, //9 + BLACKBOX_STATE_RUNNING, //10 + BLACKBOX_STATE_SHUTTING_DOWN, //11 + BLACKBOX_STATE_START_ERASE, //12 + BLACKBOX_STATE_ERASING, //13 } BlackboxState; -#define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER -#define BLACKBOX_LAST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_SYSINFO typedef struct blackboxMainState_s { uint32_t time; @@ -761,16 +770,16 @@ void validateBlackboxConfig() if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0 || blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) { - blackboxConfig()->rate_num = 1; - blackboxConfig()->rate_denom = 1; + blackboxConfigMutable()->rate_num = 1; + blackboxConfigMutable()->rate_denom = 1; } else { /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat * itself more frequently) */ div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom); - blackboxConfig()->rate_num /= div; - blackboxConfig()->rate_denom /= div; + blackboxConfigMutable()->rate_num /= div; + blackboxConfigMutable()->rate_denom /= div; } // If we've chosen an unsupported device, change the device to serial @@ -786,7 +795,7 @@ void validateBlackboxConfig() break; default: - blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL; + blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL; } } @@ -795,47 +804,44 @@ void validateBlackboxConfig() */ void startBlackbox(void) { - if (blackboxState == BLACKBOX_STATE_STOPPED) { - validateBlackboxConfig(); + validateBlackboxConfig(); - if (!blackboxDeviceOpen()) { - blackboxSetState(BLACKBOX_STATE_DISABLED); - return; - } - - memset(&gpsHistory, 0, sizeof(gpsHistory)); - - blackboxHistory[0] = &blackboxHistoryRing[0]; - blackboxHistory[1] = &blackboxHistoryRing[1]; - blackboxHistory[2] = &blackboxHistoryRing[2]; - - vbatReference = vbatLatest; - - //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it - - /* - * We use conditional tests to decide whether or not certain fields should be logged. Since our headers - * must always agree with the logged data, the results of these tests must not change during logging. So - * cache those now. - */ - blackboxBuildConditionCache(); - - blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationProfile()->modeActivationConditions, BOXBLACKBOX); - - blackboxIteration = 0; - blackboxPFrameIndex = 0; - blackboxIFrameIndex = 0; - - /* - * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when - * it finally plays the beep for this arming event. - */ - blackboxLastArmingBeep = getArmingBeepTimeMicros(); - blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status - - - blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); + if (!blackboxDeviceOpen()) { + blackboxSetState(BLACKBOX_STATE_DISABLED); + return; } + + memset(&gpsHistory, 0, sizeof(gpsHistory)); + + blackboxHistory[0] = &blackboxHistoryRing[0]; + blackboxHistory[1] = &blackboxHistoryRing[1]; + blackboxHistory[2] = &blackboxHistoryRing[2]; + + vbatReference = vbatLatest; + + //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it + + /* + * We use conditional tests to decide whether or not certain fields should be logged. Since our headers + * must always agree with the logged data, the results of these tests must not change during logging. So + * cache those now. + */ + blackboxBuildConditionCache(); + + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); + + blackboxIteration = 0; + blackboxPFrameIndex = 0; + blackboxIFrameIndex = 0; + + /* + * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when + * it finally plays the beep for this arming event. + */ + blackboxLastArmingBeep = getArmingBeepTimeMicros(); + blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status + + blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); } /** @@ -1460,18 +1466,26 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) void handleBlackbox(timeUs_t currentTimeUs) { int i; - - if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) { - blackboxReplenishHeaderBudget(); - } + static bool erasedOnce = false; //Only allow one erase per FC reboot. switch (blackboxState) { + case BLACKBOX_STATE_STOPPED: + if (ARMING_FLAG(ARMED)) { + blackboxOpen(); + startBlackbox(); + } + if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE) && !erasedOnce) { + blackboxSetState(BLACKBOX_STATE_START_ERASE); + erasedOnce = true; + } + break; case BLACKBOX_STATE_PREPARE_LOG_FILE: if (blackboxDeviceBeginLog()) { blackboxSetState(BLACKBOX_STATE_SEND_HEADER); } break; case BLACKBOX_STATE_SEND_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised /* @@ -1492,6 +1506,7 @@ void handleBlackbox(timeUs_t currentTimeUs) } break; case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { @@ -1505,6 +1520,7 @@ void handleBlackbox(timeUs_t currentTimeUs) break; #ifdef GPS case BLACKBOX_STATE_SEND_GPS_H_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAY_LENGTH(blackboxGpsHFields), NULL, NULL)) { @@ -1512,6 +1528,7 @@ void handleBlackbox(timeUs_t currentTimeUs) } break; case BLACKBOX_STATE_SEND_GPS_G_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAY_LENGTH(blackboxGpsGFields), &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) { @@ -1520,6 +1537,7 @@ void handleBlackbox(timeUs_t currentTimeUs) break; #endif case BLACKBOX_STATE_SEND_SLOW_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAY_LENGTH(blackboxSlowFields), NULL, NULL)) { @@ -1527,6 +1545,7 @@ void handleBlackbox(timeUs_t currentTimeUs) } break; case BLACKBOX_STATE_SEND_SYSINFO: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 //Keep writing chunks of the system info headers until it returns true to signal completion @@ -1556,7 +1575,6 @@ void handleBlackbox(timeUs_t currentTimeUs) blackboxLogIteration(currentTimeUs); } - // Keep the logging timers ticking so our log iteration continues to advance blackboxAdvanceIterationTimers(); break; @@ -1568,7 +1586,6 @@ void handleBlackbox(timeUs_t currentTimeUs) } else { blackboxLogIteration(currentTimeUs); } - blackboxAdvanceIterationTimers(); break; case BLACKBOX_STATE_SHUTTING_DOWN: @@ -1585,15 +1602,29 @@ void handleBlackbox(timeUs_t currentTimeUs) blackboxSetState(BLACKBOX_STATE_STOPPED); } break; + case BLACKBOX_STATE_START_ERASE: + blackboxEraseAll(); + blackboxSetState(BLACKBOX_STATE_ERASING); + beeper(BEEPER_BLACKBOX_ERASE); + break; + case BLACKBOX_STATE_ERASING: + if (isBlackboxErased()) { + //Done eraseing + blackboxSetState(BLACKBOX_STATE_STOPPED); + beeper(BEEPER_BLACKBOX_ERASE); + } + default: break; } // Did we run out of room on the device? Stop! if (isBlackboxDeviceFull()) { - blackboxSetState(BLACKBOX_STATE_STOPPED); - // ensure we reset the test mode flag if we stop due to full memory card - if (startedLoggingInTestMode) startedLoggingInTestMode = false; + if (blackboxState != BLACKBOX_STATE_ERASING && blackboxState != BLACKBOX_STATE_START_ERASE) { + blackboxSetState(BLACKBOX_STATE_STOPPED); + // ensure we reset the test mode flag if we stop due to full memory card + if (startedLoggingInTestMode) startedLoggingInTestMode = false; + } } else { // Only log in test mode if there is room! if(blackboxConfig()->on_motor_test) { diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 388ca2f75d..d085f531c4 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -21,6 +21,8 @@ #include "common/time.h" +#include "config/parameter_group.h" + typedef struct blackboxConfig_s { uint8_t rate_num; uint8_t rate_denom; @@ -28,6 +30,8 @@ typedef struct blackboxConfig_s { uint8_t on_motor_test; } blackboxConfig_t; +PG_DECLARE(blackboxConfig_t, blackboxConfig); + void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); void initBlackbox(void); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 307ada96c1..f33e738160 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -8,6 +8,7 @@ #ifdef BLACKBOX +#include "blackbox.h" #include "blackbox_io.h" #include "build/version.h" @@ -16,6 +17,10 @@ #include "common/encoding.h" #include "common/printf.h" +#include "config/config_profile.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/config.h" #include "fc/rc_controls.h" @@ -23,12 +28,10 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" +#include "io/serial.h" #include "msp/msp_serial.h" -#include "config/config_profile.h" -#include "config/config_master.h" - #define BLACKBOX_SERIAL_PORT_MODE MODE_TX // How many bytes can we transmit per loop iteration when writing headers? @@ -63,6 +66,14 @@ static struct { #endif +void blackboxOpen() +{ + serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); + if (sharedBlackboxAndMspPort) { + mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); + } +} + void blackboxWrite(uint8_t value) { switch (blackboxConfig()->device) { @@ -599,6 +610,43 @@ bool blackboxDeviceOpen(void) } } +/** + * Erase all blackbox logs + */ +void blackboxEraseAll(void) +{ + switch (blackboxConfig()->device) { +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + flashfsEraseCompletely(); + break; +#endif + default: + //not supported + break; + + } +} + +/** + * Check to see if erasing is done + */ +bool isBlackboxErased(void) +{ + switch (blackboxConfig()->device) { +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + return flashfsIsReady(); + break; +#endif + default: + //not supported + return true; + break; + + } +} + /** * Close the Blackbox logging device immediately without attempting to flush any remaining data. */ diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 8a8087f180..9dc7b7566b 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -49,6 +49,7 @@ typedef enum { extern int32_t blackboxHeaderBudget; +void blackboxOpen(void); void blackboxWrite(uint8_t value); int blackboxPrintf(const char *fmt, ...); @@ -71,6 +72,9 @@ bool blackboxDeviceFlushForce(void); bool blackboxDeviceOpen(void); void blackboxDeviceClose(void); +void blackboxEraseAll(void); +bool isBlackboxErased(void); + bool blackboxDeviceBeginLog(void); bool blackboxDeviceEndLog(bool retainLog); diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 4603091ed4..0508f77e4f 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -69,25 +69,25 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) #define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \ __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 ) -// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier. -// Be carefull when using this, you must use some method to prevent optimizer form breaking things -// - lto is used for baseflight compillation, so function call is not memory barrier -// - use ATOMIC_BARRIER or propper volatile to protect used variables -// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much -// but that can change in future versions +// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier. +// Be careful when using this, you must use some method to prevent optimizer form breaking things +// - lto is used for Cleanflight compilation, so function call is not memory barrier +// - use ATOMIC_BARRIER or volatile to protect used variables +// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much +// - gcc 5 and later works as intended, generating quite optimal code #define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \ __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \ // ATOMIC_BARRIER // Create memory barrier -// - at the beginning (all data must be reread from memory) -// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use) -// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier +// - at the beginning of containing block (value of parameter must be reread from memory) +// - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use) +// On gcc 5 and higher, this protects only memory passed as parameter (any type should work) // this macro can be used only ONCE PER LINE, but multiple uses per block are fine -#if (__GNUC__ > 5) +#if (__GNUC__ > 6) #warning "Please verify that ATOMIC_BARRIER works as intended" -// increment version number is BARRIER works +// increment version number if BARRIER works // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead // you should check that local variable scope with cleanup spans entire block #endif @@ -101,10 +101,10 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) // this macro uses local function for cleanup. CLang block can be substituded #define ATOMIC_BARRIER(data) \ __extension__ void __UNIQL(__barrierEnd)(typeof(data) **__d) { \ - __asm__ volatile ("\t# barier(" #data ") end\n" : : "m" (**__d)); \ + __asm__ volatile ("\t# barrier(" #data ") end\n" : : "m" (**__d)); \ } \ typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \ - __asm__ volatile ("\t# barier (" #data ") start\n" : "=m" (*__UNIQL(__barrier))) + __asm__ volatile ("\t# barrier (" #data ") start\n" : "+m" (*__UNIQL(__barrier))) // define these wrappers for atomic operations, use gcc buildins diff --git a/src/main/build/version.h b/src/main/build/version.h index 8069198724..4770ad98e9 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -17,8 +17,8 @@ #define FC_FIRMWARE_NAME "Betaflight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 5 // increment when a bug is fixed +#define FC_VERSION_MINOR 2 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 33b01b35d1..ae791cd1fc 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -44,19 +44,24 @@ #include "drivers/system.h" +// For rcData, stopAllMotors, stopPwmAllMotors +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + // For 'ARM' related #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -// For rcData, stopAllMotors, stopPwmAllMotors -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" +#include "flight/mixer.h" -// For VISIBLE* (Actually, included by config_master.h) +// For VISIBLE* #include "io/osd.h" +#include "rx/rx.h" + // DisplayPort management #ifndef CMS_MAX_DEVICE diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index d1ea564a88..f8b2372316 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -27,10 +27,10 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" #include "cms/cms.h" @@ -40,11 +40,13 @@ #include "common/utils.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" +#include "io/beeper.h" #include "blackbox/blackbox_io.h" @@ -62,6 +64,7 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) delay(100); } + beeper(BEEPER_BLACKBOX_ERASE); displayClearScreen(pDisplay); displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 26cce45584..7dc533ddd8 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -44,8 +44,9 @@ #include "flight/pid.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" // // PID @@ -104,10 +105,11 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void static long cmsx_PidRead(void) { + const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; for (uint8_t i = 0; i < 3; i++) { - tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i]; - tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i]; - tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i]; + tempPid[i][0] = pidProfile->P8[i]; + tempPid[i][1] = pidProfile->I8[i]; + tempPid[i][2] = pidProfile->D8[i]; } return 0; @@ -125,10 +127,11 @@ static long cmsx_PidWriteback(const OSD_Entry *self) { UNUSED(self); + pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; for (uint8_t i = 0; i < 3; i++) { - masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0]; - masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1]; - masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2]; + pidProfile->P8[i] = tempPid[i][0]; + pidProfile->I8[i] = tempPid[i][1]; + pidProfile->D8[i] = tempPid[i][2]; } pidInitConfig(¤tProfile->pidProfile); @@ -233,12 +236,13 @@ static long cmsx_profileOtherOnEnter(void) { profileIndexString[1] = '0' + tmpProfileIndex; - cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight; - cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio; + const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight; + cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio; - cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL]; - cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL]; - cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL]; + cmsx_angleStrength = pidProfile->P8[PIDLEVEL]; + cmsx_horizonStrength = pidProfile->I8[PIDLEVEL]; + cmsx_horizonTransition = pidProfile->D8[PIDLEVEL]; return 0; } @@ -247,13 +251,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) { UNUSED(self); - masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight; - masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio; + pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight; + pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio; pidInitConfig(¤tProfile->pidProfile); - masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength; - masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength; - masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition; + pidProfile->P8[PIDLEVEL] = cmsx_angleStrength; + pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength; + pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition; return 0; } @@ -311,11 +316,12 @@ static uint16_t cmsx_yaw_p_limit; static long cmsx_FilterPerProfileRead(void) { - cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz; - cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz; - cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff; - cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz; - cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit; + const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz; + cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; + cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff; + cmsx_yaw_lpf_hz = pidProfile->yaw_lpf_hz; + cmsx_yaw_p_limit = pidProfile->yaw_p_limit; return 0; } @@ -324,11 +330,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) { UNUSED(self); - masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz; - masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz; - masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff; - masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz; - masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit; + pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz; + pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; + pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff; + pidProfile->yaw_lpf_hz = cmsx_yaw_lpf_hz; + pidProfile->yaw_p_limit = cmsx_yaw_p_limit; return 0; } diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index 45f9863f8c..e92f42c99a 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -22,15 +22,16 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "cms/cms.h" #include "cms/cms_types.h" diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 38d1e07160..05e1e54905 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -28,14 +28,15 @@ #include "drivers/system.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #include "cms/cms.h" #include "cms/cms_types.h" #include "cms/cms_menu_ledstrip.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + // // Misc // diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 2d8b88f0ac..28be0d4743 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -21,6 +21,8 @@ #include "platform.h" +#if defined(OSD) && defined(CMS) + #include "build/version.h" #include "cms/cms.h" @@ -28,10 +30,9 @@ #include "cms/cms_menu_osd.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" - -#if defined(OSD) && defined(CMS) +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5}; OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50}; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 066fe8eeca..a07a776b1a 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -21,6 +21,10 @@ #include "platform.h" +#ifdef CMS + +#if defined(VTX) || defined(USE_RTC6705) + #include "build/version.h" #include "cms/cms.h" @@ -30,12 +34,9 @@ #include "common/utils.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" - -#ifdef CMS - -#if defined(VTX) || defined(USE_RTC6705) +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" static bool featureRead = false; static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; diff --git a/src/main/flight/gps_conversion.c b/src/main/common/gps_conversion.c similarity index 100% rename from src/main/flight/gps_conversion.c rename to src/main/common/gps_conversion.c diff --git a/src/main/flight/gps_conversion.h b/src/main/common/gps_conversion.h similarity index 100% rename from src/main/flight/gps_conversion.h rename to src/main/common/gps_conversion.h diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 78fd819d77..5283d6eba4 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -30,6 +30,10 @@ #define M_PIf 3.14159265358979323846f #define RAD (M_PIf / 180.0f) +#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) +#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) +#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) +#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f) #define MIN(a, b) ((a) < (b) ? (a) : (b)) #define MAX(a, b) ((a) > (b) ? (a) : (b)) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 5c32f18183..295cd77b97 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -18,294 +18,285 @@ #include #include #include +#include #include "platform.h" -#include "drivers/system.h" - -#include "config/config_master.h" - #include "build/build_config.h" +#include "common/maths.h" + #include "config/config_eeprom.h" +#include "config/config_streamer.h" +#include "config/config_master.h" +#include "config/parameter_group.h" -#if !defined(FLASH_SIZE) -#error "Flash size not defined for target. (specify in KB)" -#endif +#include "drivers/system.h" +#include "fc/config.h" -#ifndef FLASH_PAGE_SIZE - #ifdef STM32F303xC - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif +// declare a dummy PG, since scanEEPROM assumes there is at least one PG +// !!TODO remove once first PG has been created out of masterConfg +typedef struct dummpConfig_s { + uint8_t dummy; +} dummyConfig_t; +PG_DECLARE(dummyConfig_t, dummyConfig); +#define PG_DUMMY_CONFIG 1 +PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0); - #ifdef STM32F10X_MD - #define FLASH_PAGE_SIZE ((uint16_t)0x400) - #endif +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; - #ifdef STM32F10X_HD - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif +static uint16_t eepromConfigSize; - #if defined(STM32F745xx) - #define FLASH_PAGE_SIZE ((uint32_t)0x40000) - #endif +typedef enum { + CR_CLASSICATION_SYSTEM = 0, + CR_CLASSICATION_PROFILE1 = 1, + CR_CLASSICATION_PROFILE2 = 2, + CR_CLASSICATION_PROFILE3 = 3, + CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_PROFILE3, +} configRecordFlags_e; - #if defined(STM32F746xx) - #define FLASH_PAGE_SIZE ((uint32_t)0x40000) - #endif +#define CR_CLASSIFICATION_MASK (0x3) - #if defined(STM32F722xx) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif +// Header for the saved copy. +typedef struct { + uint8_t format; +} PG_PACKED configHeader_t; - #if defined(STM32F40_41xxx) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif +// Header for each stored PG. +typedef struct { + // split up. + uint16_t size; + pgn_t pgn; + uint8_t version; - #if defined (STM32F411xE) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif + // lower 2 bits used to indicate system or profile number, see CR_CLASSIFICATION_MASK + uint8_t flags; -#endif + uint8_t pg[]; +} PG_PACKED configRecord_t; -#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT) - #ifdef STM32F10X_MD - #define FLASH_PAGE_COUNT 128 - #endif - - #ifdef STM32F10X_HD - #define FLASH_PAGE_COUNT 128 - #endif -#endif - -#if defined(FLASH_SIZE) -#if defined(STM32F40_41xxx) -#define FLASH_PAGE_COUNT 4 -#elif defined (STM32F411xE) -#define FLASH_PAGE_COUNT 3 -#elif defined (STM32F722xx) -#define FLASH_PAGE_COUNT 3 -#elif defined (STM32F745xx) -#define FLASH_PAGE_COUNT 4 -#elif defined (STM32F746xx) -#define FLASH_PAGE_COUNT 4 -#else -#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE) -#endif -#endif - -#if !defined(FLASH_PAGE_SIZE) -#error "Flash page size not defined for target." -#endif - -#if !defined(FLASH_PAGE_COUNT) -#error "Flash page count not defined for target." -#endif - -#if FLASH_SIZE <= 128 -#define FLASH_TO_RESERVE_FOR_CONFIG 0x800 -#else -#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000 -#endif - -// use the last flash pages for storage -#ifdef CUSTOM_FLASH_MEMORY_ADDRESS -size_t custom_flash_memory_address = 0; -#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) -#else -// use the last flash pages for storage -#ifndef CONFIG_START_FLASH_ADDRESS -#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) -#endif -#endif +// Footer for the saved copy. +typedef struct { + uint16_t terminator; +} PG_PACKED configFooter_t; +// checksum is appended just after footer. It is not included in footer to make checksum calculation consistent +// Used to check the compiler packing at build time. +typedef struct { + uint8_t byte; + uint32_t word; +} PG_PACKED packingTest_t; void initEEPROM(void) { + // Verify that this architecture packs as expected. + BUILD_BUG_ON(offsetof(packingTest_t, byte) != 0); + BUILD_BUG_ON(offsetof(packingTest_t, word) != 1); + BUILD_BUG_ON(sizeof(packingTest_t) != 5); + + BUILD_BUG_ON(sizeof(configHeader_t) != 1); + BUILD_BUG_ON(sizeof(configFooter_t) != 2); + BUILD_BUG_ON(sizeof(configRecord_t) != 6); } -static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) +static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length) { - uint8_t checksum = 0; - const uint8_t *byteOffset; + const uint8_t *p = (const uint8_t *)data; + const uint8_t *pend = p + length; - for (byteOffset = data; byteOffset < (data + length); byteOffset++) - checksum ^= *byteOffset; - return checksum; + for (; p != pend; p++) { + chk ^= *p; + } + return chk; } +// Scan the EEPROM config. Returns true if the config is valid. bool isEEPROMContentValid(void) { - const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; - uint8_t checksum = 0; + uint8_t chk = 0; + const uint8_t *p = &__config_start; + const configHeader_t *header = (const configHeader_t *)p; - // check version number - if (EEPROM_CONF_VERSION != temp->version) + if (header->format != EEPROM_CONF_VERSION) { return false; + } + chk = updateChecksum(chk, header, sizeof(*header)); + p += sizeof(*header); + // include the transitional masterConfig record + chk = updateChecksum(chk, p, sizeof(masterConfig)); + p += sizeof(masterConfig); - // check size and magic numbers - if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) - return false; + for (;;) { + const configRecord_t *record = (const configRecord_t *)p; - if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) - return false; + if (record->size == 0) { + // Found the end. Stop scanning. + break; + } + if (p + record->size >= &__config_end + || record->size < sizeof(*record)) { + // Too big or too small. + return false; + } - // verify integrity of temporary copy - checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t)); - if (checksum != 0) - return false; + chk = updateChecksum(chk, p, record->size); - // looks good, let's roll! + p += record->size; + } + + const configFooter_t *footer = (const configFooter_t *)p; + chk = updateChecksum(chk, footer, sizeof(*footer)); + p += sizeof(*footer); + chk = ~chk; + const uint8_t checkSum = *p; + p += sizeof(checkSum); + eepromConfigSize = p - &__config_start; + return chk == checkSum; +} + +uint16_t getEEPROMConfigSize(void) +{ + return eepromConfigSize; +} + +// find config record for reg + classification (profile info) in EEPROM +// return NULL when record is not found +// this function assumes that EEPROM content is valid +static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFlags_e classification) +{ + const uint8_t *p = &__config_start; + p += sizeof(configHeader_t); // skip header + p += sizeof(master_t); // skip the transitional master_t record + while (true) { + const configRecord_t *record = (const configRecord_t *)p; + if (record->size == 0 + || p + record->size >= &__config_end + || record->size < sizeof(*record)) + break; + if (pgN(reg) == record->pgn + && (record->flags & CR_CLASSIFICATION_MASK) == classification) + return record; + p += record->size; + } + // record not found + return NULL; +} + +// Initialize all PG records from EEPROM. +// This functions processes all PGs sequentially, scanning EEPROM for each one. This is suboptimal, +// but each PG is loaded/initialized exactly once and in defined order. +bool loadEEPROM(void) +{ + // read in the transitional masterConfig record + const uint8_t *p = &__config_start; + p += sizeof(configHeader_t); // skip header + masterConfig = *(master_t*)p; + + PG_FOREACH(reg) { + configRecordFlags_e cls_start, cls_end; + if (pgIsSystem(reg)) { + cls_start = CR_CLASSICATION_SYSTEM; + cls_end = CR_CLASSICATION_SYSTEM; + } else { + cls_start = CR_CLASSICATION_PROFILE1; + cls_end = CR_CLASSICATION_PROFILE_LAST; + } + for (configRecordFlags_e cls = cls_start; cls <= cls_end; cls++) { + int profileIndex = cls - cls_start; + const configRecord_t *rec = findEEPROM(reg, cls); + if (rec) { + // config from EEPROM is available, use it to initialize PG. pgLoad will handle version mismatch + pgLoad(reg, profileIndex, rec->pg, rec->size - offsetof(configRecord_t, pg), rec->version); + } else { + pgReset(reg, profileIndex); + } + } + } return true; } -#if defined(STM32F7) - -// FIXME: HAL for now this will only work for F4/F7 as flash layout is different -void writeEEPROM(void) +static bool writeSettingsToEEPROM(void) { - // Generate compile time error if the config does not fit in the reserved area of flash. - BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); + config_streamer_t streamer; + config_streamer_init(&streamer); - HAL_StatusTypeDef status; - uint32_t wordOffset; - int8_t attemptsRemaining = 3; + config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start); + uint8_t chk = 0; - suspendRxSignal(); + configHeader_t header = { + .format = EEPROM_CONF_VERSION, + }; - // prepare checksum/version constants - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.size = sizeof(master_t); - masterConfig.magic_be = 0xBE; - masterConfig.magic_ef = 0xEF; - masterConfig.chk = 0; // erase checksum before recalculating - masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); + config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)); + chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header)); + // write the transitional masterConfig record + config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig)); + chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig)); + PG_FOREACH(reg) { + const uint16_t regSize = pgSize(reg); + configRecord_t record = { + .size = sizeof(configRecord_t) + regSize, + .pgn = pgN(reg), + .version = pgVersion(reg), + .flags = 0 + }; - // write it - /* Unlock the Flash to enable the flash control register access *************/ - HAL_FLASH_Unlock(); - while (attemptsRemaining--) - { - /* Fill EraseInit structure*/ - FLASH_EraseInitTypeDef EraseInitStruct = {0}; - EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; - EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V - EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1); - EraseInitStruct.NbSectors = 1; - uint32_t SECTORError; - status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); - if (status != HAL_OK) - { - continue; - } - else - { - for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) - { - status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset)); - if(status != HAL_OK) - { - break; - } + if (pgIsSystem(reg)) { + // write the only instance + record.flags |= CR_CLASSICATION_SYSTEM; + config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); + chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record)); + config_streamer_write(&streamer, reg->address, regSize); + chk = updateChecksum(chk, reg->address, regSize); + } else { + // write one instance for each profile + for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) { + record.flags = 0; + + record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK); + config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); + chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record)); + const uint8_t *address = reg->address + (regSize * profileIndex); + config_streamer_write(&streamer, address, regSize); + chk = updateChecksum(chk, address, regSize); } } - if (status == HAL_OK) { - break; + } + + configFooter_t footer = { + .terminator = 0, + }; + + config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer)); + chk = updateChecksum(chk, (uint8_t *)&footer, sizeof(footer)); + + // append checksum now + chk = ~chk; + config_streamer_write(&streamer, &chk, sizeof(chk)); + + config_streamer_flush(&streamer); + + bool success = config_streamer_finish(&streamer) == 0; + + return success; +} + +void writeConfigToEEPROM(void) +{ + bool success = false; + // write it + for (int attempt = 0; attempt < 3 && !success; attempt++) { + if (writeSettingsToEEPROM()) { + success = true; } } - HAL_FLASH_Lock(); + + if (success && isEEPROMContentValid()) { + return; + } // Flash write failed - just die now - if (status != HAL_OK || !isEEPROMContentValid()) { - failureMode(FAILURE_FLASH_WRITE_FAILED); - } - - resumeRxSignal(); -} -#else -void writeEEPROM(void) -{ - // Generate compile time error if the config does not fit in the reserved area of flash. - BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); - - FLASH_Status status = 0; - uint32_t wordOffset; - int8_t attemptsRemaining = 3; - - suspendRxSignal(); - - // prepare checksum/version constants - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.size = sizeof(master_t); - masterConfig.magic_be = 0xBE; - masterConfig.magic_ef = 0xEF; - masterConfig.chk = 0; // erase checksum before recalculating - masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); - - // write it - FLASH_Unlock(); - while (attemptsRemaining--) { -#if defined(STM32F4) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); -#elif defined(STM32F303) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); -#elif defined(STM32F10X) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); -#endif - for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { - if (wordOffset % FLASH_PAGE_SIZE == 0) { -#if defined(STM32F40_41xxx) - status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 -#elif defined (STM32F411xE) - status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 -#else - status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); -#endif - if (status != FLASH_COMPLETE) { - break; - } - } - - status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, - *(uint32_t *) ((char *) &masterConfig + wordOffset)); - if (status != FLASH_COMPLETE) { - break; - } - } - if (status == FLASH_COMPLETE) { - break; - } - } - FLASH_Lock(); - - // Flash write failed - just die now - if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { - failureMode(FAILURE_FLASH_WRITE_FAILED); - } - - resumeRxSignal(); -} -#endif - -void readEEPROM(void) -{ - // Sanity check - if (!isEEPROMContentValid()) - failureMode(FAILURE_INVALID_EEPROM_CONTENTS); - - suspendRxSignal(); - - // Read flash - memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); - - if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check - masterConfig.current_profile_index = 0; - - setProfile(masterConfig.current_profile_index); - - validateAndFixConfig(); - activateConfig(); - - resumeRxSignal(); + failureMode(FAILURE_FLASH_WRITE_FAILED); } diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index c87f07b4b7..640bc0299e 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,9 +17,12 @@ #pragma once +#include +#include + #define EEPROM_CONF_VERSION 156 -void initEEPROM(void); -void writeEEPROM(); -void readEEPROM(void); bool isEEPROMContentValid(void); +bool loadEEPROM(void); +void writeConfigToEEPROM(void); +uint16_t getEEPROMConfigSize(void); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index e7ae046c1e..f3ad9ac0a2 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -35,6 +35,7 @@ #include "drivers/flash.h" #include "drivers/display.h" +#include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "flight/failsafe.h" @@ -42,10 +43,10 @@ #include "flight/servos.h" #include "flight/imu.h" #include "flight/navigation.h" +#include "flight/pid.h" #include "io/serial.h" #include "io/gimbal.h" -#include "io/motors.h" #include "io/servos.h" #include "io/gps.h" #include "io/osd.h" @@ -64,6 +65,7 @@ #include "sensors/battery.h" #include "sensors/compass.h" +#ifndef USE_PARAMETER_GROUPS #define motorConfig(x) (&masterConfig.motorConfig) #define flight3DConfig(x) (&masterConfig.flight3DConfig) #define servoConfig(x) (&masterConfig.servoConfig) @@ -106,11 +108,71 @@ #define modeActivationProfile(x) (&masterConfig.modeActivationProfile) #define servoProfile(x) (&masterConfig.servoProfile) #define customMotorMixer(i) (&masterConfig.customMotorMixer[i]) -#define customServoMixer(i) (&masterConfig.customServoMixer[i]) +#define customServoMixers(i) (&masterConfig.customServoMixer[i]) #define displayPortProfileMsp(x) (&masterConfig.displayPortProfileMsp) #define displayPortProfileMax7456(x) (&masterConfig.displayPortProfileMax7456) #define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled) + +#define motorConfigMutable(x) (&masterConfig.motorConfig) +#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig) +#define servoConfigMutable(x) (&masterConfig.servoConfig) +#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig) +#define gimbalConfigMutable(x) (&masterConfig.gimbalConfig) +#define boardAlignmentMutable(x) (&masterConfig.boardAlignment) +#define imuConfigMutable(x) (&masterConfig.imuConfig) +#define gyroConfigMutable(x) (&masterConfig.gyroConfig) +#define compassConfigMutable(x) (&masterConfig.compassConfig) +#define accelerometerConfigMutable(x) (&masterConfig.accelerometerConfig) +#define barometerConfigMutable(x) (&masterConfig.barometerConfig) +#define throttleCorrectionConfigMutable(x) (&masterConfig.throttleCorrectionConfig) +#define batteryConfigMutable(x) (&masterConfig.batteryConfig) +#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig) +#define gpsProfileMutable(x) (&masterConfig.gpsProfile) +#define gpsConfigMutable(x) (&masterConfig.gpsConfig) +#define rxConfigMutable(x) (&masterConfig.rxConfig) +#define armingConfigMutable(x) (&masterConfig.armingConfig) +#define mixerConfigMutable(x) (&masterConfig.mixerConfig) +#define airplaneConfigMutable(x) (&masterConfig.airplaneConfig) +#define failsafeConfigMutable(x) (&masterConfig.failsafeConfig) +#define serialConfigMutable(x) (&masterConfig.serialConfig) +#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig) +#define ibusTelemetryConfigMutable(x) (&masterConfig.telemetryConfig) +#define ppmConfigMutable(x) (&masterConfig.ppmConfig) +#define pwmConfigMutable(x) (&masterConfig.pwmConfig) +#define adcConfigMutable(x) (&masterConfig.adcConfig) +#define beeperConfigMutable(x) (&masterConfig.beeperConfig) +#define sonarConfigMutable(x) (&masterConfig.sonarConfig) +#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig) +#define statusLedConfigMutable(x) (&masterConfig.statusLedConfig) +#define osdProfileMutable(x) (&masterConfig.osdProfile) +#define vcdProfileMutable(x) (&masterConfig.vcdProfile) +#define sdcardConfigMutable(x) (&masterConfig.sdcardConfig) +#define blackboxConfigMutable(x) (&masterConfig.blackboxConfig) +#define flashConfigMutable(x) (&masterConfig.flashConfig) +#define pidConfigMutable(x) (&masterConfig.pidConfig) +#define adjustmentProfileMutable(x) (&masterConfig.adjustmentProfile) +#define modeActivationProfileMutable(x) (&masterConfig.modeActivationProfile) +#define servoProfileMutable(x) (&masterConfig.servoProfile) +#define customMotorMixerMutable(i) (&masterConfig.customMotorMixer[i]) +#define customServoMixersMutable(i) (&masterConfig.customServoMixer[i]) +#define displayPortProfileMspMutable(x) (&masterConfig.displayPortProfileMsp) +#define displayPortProfileMax7456Mutable(x) (&masterConfig.displayPortProfileMax7456) +#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled) + +#define servoParams(x) (&servoProfile()->servoConf[i]) +#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x]) +#define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x]) +#define osdConfig(x) (&masterConfig.osdProfile) +#define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) + +#define servoParamsMutable(x) (&servoProfile()->servoConf[i]) +#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[i]) +#define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x]) +#define osdConfigMutable(x) (&masterConfig.osdProfile) +#define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) +#endif + // System-wide typedef struct master_s { uint8_t version; @@ -267,7 +329,5 @@ typedef struct master_s { } master_t; extern master_t masterConfig; -extern profile_t *currentProfile; -extern controlRateConfig_t *currentControlRateProfile; void createDefaultConfig(master_t *config); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index a0ffa53905..d1f2c513fd 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -17,7 +17,6 @@ #pragma once -#include "common/axis.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "flight/pid.h" diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h new file mode 100644 index 0000000000..5d13e12475 --- /dev/null +++ b/src/main/config/config_reset.h @@ -0,0 +1,40 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#ifndef __UNIQL +# define __UNIQL_CONCAT2(x,y) x ## y +# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y) +# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__) +#endif + +// overwrite _name with data passed as arguments. This version forces GCC to really copy data +// It is not possible to use multiple RESET_CONFIGs on single line (__UNIQL limitation) +#define RESET_CONFIG(_type, _name, ...) \ + static const _type __UNIQL(_reset_template_) = { \ + __VA_ARGS__ \ + }; \ + memcpy((_name), &__UNIQL(_reset_template_), sizeof(*(_name))); \ + /**/ + +// overwrite _name with data passed as arguments. GCC is allowed to set structure field-by-field +#define RESET_CONFIG_2(_type, _name, ...) \ + *(_name) = (_type) { \ + __VA_ARGS__ \ + }; \ + /**/ diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c new file mode 100644 index 0000000000..c4b6f6af03 --- /dev/null +++ b/src/main/config/config_streamer.c @@ -0,0 +1,263 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/system.h" + +#include "config/config_streamer.h" + +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; + +#if !defined(FLASH_PAGE_SIZE) +// F1 +# if defined(STM32F10X_MD) +# define FLASH_PAGE_SIZE (0x400) +# elif defined(STM32F10X_HD) +# define FLASH_PAGE_SIZE (0x800) +// F3 +# elif defined(STM32F303xC) +# define FLASH_PAGE_SIZE (0x800) +// F4 +# elif defined(STM32F40_41xxx) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) +# elif defined (STM32F411xE) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) +# elif defined(STM32F427_437xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) // 128K sectors +// F7 +#elif defined(STM32F722xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) +# elif defined(STM32F745xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x40000) +# elif defined(STM32F746xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x40000) +# elif defined(UNIT_TEST) +# define FLASH_PAGE_SIZE (0x400) +# else +# error "Flash page size not defined for target." +# endif +#endif + +void config_streamer_init(config_streamer_t *c) +{ + memset(c, 0, sizeof(*c)); +} + +void config_streamer_start(config_streamer_t *c, uintptr_t base, int size) +{ + // base must start at FLASH_PAGE_SIZE boundary + c->address = base; + c->size = size; + if (!c->unlocked) { +#if defined(STM32F7) + HAL_FLASH_Unlock(); +#else + FLASH_Unlock(); +#endif + c->unlocked = true; + } + +#if defined(STM32F10X) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); +#elif defined(STM32F303) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); +#elif defined(STM32F4) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); +#elif defined(STM32F7) + // NOP +#elif defined(UNIT_TEST) + // NOP +#else +# error "Unsupported CPU" +#endif + c->err = 0; +} + +#if defined(STM32F7) +/* +Sector 0 0x08000000 - 0x08007FFF 32 Kbytes +Sector 1 0x08008000 - 0x0800FFFF 32 Kbytes +Sector 2 0x08010000 - 0x08017FFF 32 Kbytes +Sector 3 0x08018000 - 0x0801FFFF 32 Kbytes +Sector 4 0x08020000 - 0x0803FFFF 128 Kbytes +Sector 5 0x08040000 - 0x0807FFFF 256 Kbytes +Sector 6 0x08080000 - 0x080BFFFF 256 Kbytes +Sector 7 0x080C0000 - 0x080FFFFF 256 Kbytes +*/ + +static uint32_t getFLASHSectorForEEPROM(void) +{ + if ((uint32_t)&__config_start <= 0x08007FFF) + return FLASH_SECTOR_0; + if ((uint32_t)&__config_start <= 0x0800FFFF) + return FLASH_SECTOR_1; + if ((uint32_t)&__config_start <= 0x08017FFF) + return FLASH_SECTOR_2; + if ((uint32_t)&__config_start <= 0x0801FFFF) + return FLASH_SECTOR_3; + if ((uint32_t)&__config_start <= 0x0803FFFF) + return FLASH_SECTOR_4; + if ((uint32_t)&__config_start <= 0x0807FFFF) + return FLASH_SECTOR_5; + if ((uint32_t)&__config_start <= 0x080BFFFF) + return FLASH_SECTOR_6; + if ((uint32_t)&__config_start <= 0x080FFFFF) + return FLASH_SECTOR_7; + + // Not good + while (1) { + failureMode(FAILURE_FLASH_WRITE_FAILED); + } +} + +#elif defined(STM32F4) +/* +Sector 0 0x08000000 - 0x08003FFF 16 Kbytes +Sector 1 0x08004000 - 0x08007FFF 16 Kbytes +Sector 2 0x08008000 - 0x0800BFFF 16 Kbytes +Sector 3 0x0800C000 - 0x0800FFFF 16 Kbytes +Sector 4 0x08010000 - 0x0801FFFF 64 Kbytes +Sector 5 0x08020000 - 0x0803FFFF 128 Kbytes +Sector 6 0x08040000 - 0x0805FFFF 128 Kbytes +Sector 7 0x08060000 - 0x0807FFFF 128 Kbytes +Sector 8 0x08080000 - 0x0809FFFF 128 Kbytes +Sector 9 0x080A0000 - 0x080BFFFF 128 Kbytes +Sector 10 0x080C0000 - 0x080DFFFF 128 Kbytes +Sector 11 0x080E0000 - 0x080FFFFF 128 Kbytes +*/ + +static uint32_t getFLASHSectorForEEPROM(void) +{ + if ((uint32_t)&__config_start <= 0x08003FFF) + return FLASH_Sector_0; + if ((uint32_t)&__config_start <= 0x08007FFF) + return FLASH_Sector_1; + if ((uint32_t)&__config_start <= 0x0800BFFF) + return FLASH_Sector_2; + if ((uint32_t)&__config_start <= 0x0800FFFF) + return FLASH_Sector_3; + if ((uint32_t)&__config_start <= 0x0801FFFF) + return FLASH_Sector_4; + if ((uint32_t)&__config_start <= 0x0803FFFF) + return FLASH_Sector_5; + if ((uint32_t)&__config_start <= 0x0805FFFF) + return FLASH_Sector_6; + if ((uint32_t)&__config_start <= 0x0807FFFF) + return FLASH_Sector_7; + if ((uint32_t)&__config_start <= 0x0809FFFF) + return FLASH_Sector_8; + if ((uint32_t)&__config_start <= 0x080DFFFF) + return FLASH_Sector_9; + if ((uint32_t)&__config_start <= 0x080BFFFF) + return FLASH_Sector_10; + if ((uint32_t)&__config_start <= 0x080FFFFF) + return FLASH_Sector_11; + + // Not good + while (1) { + failureMode(FAILURE_FLASH_WRITE_FAILED); + } +} +#endif + +static int write_word(config_streamer_t *c, uint32_t value) +{ + if (c->err != 0) { + return c->err; + } +#if defined(STM32F7) + if (c->address % FLASH_PAGE_SIZE == 0) { + FLASH_EraseInitTypeDef EraseInitStruct = { + .TypeErase = FLASH_TYPEERASE_SECTORS, + .VoltageRange = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V + .NbSectors = 1 + }; + EraseInitStruct.Sector = getFLASHSectorForEEPROM(); + uint32_t SECTORError; + const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); + if (status != HAL_OK){ + return -1; + } + } + const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value); + if (status != HAL_OK) { + return -2; + } +#else + if (c->address % FLASH_PAGE_SIZE == 0) { +#if defined(STM32F4) + const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000 +#else + const FLASH_Status status = FLASH_ErasePage(c->address); +#endif + if (status != FLASH_COMPLETE) { + return -1; + } + } + const FLASH_Status status = FLASH_ProgramWord(c->address, value); + if (status != FLASH_COMPLETE) { + return -2; + } +#endif + c->address += sizeof(value); + return 0; +} + +int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size) +{ + for (const uint8_t *pat = p; pat != (uint8_t*)p + size; pat++) { + c->buffer.b[c->at++] = *pat; + + if (c->at == sizeof(c->buffer)) { + c->err = write_word(c, c->buffer.w); + c->at = 0; + } + } + return c->err; +} + +int config_streamer_status(config_streamer_t *c) +{ + return c->err; +} + +int config_streamer_flush(config_streamer_t *c) +{ + if (c->at != 0) { + memset(c->buffer.b + c->at, 0, sizeof(c->buffer) - c->at); + c->err = write_word(c, c->buffer.w); + c->at = 0; + } + return c-> err; +} + +int config_streamer_finish(config_streamer_t *c) +{ + if (c->unlocked) { +#if defined(STM32F7) + HAL_FLASH_Lock(); +#else + FLASH_Lock(); +#endif + c->unlocked = false; + } + return c->err; +} diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h new file mode 100644 index 0000000000..a62356fbea --- /dev/null +++ b/src/main/config/config_streamer.h @@ -0,0 +1,45 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +// Streams data out to the EEPROM, padding to the write size as +// needed, and updating the checksum as it goes. + +typedef struct config_streamer_s { + uintptr_t address; + int size; + union { + uint8_t b[4]; + uint32_t w; + } buffer; + int at; + int err; + bool unlocked; +} config_streamer_t; + +void config_streamer_init(config_streamer_t *c); + +void config_streamer_start(config_streamer_t *c, uintptr_t base, int size); +int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size); +int config_streamer_flush(config_streamer_t *c); + +int config_streamer_finish(config_streamer_t *c); +int config_streamer_status(config_streamer_t *c); diff --git a/src/main/config/feature.c b/src/main/config/feature.c index 44b3c12d8b..508e5abeb1 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -21,8 +21,9 @@ #include "platform.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" static uint32_t activeFeaturesLatch = 0; diff --git a/src/main/config/feature.h b/src/main/config/feature.h index 11f6ff355f..2eb75f8a42 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -17,6 +17,14 @@ #pragma once +#include "config/parameter_group.h" + +typedef struct featureConfig_s { + uint32_t enabledFeatures; +} featureConfig_t; + +PG_DECLARE(featureConfig_t, featureConfig); + void latchActiveFeatures(void); bool featureConfigured(uint32_t mask); bool feature(uint32_t mask); @@ -27,4 +35,4 @@ uint32_t featureMask(void); void intFeatureClearAll(uint32_t *features); void intFeatureSet(uint32_t mask, uint32_t *features); -void intFeatureClear(uint32_t mask, uint32_t *features); \ No newline at end of file +void intFeatureClear(uint32_t mask, uint32_t *features); diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index 2711639cc1..58dbe51e1a 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -19,6 +19,8 @@ #include #include +#include "platform.h" + #include "parameter_group.h" #include "common/maths.h" @@ -122,4 +124,3 @@ void pgActivateProfile(int profileIndex) } } } - diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 734933ca9c..33d49c6f4f 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -20,6 +20,8 @@ #include #include +#include "build/build_config.h" + typedef uint16_t pgn_t; // parameter group registry flags @@ -54,6 +56,7 @@ static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_M static inline uint8_t pgVersion(const pgRegistry_t* reg) {return reg->pgn >> 12;} static inline uint16_t pgSize(const pgRegistry_t* reg) {return reg->size & PGR_SIZE_MASK;} static inline uint16_t pgIsSystem(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == 0;} +static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == PGR_SIZE_PROFILE_FLAG;} #define PG_PACKED __attribute__((packed)) @@ -97,28 +100,47 @@ extern const uint8_t __pg_resetdata_end[]; } while(0) \ /**/ +#ifdef USE_PARAMETER_GROUPS // Declare system config #define PG_DECLARE(_type, _name) \ extern _type _name ## _System; \ - static inline _type* _name(void) { return &_name ## _System; } \ + static inline const _type* _name(void) { return &_name ## _System; }\ + static inline _type* _name ## Mutable(void) { return &_name ## _System; }\ struct _dummy \ /**/ // Declare system config array -#define PG_DECLARE_ARR(_type, _size, _name) \ +#define PG_DECLARE_ARRAY(_type, _size, _name) \ extern _type _name ## _SystemArray[_size]; \ - static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \ - static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \ + static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \ + static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \ + static inline _type (* _name ## _array(void))[_size] { return &_name ## _SystemArray; } \ struct _dummy \ /**/ // Declare profile config #define PG_DECLARE_PROFILE(_type, _name) \ extern _type *_name ## _ProfileCurrent; \ - static inline _type* _name(void) { return _name ## _ProfileCurrent; } \ + static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \ + static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \ struct _dummy \ /**/ +#else + +#define PG_DECLARE(_type, _name) \ + extern _type _name ## _System + +#define PG_DECLARE_ARRAY(_type, _size, _name) \ + extern _type _name ## _SystemArray[_size] + +// Declare profile config +#define PG_DECLARE_PROFILE(_type, _name) \ + extern _type *_name ## _ProfileCurrent + +#endif // USE_PARAMETER_GROUPS + +#ifdef USE_PARAMETER_GROUPS // Register system config #define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ _type _name ## _System; \ @@ -148,7 +170,7 @@ extern const uint8_t __pg_resetdata_end[]; /**/ // Register system config array -#define PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, _reset) \ +#define PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, _reset) \ _type _name ## _SystemArray[_size]; \ extern const pgRegistry_t _name ##_Registry; \ const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = { \ @@ -160,20 +182,20 @@ extern const uint8_t __pg_resetdata_end[]; } \ /**/ -#define PG_REGISTER_ARR(_type, _size, _name, _pgn, _version) \ - PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \ +#define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version) \ + PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \ /**/ -#define PG_REGISTER_ARR_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \ +#define PG_REGISTER_ARRAY_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \ extern void pgResetFn_ ## _name(_type *); \ - PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ + PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ /**/ #if 0 // ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it - See pgResetInstance -#define PG_REGISTER_ARR_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \ +#define PG_REGISTER_ARRAY_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \ extern const _type pgResetTemplate_ ## _name; \ - PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ /**/ #endif @@ -221,6 +243,29 @@ extern const uint8_t __pg_resetdata_end[]; __VA_ARGS__ \ } \ /**/ +#else +#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ + _type _name ## _System + +#define PG_REGISTER(_type, _name, _pgn, _version) \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ + /**/ + +#define PG_REGISTER_WITH_RESET_FN(_type, _name, _pgn, _version) \ + extern void pgResetFn_ ## _name(_type *); \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name }) \ + /**/ + +#define PG_REGISTER_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \ + extern const _type pgResetTemplate_ ## _name; \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + /**/ +#define PG_RESET_TEMPLATE(_type, _name, ...) \ + const _type pgResetTemplate_ ## _name PG_RESETDATA_ATTRIBUTES = { \ + __VA_ARGS__ \ + } \ + /**/ +#endif const pgRegistry_t* pgFind(pgn_t pgn); diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index c42ef32f0e..44ad13b037 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -15,8 +15,12 @@ * along with Cleanflight. If not, see . */ +#ifndef USE_PARAMETER_GROUPS +#include "config/config_master.h" +#endif + // FC configuration -#define PG_FAILSAFE_CONFIG 1 // strruct OK +#define PG_FAILSAFE_CONFIG 1 // struct OK #define PG_BOARD_ALIGNMENT 2 // struct OK #define PG_GIMBAL_CONFIG 3 // struct OK #define PG_MOTOR_MIXER 4 // two structs mixerConfig_t servoMixerConfig_t diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index b452afc774..9e7c3ea98f 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -48,16 +48,7 @@ #include "accgyro_mpu.h" -mpuResetFuncPtr mpuReset; - -static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data); -static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data); - -static void mpu6050FindRevision(gyroDev_t *gyro); - -#ifdef USE_SPI -static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro); -#endif +mpuResetFnPtr mpuResetFn; #ifndef MPU_I2C_INSTANCE #define MPU_I2C_INSTANCE I2C_DEVICE @@ -71,110 +62,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro); #define MPU_INQUIRY_MASK 0x7E -mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) -{ - bool ack; - uint8_t sig; - uint8_t inquiryResult; - - // MPU datasheet specifies 30ms. - delay(35); - -#ifndef USE_I2C - ack = false; - sig = 0; -#else - ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); -#endif - if (ack) { - gyro->mpuConfiguration.read = mpuReadRegisterI2C; - gyro->mpuConfiguration.write = mpuWriteRegisterI2C; - } else { -#ifdef USE_SPI - bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); - UNUSED(detectedSpiSensor); -#endif - - return &gyro->mpuDetectionResult; - } - - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - - // If an MPU3050 is connected sig will contain 0. - ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); - inquiryResult &= MPU_INQUIRY_MASK; - if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_3050; - gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; - return &gyro->mpuDetectionResult; - } - - sig &= MPU_INQUIRY_MASK; - - if (sig == MPUx0x0_WHO_AM_I_CONST) { - - gyro->mpuDetectionResult.sensor = MPU_60x0; - - mpu6050FindRevision(gyro); - } else if (sig == MPU6500_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; - } - - return &gyro->mpuDetectionResult; -} - -#ifdef USE_SPI -static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) -{ -#ifdef USE_GYRO_SPI_MPU6500 - uint8_t mpu6500Sensor = mpu6500SpiDetect(); - if (mpu6500Sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = mpu6500Sensor; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6500ReadRegister; - gyro->mpuConfiguration.write = mpu6500WriteRegister; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_ICM20689 - if (icm20689SpiDetect()) { - gyro->mpuDetectionResult.sensor = ICM_20689_SPI; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = icm20689ReadRegister; - gyro->mpuConfiguration.write = icm20689WriteRegister; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiDetect()) { - gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6000ReadRegister; - gyro->mpuConfiguration.write = mpu6000WriteRegister; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_MPU9250 - if (mpu9250SpiDetect()) { - gyro->mpuDetectionResult.sensor = MPU_9250_SPI; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu9250ReadRegister; - gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister; - gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister; - gyro->mpuConfiguration.write = mpu9250WriteRegister; - gyro->mpuConfiguration.reset = mpu9250ResetGyro; - return true; - } -#endif - - UNUSED(gyro); - return false; -} -#endif - static void mpu6050FindRevision(gyroDev_t *gyro) { bool ack; @@ -188,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c // determine product ID and accel revision - ack = gyro->mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer); + ack = gyro->mpuConfiguration.readFn(MPU_RA_XA_OFFS_H, 6, readBuffer); revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); if (revision) { /* Congrats, these parts are better. */ @@ -202,7 +89,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) failureMode(FAILURE_ACC_INCOMPATIBLE); } } else { - ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId); + ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId); revision = productId & 0x0F; if (!revision) { failureMode(FAILURE_ACC_INCOMPATIBLE); @@ -289,7 +176,7 @@ bool mpuAccRead(accDev_t *acc) { uint8_t data[6]; - bool ack = acc->mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data); + bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data); if (!ack) { return false; } @@ -312,7 +199,7 @@ bool mpuGyroRead(gyroDev_t *gyro) { uint8_t data[6]; - const bool ack = gyro->mpuConfiguration.read(gyro->mpuConfiguration.gyroReadXRegister, 6, data); + const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data); if (!ack) { return false; } @@ -324,11 +211,6 @@ bool mpuGyroRead(gyroDev_t *gyro) return true; } -void mpuGyroInit(gyroDev_t *gyro) -{ - mpuIntExtiInit(gyro); -} - bool mpuCheckDataReady(gyroDev_t* gyro) { bool ret; @@ -340,3 +222,111 @@ bool mpuCheckDataReady(gyroDev_t* gyro) } return ret; } + +#ifdef USE_SPI +static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) +{ +#ifdef USE_GYRO_SPI_MPU6000 + if (mpu6000SpiDetect()) { + gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.readFn = mpu6000ReadRegister; + gyro->mpuConfiguration.writeFn = mpu6000WriteRegister; + return true; + } +#endif + +#ifdef USE_GYRO_SPI_MPU6500 + uint8_t mpu6500Sensor = mpu6500SpiDetect(); + if (mpu6500Sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = mpu6500Sensor; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.readFn = mpu6500ReadRegister; + gyro->mpuConfiguration.writeFn = mpu6500WriteRegister; + return true; + } +#endif + +#ifdef USE_GYRO_SPI_MPU9250 + if (mpu9250SpiDetect()) { + gyro->mpuDetectionResult.sensor = MPU_9250_SPI; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.readFn = mpu9250ReadRegister; + gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister; + gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister; + gyro->mpuConfiguration.writeFn = mpu9250WriteRegister; + gyro->mpuConfiguration.resetFn = mpu9250ResetGyro; + return true; + } +#endif + +#ifdef USE_GYRO_SPI_ICM20689 + if (icm20689SpiDetect()) { + gyro->mpuDetectionResult.sensor = ICM_20689_SPI; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.readFn = icm20689ReadRegister; + gyro->mpuConfiguration.writeFn = icm20689WriteRegister; + return true; + } +#endif + + UNUSED(gyro); + return false; +} +#endif + +mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) +{ + + // MPU datasheet specifies 30ms. + delay(35); + +#ifndef USE_I2C + uint8_t sig = 0; + bool ack = false; +#else + uint8_t sig; + bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); +#endif + if (ack) { + gyro->mpuConfiguration.readFn = mpuReadRegisterI2C; + gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C; + } else { +#ifdef USE_SPI + bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); + UNUSED(detectedSpiSensor); +#endif + + return &gyro->mpuDetectionResult; + } + + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + + // If an MPU3050 is connected sig will contain 0. + uint8_t inquiryResult; + ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); + inquiryResult &= MPU_INQUIRY_MASK; + if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { + gyro->mpuDetectionResult.sensor = MPU_3050; + gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; + return &gyro->mpuDetectionResult; + } + + sig &= MPU_INQUIRY_MASK; + + if (sig == MPUx0x0_WHO_AM_I_CONST) { + + gyro->mpuDetectionResult.sensor = MPU_60x0; + + mpu6050FindRevision(gyro); + } else if (sig == MPU6500_WHO_AM_I_CONST) { + gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; + } + + return &gyro->mpuDetectionResult; +} + +void mpuGyroInit(gyroDev_t *gyro) +{ + mpuIntExtiInit(gyro); +} diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 8ba01540a3..8a44af0464 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -124,18 +124,18 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data); -typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); -typedef void(*mpuResetFuncPtr)(void); +typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data); +typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data); +typedef void(*mpuResetFnPtr)(void); -extern mpuResetFuncPtr mpuReset; +extern mpuResetFnPtr mpuResetFn; typedef struct mpuConfiguration_s { - mpuReadRegisterFunc read; - mpuWriteRegisterFunc write; - mpuReadRegisterFunc slowread; - mpuWriteRegisterFunc verifywrite; - mpuResetFuncPtr reset; + mpuReadRegisterFnPtr readFn; + mpuWriteRegisterFnPtr writeFn; + mpuReadRegisterFnPtr slowreadFn; + mpuWriteRegisterFnPtr verifywriteFn; + mpuResetFnPtr resetFn; uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each } mpuConfiguration_t; @@ -178,7 +178,7 @@ typedef enum { ICM_20689_SPI, ICM_20608_SPI, ICM_20602_SPI -} detectedMPUSensor_e; +} mpuSensor_e; typedef enum { MPU_HALF_RESOLUTION, @@ -186,7 +186,7 @@ typedef enum { } mpu6050Resolution_e; typedef struct mpuDetectionResult_s { - detectedMPUSensor_e sensor; + mpuSensor_e sensor; mpu6050Resolution_e resolution; } mpuDetectionResult_t; diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index 15728d9e12..374604b7c1 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -53,21 +53,21 @@ static void mpu3050Init(gyroDev_t *gyro) delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe - ack = gyro->mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0); + ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0); if (!ack) failureMode(FAILURE_ACC_INIT); - gyro->mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); - gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0); - gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET); - gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); + gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0); + gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET); + gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); } static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) { UNUSED(gyro); uint8_t buf[2]; - if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) { + if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) { return false; } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 631f7ec805..0a1fe6f336 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. // Accel scale 8g (4096 LSB/g) - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif } diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index f9a46fbfc1..30d7caa0fb 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); - gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); + gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); + gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN #else - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR #endif delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable + gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable #endif delay(15); } diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 29e22d5743..8d0af213df 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -130,32 +130,32 @@ void icm20689GyroInit(gyroDev_t *gyro) spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); - gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03); + gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03); delay(100); -// gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); +// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); // delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); + gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration -// gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN +// gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable + gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index e27f8fa652..244cb5e9e5 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -72,7 +72,7 @@ static spiDevice_t spiHardwareMap[] = { { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER }, { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER }, - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF5_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER }, + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER }, { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER } }; diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 8011f04cf4..8f3fcd9d15 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -25,7 +25,7 @@ static IO_t leds[LED_NUMBER]; static uint8_t ledPolarity = 0; -void ledInit(statusLedConfig_t *statusLedConfig) +void ledInit(const statusLedConfig_t *statusLedConfig) { LED0_OFF; LED1_OFF; diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 25961579e4..341bbb81d9 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/io_types.h" #define LED_NUMBER 3 @@ -26,6 +27,8 @@ typedef struct statusLedConfig_s { uint8_t polarity; } statusLedConfig_t; +PG_DECLARE(statusLedConfig_t, statusLedConfig); + // Helpful macros #ifdef LED0 # define LED0_TOGGLE ledToggle(0) @@ -57,6 +60,6 @@ typedef struct statusLedConfig_s { # define LED2_ON do {} while(0) #endif -void ledInit(statusLedConfig_t *statusLedConfig); +void ledInit(const statusLedConfig_t *statusLedConfig); void ledToggle(int led); void ledSet(int led, bool state); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 8f99c16adc..eb71818010 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -83,7 +83,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 #endif } -static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) +static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion) { #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); @@ -91,7 +91,8 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard #endif configTimeBase(timerHardware->tim, period, mhz); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output); + pwmOCConfig(timerHardware->tim, timerHardware->channel, value, + inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); #if defined(USE_HAL_DRIVER) HAL_TIM_PWM_Start(Handle, timerHardware->channel); @@ -259,7 +260,8 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot #ifdef USE_DSHOT if (isDigital) { - pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol); + pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, + motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); motors[motorIndex].enabled = true; continue; } @@ -274,9 +276,9 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot if (useUnsyncedPwm) { const uint32_t hz = timerMhzCounter * 1000000; - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse); + pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion); } else { - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0); + pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion); } bool timerAlreadyUsed = false; @@ -348,7 +350,7 @@ void servoInit(const servoConfig_t *servoConfig) break; } - pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse); + pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); servos[servoIndex].enabled = true; } } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7102e7e351..97c5f5e662 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -119,7 +119,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDigital(uint8_t index, uint16_t value); -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType); +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); #endif diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 5ae299dcf0..9e29cedaa5 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -105,7 +105,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; @@ -139,14 +139,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 03156953a0..95d53b3d5d 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -102,7 +102,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; @@ -136,14 +136,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index b898493250..848cb88ea6 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -96,7 +96,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) UNUSED(motorCount); } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motor->timerHardware = timerHardware; @@ -174,9 +174,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t /* PWM1 Mode configuration: Channel1 */ TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; - TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH; - TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; + if (output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; + TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; + } else { + TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; + } TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.Pulse = 0; diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 87af44df54..5ac340c31e 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -17,6 +17,9 @@ #pragma once +#include +#include + void systemInit(void); void delayMicroseconds(uint32_t us); void delay(uint32_t ms); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 55d570eecb..0216d45427 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -31,8 +31,8 @@ void SetSysClock(void); void systemReset(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } __disable_irq(); @@ -41,8 +41,8 @@ void systemReset(void) void systemResetToBootloader(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index 3254c475d6..0099833674 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -33,8 +33,8 @@ void SystemClock_Config(void); void systemReset(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } __disable_irq(); @@ -43,8 +43,8 @@ void systemReset(void) void systemResetToBootloader(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 39e99d53d7..d8e52a8edd 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -23,6 +23,7 @@ #include #include +//#define USE_PARAMETER_GROUPS #include "platform.h" // FIXME remove this for targets that don't need a CLI. Perhaps use a no-op macro when USE_CLI is not enabled @@ -31,6 +32,8 @@ uint8_t cliMode = 0; #ifdef USE_CLI +#include "blackbox/blackbox.h" + #include "build/build_config.h" #include "build/debug.h" #include "build/version.h" @@ -45,8 +48,12 @@ uint8_t cliMode = 0; #include "config/config_eeprom.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/accgyro.h" #include "drivers/buf_writer.h" @@ -77,6 +84,7 @@ uint8_t cliMode = 0; #include "flight/mixer.h" #include "flight/navigation.h" #include "flight/pid.h" +#include "flight/servos.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -113,16 +121,6 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128]; static char cliBuffer[48]; static uint32_t bufferIndex = 0; -typedef enum { - DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), - DO_DIFF = (1 << 4), - SHOW_DEFAULTS = (1 << 5), - HIDE_UNUSED = (1 << 6) -} dumpFlags_e; - static const char* const emptyName = "-"; #ifndef USE_QUAD_MIXER_ONLY @@ -448,21 +446,21 @@ static const lookupTableEntry_t lookupTables[] = { #define VALUE_MODE_OFFSET 6 typedef enum { - // value type + // value type, bits 0-3 VAR_UINT8 = (0 << VALUE_TYPE_OFFSET), VAR_INT8 = (1 << VALUE_TYPE_OFFSET), VAR_UINT16 = (2 << VALUE_TYPE_OFFSET), VAR_INT16 = (3 << VALUE_TYPE_OFFSET), //VAR_UINT32 = (4 << VALUE_TYPE_OFFSET), - VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), + VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), // 0x05 - // value section + // value section, bits 4-5 MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), - PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), + PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20 // value mode - MODE_DIRECT = (0 << VALUE_MODE_OFFSET), - MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) + MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40 + MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80 } cliValueFlag_e; #define VALUE_TYPE_MASK (0x0F) @@ -483,6 +481,22 @@ typedef union { cliMinMaxConfig_t minmax; } cliValueConfig_t; +#ifdef USE_PARAMETER_GROUPS +typedef struct { + const char *name; + const uint8_t type; // see cliValueFlag_e + const cliValueConfig_t config; + + pgn_t pgn; + uint16_t offset; +} __attribute__((packed)) clivalue_t; + +static const clivalue_t valueTable[] = { + { "dummy", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, 0, 0 } +}; + +#else + typedef struct { const char *name; const uint8_t type; // see cliValueFlag_e @@ -490,7 +504,7 @@ typedef struct { const cliValueConfig_t config; } clivalue_t; -const clivalue_t valueTable[] = { +static const clivalue_t valueTable[] = { #ifndef SKIP_TASK_STATISTICS { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.task_statistics, .config.lookup = { TABLE_OFF_ON } }, #endif @@ -524,6 +538,7 @@ const clivalue_t valueTable[] = { { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } }, + { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE, &motorConfig()->motorPwmInversion, .config.lookup = { TABLE_OFF_ON } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, @@ -810,8 +825,72 @@ const clivalue_t valueTable[] = { { "displayport_max7456_row_adjust", VAR_INT8 | MASTER_VALUE, &displayPortProfileMax7456()->rowAdjust, .config.minmax = { -3, 0 } }, #endif }; +#endif -#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) +#ifdef USE_PARAMETER_GROUPS +static featureConfig_t featureConfigCopy; +static gyroConfig_t gyroConfigCopy; +static accelerometerConfig_t accelerometerConfigCopy; +#ifdef MAG +static compassConfig_t compassConfigCopy; +#endif +#ifdef BARO +static barometerConfig_t barometerConfigCopy; +#endif +#ifdef PITOT +static pitotmeterConfig_t pitotmeterConfigCopy; +#endif +static featureConfig_t featureConfigCopy; +static rxConfig_t rxConfigCopy; +#ifdef BLACKBOX +static blackboxConfig_t blackboxConfigCopy; +#endif +static rxFailsafeChannelConfig_t rxFailsafeChannelConfigsCopy[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +static rxChannelRangeConfig_t rxChannelRangeConfigsCopy[NON_AUX_CHANNEL_COUNT]; +static motorConfig_t motorConfigCopy; +static failsafeConfig_t failsafeConfigCopy; +static boardAlignment_t boardAlignmentCopy; +#ifdef USE_SERVOS +static servoConfig_t servoConfigCopy; +static gimbalConfig_t gimbalConfigCopy; +static servoMixer_t customServoMixersCopy[MAX_SERVO_RULES]; +static servoParam_t servoParamsCopy[MAX_SUPPORTED_SERVOS]; +#endif +static batteryConfig_t batteryConfigCopy; +static motorMixer_t customMotorMixerCopy[MAX_SUPPORTED_MOTORS]; +static mixerConfig_t mixerConfigCopy; +static flight3DConfig_t flight3DConfigCopy; +static serialConfig_t serialConfigCopy; +static imuConfig_t imuConfigCopy; +static armingConfig_t armingConfigCopy; +static rcControlsConfig_t rcControlsConfigCopy; +#ifdef GPS +static gpsConfig_t gpsConfigCopy; +#endif +#ifdef NAV +static positionEstimationConfig_t positionEstimationConfigCopy; +static navConfig_t navConfigCopy; +#endif +#ifdef TELEMETRY +static telemetryConfig_t telemetryConfigCopy; +#endif +static modeActivationCondition_t modeActivationConditionsCopy[MAX_MODE_ACTIVATION_CONDITION_COUNT]; +static adjustmentRange_t adjustmentRangesCopy[MAX_ADJUSTMENT_RANGE_COUNT]; +#ifdef LED_STRIP +static ledStripConfig_t ledStripConfigCopy; +#endif +#ifdef OSD +static osdConfig_t osdConfigCopy; +#endif +static systemConfig_t systemConfigCopy; +#ifdef BEEPER +static beeperConfig_t beeperConfigCopy; +#endif +static controlRateConfig_t controlRateProfilesCopy[MAX_CONTROL_RATE_PROFILE_COUNT]; +static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT]; +static modeActivationOperatorConfig_t modeActivationOperatorConfigCopy; +static beeperConfig_t beeperConfigCopy; +#endif // USE_PARAMETER_GROUPS static void cliPrint(const char *str) { @@ -837,6 +916,16 @@ static void cliPutp(void *p, char ch) bufWriterAppend(p, ch); } +typedef enum { + DUMP_MASTER = (1 << 0), + DUMP_PROFILE = (1 << 1), + DUMP_RATES = (1 << 2), + DUMP_ALL = (1 << 3), + DO_DIFF = (1 << 4), + SHOW_DEFAULTS = (1 << 5), + HIDE_UNUSED = (1 << 6) +} dumpFlags_e; + static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...) { if (!((dumpMask & DO_DIFF) && equalsDefault)) { @@ -930,6 +1019,296 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_ } } +#ifdef USE_PARAMETER_GROUPS + +static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault) +{ + bool result = false; + switch (type & VALUE_TYPE_MASK) { + case VAR_UINT8: + result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; + break; + + case VAR_INT8: + result = *(int8_t *)ptr == *(int8_t *)ptrDefault; + break; + + case VAR_UINT16: + result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; + break; + + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; + +/* not currently used + case VAR_UINT32: + result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; + break;*/ + + case VAR_FLOAT: + result = *(float *)ptr == *(float *)ptrDefault; + break; + } + return result; +} + +typedef struct cliCurrentAndDefaultConfig_s { + const void *currentConfig; // the copy + const void *defaultConfig; // the PG value as set by default +} cliCurrentAndDefaultConfig_t; + +static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn) +{ + static cliCurrentAndDefaultConfig_t ret; + + switch (pgn) { + case PG_GYRO_CONFIG: + ret.currentConfig = &gyroConfigCopy; + ret.defaultConfig = gyroConfig(); + break; + case PG_ACCELEROMETER_CONFIG: + ret.currentConfig = &accelerometerConfigCopy; + ret.defaultConfig = accelerometerConfig(); + break; +#ifdef MAG + case PG_COMPASS_CONFIG: + ret.currentConfig = &compassConfigCopy; + ret.defaultConfig = compassConfig(); + break; +#endif +#ifdef BARO + case PG_BAROMETER_CONFIG: + ret.currentConfig = &barometerConfigCopy; + ret.defaultConfig = barometerConfig(); + break; +#endif +#ifdef PITOT + case PG_PITOTMETER_CONFIG: + ret.currentConfig = &pitotmeterConfigCopy; + ret.defaultConfig = pitotmeterConfig(); + break; +#endif + case PG_FEATURE_CONFIG: + ret.currentConfig = &featureConfigCopy; + ret.defaultConfig = featureConfig(); + break; + case PG_RX_CONFIG: + ret.currentConfig = &rxConfigCopy; + ret.defaultConfig = rxConfig(); + break; +#ifdef BLACKBOX + case PG_BLACKBOX_CONFIG: + ret.currentConfig = &blackboxConfigCopy; + ret.defaultConfig = blackboxConfig(); + break; +#endif + case PG_MOTOR_CONFIG: + ret.currentConfig = &motorConfigCopy; + ret.defaultConfig = motorConfig(); + break; + case PG_FAILSAFE_CONFIG: + ret.currentConfig = &failsafeConfigCopy; + ret.defaultConfig = failsafeConfig(); + break; + case PG_BOARD_ALIGNMENT: + ret.currentConfig = &boardAlignmentCopy; + ret.defaultConfig = boardAlignment(); + break; + case PG_MIXER_CONFIG: + ret.currentConfig = &mixerConfigCopy; + ret.defaultConfig = mixerConfig(); + break; + case PG_MOTOR_3D_CONFIG: + ret.currentConfig = &flight3DConfigCopy; + ret.defaultConfig = flight3DConfig(); + break; +#ifdef USE_SERVOS + case PG_SERVO_CONFIG: + ret.currentConfig = &servoConfigCopy; + ret.defaultConfig = servoConfig(); + break; + case PG_GIMBAL_CONFIG: + ret.currentConfig = &gimbalConfigCopy; + ret.defaultConfig = gimbalConfig(); + break; +#endif + case PG_BATTERY_CONFIG: + ret.currentConfig = &batteryConfigCopy; + ret.defaultConfig = batteryConfig(); + break; + case PG_SERIAL_CONFIG: + ret.currentConfig = &serialConfigCopy; + ret.defaultConfig = serialConfig(); + break; + case PG_IMU_CONFIG: + ret.currentConfig = &imuConfigCopy; + ret.defaultConfig = imuConfig(); + break; + case PG_RC_CONTROLS_CONFIG: + ret.currentConfig = &rcControlsConfigCopy; + ret.defaultConfig = rcControlsConfig(); + break; + case PG_ARMING_CONFIG: + ret.currentConfig = &armingConfigCopy; + ret.defaultConfig = armingConfig(); + break; +#ifdef GPS + case PG_GPS_CONFIG: + ret.currentConfig = &gpsConfigCopy; + ret.defaultConfig = gpsConfig(); + break; +#endif +#ifdef NAV + case PG_POSITION_ESTIMATION_CONFIG: + ret.currentConfig = &positionEstimationConfigCopy; + ret.defaultConfig = positionEstimationConfig(); + break; + case PG_NAV_CONFIG: + ret.currentConfig = &navConfigCopy; + ret.defaultConfig = navConfig(); + break; +#endif +#ifdef TELEMETRY + case PG_TELEMETRY_CONFIG: + ret.currentConfig = &telemetryConfigCopy; + ret.defaultConfig = telemetryConfig(); + break; +#endif +#ifdef LED_STRIP + case PG_LED_STRIP_CONFIG: + ret.currentConfig = &ledStripConfigCopy; + ret.defaultConfig = ledStripConfig(); + break; +#endif +#ifdef OSD + case PG_OSD_CONFIG: + ret.currentConfig = &osdConfigCopy; + ret.defaultConfig = osdConfig(); + break; +#endif + case PG_SYSTEM_CONFIG: + ret.currentConfig = &systemConfigCopy; + ret.defaultConfig = systemConfig(); + break; + case PG_MODE_ACTIVATION_OPERATOR_CONFIG: + ret.currentConfig = &modeActivationOperatorConfigCopy; + ret.defaultConfig = modeActivationOperatorConfig(); + break; + case PG_CONTROL_RATE_PROFILES: + ret.currentConfig = &controlRateProfilesCopy[0]; + ret.defaultConfig = controlRateProfiles(0); + break; + case PG_PID_PROFILE: + ret.currentConfig = &pidProfileCopy[getConfigProfile()]; + ret.defaultConfig = pidProfile(); + break; + case PG_RX_FAILSAFE_CHANNEL_CONFIG: + ret.currentConfig = &rxFailsafeChannelConfigsCopy[0]; + ret.defaultConfig = rxFailsafeChannelConfigs(0); + break; + case PG_RX_CHANNEL_RANGE_CONFIG: + ret.currentConfig = &rxChannelRangeConfigsCopy[0]; + ret.defaultConfig = rxChannelRangeConfigs(0); + break; +#ifdef USE_SERVOS + case PG_SERVO_MIXER: + ret.currentConfig = &customServoMixersCopy[0]; + ret.defaultConfig = customServoMixers(0); + break; + case PG_SERVO_PARAMS: + ret.currentConfig = &servoParamsCopy[0]; + ret.defaultConfig = servoParams(0); + break; +#endif + case PG_MOTOR_MIXER: + ret.currentConfig = &customMotorMixerCopy[0]; + ret.defaultConfig = customMotorMixer(0); + break; + case PG_MODE_ACTIVATION_PROFILE: + ret.currentConfig = &modeActivationConditionsCopy[0]; + ret.defaultConfig = modeActivationConditions(0); + break; + case PG_ADJUSTMENT_RANGE_CONFIG: + ret.currentConfig = &adjustmentRangesCopy[0]; + ret.defaultConfig = adjustmentRanges(0); + break; + case PG_BEEPER_CONFIG: + ret.currentConfig = &beeperConfigCopy; + ret.defaultConfig = beeperConfig(); + break; + default: + ret.currentConfig = NULL; + ret.defaultConfig = NULL; + break; + } + return &ret; +} + +static uint16_t getValueOffset(const clivalue_t *value) +{ + switch (value->type & VALUE_SECTION_MASK) { + case MASTER_VALUE: + case PROFILE_VALUE: + return value->offset; + case PROFILE_RATE_VALUE: + return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + } + return 0; +} + +static void *getValuePointer(const clivalue_t *value) +{ + const pgRegistry_t* rec = pgFind(value->pgn); + + switch (value->type & VALUE_SECTION_MASK) { + case MASTER_VALUE: + case PROFILE_VALUE: + return rec->address + value->offset; + case PROFILE_RATE_VALUE: + return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + } + return NULL; +} + +static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) +{ + const char *format = "set %s = "; + const cliCurrentAndDefaultConfig_t *config = getCurrentAndDefaultConfigs(value->pgn); + if (config->currentConfig == NULL || config->defaultConfig == NULL) { + // has not been set up properly + cliPrintf("VALUE %s ERROR\r\n", value->name); + return; + } + const int valueOffset = getValueOffset(value); + switch (dumpMask & (DO_DIFF | SHOW_DEFAULTS)) { + case DO_DIFF: + if (valuePtrEqualsDefault(value->type, (uint8_t*)config->currentConfig + valueOffset, (uint8_t*)config->defaultConfig + valueOffset)) { + break; + } + // drop through, since not equal to default + case 0: + case SHOW_DEFAULTS: + cliPrintf(format, value->name); + printValuePointer(value, (uint8_t*)config->currentConfig + valueOffset, 0); + cliPrint("\r\n"); + break; + } +} + +static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) +{ + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { + const clivalue_t *value = &valueTable[i]; + bufWriterFlush(cliWriter); + if ((value->type & VALUE_SECTION_MASK) == valueSection) { + dumpPgValue(value, dumpMask); + } + } +} + +#else + void *getValuePointer(const clivalue_t *value) { void *ptr = value->ptr; @@ -944,6 +1323,7 @@ void *getValuePointer(const clivalue_t *value) return ptr; } +#endif static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig) { @@ -1005,7 +1385,7 @@ static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const maste static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *defaultConfig) { const clivalue_t *value; - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { value = &valueTable[i]; if ((value->type & VALUE_SECTION_MASK) != valueSection) { @@ -1052,13 +1432,7 @@ typedef union { static void cliSetVar(const clivalue_t *var, const int_float_value_t value) { - void *ptr = var->ptr; - if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); - } - if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); - } + void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -1107,9 +1481,9 @@ static void cliShowArgumentRangeError(char *name, int min, int max) cliPrintf("%s not between %d and %d\r\n", name, min, max); } -static char *nextArg(char *currentArg) +static const char *nextArg(const char *currentArg) { - char *ptr = strchr(currentArg, ' '); + const char *ptr = strchr(currentArg, ' '); while (ptr && *ptr == ' ') { ptr++; } @@ -1117,14 +1491,12 @@ static char *nextArg(char *currentArg) return ptr; } -static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount) +static const char *processChannelRangeArgs(const char *ptr, channelRange_t *range, uint8_t *validArgumentCount) { - int val; - for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); val = CHANNEL_VALUE_TO_STEP(val); if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { if (argIndex == 0) { @@ -1143,10 +1515,10 @@ static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t * // Check if a string's length is zero static bool isEmpty(const char *string) { - return *string == '\0'; + return (string == NULL || *string == '\0') ? true : false; } -static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) +static void printRxFailsafe(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) { // print out rxConfig failsafe settings for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { @@ -1185,7 +1557,7 @@ static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxCo } } -static void cliRxFail(char *cmdline) +static void cliRxFailsafe(char *cmdline) { uint8_t channel; char buf[3]; @@ -1193,10 +1565,10 @@ static void cliRxFail(char *cmdline) if (isEmpty(cmdline)) { // print out rxConfig failsafe settings for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { - cliRxFail(itoa(channel, buf, 10)); + cliRxFailsafe(itoa(channel, buf, 10)); } } else { - char *ptr = cmdline; + const char *ptr = cmdline; channel = atoi(ptr++); if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) { @@ -1303,7 +1675,7 @@ static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActiva static void cliAux(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printAux(DUMP_MASTER, modeActivationProfile(), NULL); @@ -1377,13 +1749,9 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co static void cliSerial(char *cmdline) { - int i, val; - char *ptr; - if (isEmpty(cmdline)) { printSerial(DUMP_MASTER, serialConfig(), NULL); - - return; + return; } serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); @@ -1392,9 +1760,9 @@ static void cliSerial(char *cmdline) uint8_t validArgumentCount = 0; - ptr = cmdline; + const char *ptr = cmdline; - val = atoi(ptr++); + int val = atoi(ptr++); currentConfig = serialFindPortConfiguration(val); if (currentConfig) { portConfig.identifier = val; @@ -1408,7 +1776,7 @@ static void cliSerial(char *cmdline) validArgumentCount++; } - for (i = 0; i < 4; i ++) { + for (int i = 0; i < 4; i ++) { ptr = nextArg(ptr); if (!ptr) { break; @@ -1457,7 +1825,6 @@ static void cliSerial(char *cmdline) } memcpy(currentConfig, &portConfig, sizeof(portConfig)); - } #ifndef SKIP_SERIAL_PASSTHROUGH @@ -1471,7 +1838,8 @@ static void cliSerialPassthrough(char *cmdline) int id = -1; uint32_t baud = 0; unsigned mode = 0; - char* tok = strtok(cmdline, " "); + char *saveptr; + char* tok = strtok_r(cmdline, " ", &saveptr); int index = 0; while (tok != NULL) { @@ -1490,7 +1858,7 @@ static void cliSerialPassthrough(char *cmdline) break; } index++; - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ", &saveptr); } serialPort_t *passThroughPort; @@ -1575,7 +1943,7 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *ad static void cliAdjustmentRange(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printAdjustmentRange(DUMP_MASTER, adjustmentProfile(), NULL); @@ -1679,14 +2047,14 @@ static void cliMotorMix(char *cmdline) #else int check = 0; uint8_t len; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - customMotorMixer(i)->throttle = 0.0f; + customMotorMixerMutable(i)->throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = nextArg(cmdline); if (ptr) { @@ -1697,7 +2065,7 @@ static void cliMotorMix(char *cmdline) break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { - mixerLoadMix(i, masterConfig.customMotorMixer); + mixerLoadMix(i, customMotorMixerMutable(0)); cliPrintf("Loaded %s\r\n", mixerNames[i]); cliMotorMix(""); break; @@ -1710,22 +2078,22 @@ static void cliMotorMix(char *cmdline) if (i < MAX_SUPPORTED_MOTORS) { ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->throttle = fastA2F(ptr); + customMotorMixerMutable(i)->throttle = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->roll = fastA2F(ptr); + customMotorMixerMutable(i)->roll = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->pitch = fastA2F(ptr); + customMotorMixerMutable(i)->pitch = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->yaw = fastA2F(ptr); + customMotorMixerMutable(i)->yaw = fastA2F(ptr); check++; } if (check != 4) { @@ -1767,7 +2135,7 @@ static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxC static void cliRxRange(char *cmdline) { int i, validArgumentCount = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printRxRange(DUMP_MASTER, rxConfig(), NULL); @@ -1829,7 +2197,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC static void cliLed(char *cmdline) { int i; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printLed(DUMP_MASTER, ledStripConfig()->ledConfigs, NULL); @@ -1867,7 +2235,7 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo static void cliColor(char *cmdline) { int i; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printColor(DUMP_MASTER, ledStripConfig()->colors, NULL); @@ -1930,10 +2298,11 @@ static void cliModeColor(char *cmdline) enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; int args[ARGS_COUNT]; int argNo = 0; - char* ptr = strtok(cmdline, " "); + char *saveptr; + char* ptr = strtok_r(cmdline, " ", &saveptr); while (ptr && argNo < ARGS_COUNT) { args[argNo++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr != NULL || argNo != ARGS_COUNT) { @@ -2076,7 +2445,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) { const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - servoMixer_t customServoMixer = *customServoMixer(i); + const servoMixer_t customServoMixer = *customServoMixers(i); if (customServoMixer.rate == 0) { break; } @@ -2145,10 +2514,8 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) static void cliServoMix(char *cmdline) { - uint8_t len; - char *ptr; int args[8], check = 0; - len = strlen(cmdline); + int len = strlen(cmdline); if (len == 0) { printServoMix(DUMP_MASTER, NULL); @@ -2159,7 +2526,7 @@ static void cliServoMix(char *cmdline) servoProfile()->servoConf[i].reversedSources = 0; } } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = nextArg(cmdline); + const char *ptr = nextArg(cmdline); if (ptr) { len = strlen(ptr); for (uint32_t i = 0; ; i++) { @@ -2177,7 +2544,7 @@ static void cliServoMix(char *cmdline) } } else if (strncasecmp(cmdline, "reverse", 7) == 0) { enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; - ptr = strchr(cmdline, ' '); + char *ptr = strchr(cmdline, ' '); len = strlen(ptr); if (len == 0) { @@ -2195,10 +2562,11 @@ static void cliServoMix(char *cmdline) return; } - ptr = strtok(ptr, " "); + char *saveptr; + ptr = strtok_r(ptr, " ", &saveptr); while (ptr != NULL && check < ARGS_COUNT - 1) { args[check++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr == NULL || check != ARGS_COUNT - 1) { @@ -2219,10 +2587,11 @@ static void cliServoMix(char *cmdline) cliServoMix("reverse"); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; - ptr = strtok(cmdline, " "); + char *saveptr; + char *ptr = strtok_r(cmdline, " ", &saveptr); while (ptr != NULL && check < ARGS_COUNT) { args[check++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr != NULL || check != ARGS_COUNT) { @@ -2239,13 +2608,13 @@ static void cliServoMix(char *cmdline) args[MIN] >= 0 && args[MIN] <= 100 && args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { - customServoMixer(i)->targetChannel = args[TARGET]; - customServoMixer(i)->inputSource = args[INPUT]; - customServoMixer(i)->rate = args[RATE]; - customServoMixer(i)->speed = args[SPEED]; - customServoMixer(i)->min = args[MIN]; - customServoMixer(i)->max = args[MAX]; - customServoMixer(i)->box = args[BOX]; + customServoMixersMutable(i)->targetChannel = args[TARGET]; + customServoMixersMutable(i)->inputSource = args[INPUT]; + customServoMixersMutable(i)->rate = args[RATE]; + customServoMixersMutable(i)->speed = args[SPEED]; + customServoMixersMutable(i)->min = args[MIN]; + customServoMixersMutable(i)->max = args[MAX]; + customServoMixersMutable(i)->box = args[BOX]; cliServoMix(""); } else { cliShowParseError(); @@ -2364,7 +2733,7 @@ static void cliFlashErase(char *cmdline) #endif delay(100); } - + beeper(BEEPER_BLACKBOX_ERASE); cliPrintf("\r\nDone.\r\n"); } @@ -2393,7 +2762,7 @@ static void cliFlashRead(char *cmdline) uint8_t buffer[32]; - char *nextArg = strchr(cmdline, ' '); + const char *nextArg = strchr(cmdline, ' '); if (!nextArg) { cliShowParseError(); @@ -2462,7 +2831,7 @@ static void printVtx(uint8_t dumpMask, const master_t *defaultConfig) static void cliVtx(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printVtx(DUMP_MASTER, NULL); @@ -2901,7 +3270,7 @@ static void cliMixer(char *cmdline) return; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - mixerConfig()->mixerMode = i + 1; + mixerConfigMutable()->mixerMode = i + 1; break; } } @@ -3033,7 +3402,13 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_ cliPrintHashLine("profile"); cliProfile(""); cliPrint("\r\n"); +#ifdef USE_PARAMETER_GROUPS + (void)(defaultConfig); + dumpAllValues(PROFILE_VALUE, dumpMask); + dumpAllValues(PROFILE_RATE_VALUE, dumpMask); +#else dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); +#endif cliRateProfile(""); } @@ -3073,7 +3448,7 @@ static void cliGet(char *cmdline) const clivalue_t *val; int matchedCommands = 0; - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { if (strstr(valueTable[i].name, cmdline)) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); @@ -3104,7 +3479,7 @@ static void cliSet(char *cmdline) if (len == 0 || (len == 1 && cmdline[0] == '*')) { cliPrint("Current settings: \r\n"); - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui @@ -3125,7 +3500,7 @@ static void cliSet(char *cmdline) eqptr++; } - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { val = &valueTable[i]; // ensure exact match when setting to prevent setting variables with shorter names if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { @@ -3315,7 +3690,7 @@ static void cliVersion(char *cmdline) #if defined(USE_RESOURCE_MGMT) -#define MAX_RESOURCE_INDEX(x) (x == 0 ? 1 : x) +#define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x)) typedef struct { const uint8_t owner; @@ -3384,6 +3759,10 @@ static void printResourceOwner(uint8_t owner, uint8_t index) static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) { + if (!tag) { + return; + } + const char * format = "\r\nNOTE: %c%02d already assigned to "; for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) { for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) { @@ -3540,6 +3919,51 @@ static void cliResource(char *cmdline) } #endif /* USE_RESOURCE_MGMT */ +#ifdef USE_PARAMETER_GROUPS +static void backupConfigs(void) +{ + // make copies of configs to do differencing + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("BACKUP %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(&pidProfileCopy[0], reg->address, sizeof(pidProfile_t) * MAX_PROFILE_COUNT); +} + +static void restoreConfigs(void) +{ + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("RESTORE %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(reg->address, &pidProfileCopy[0], sizeof(pidProfile_t) * MAX_PROFILE_COUNT); +} +#endif + static void printConfig(char *cmdline, bool doDiff) { uint8_t dumpMask = DUMP_MASTER; @@ -3556,13 +3980,21 @@ static void printConfig(char *cmdline, bool doDiff) options = cmdline; } - static master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; } + static master_t defaultConfig; createDefaultConfig(&defaultConfig); +#ifdef USE_PARAMETER_GROUPS + backupConfigs(); + // reset all configs to defaults to do differencing + resetConfigs(); +#if defined(TARGET_CONFIG) + targetConfiguration(&defaultConfig); +#endif +#endif if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values } @@ -3646,7 +4078,7 @@ static void printConfig(char *cmdline, bool doDiff) #endif cliPrintHashLine("rxfail"); - printRxFail(dumpMask, rxConfig(), &defaultConfig.rxConfig); + printRxFailsafe(dumpMask, rxConfig(), &defaultConfig.rxConfig); cliPrintHashLine("master"); dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); @@ -3685,6 +4117,10 @@ static void printConfig(char *cmdline, bool doDiff) if (dumpMask & DUMP_RATES) { cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } +#ifdef USE_PARAMETER_GROUPS + // restore configs from copies + restoreConfigs(); +#endif } static void cliDump(char *cmdline) @@ -3782,7 +4218,7 @@ const clicmd_t cmdTable[] = { #if defined(USE_RESOURCE_MGMT) CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource), #endif - CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), + CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFailsafe), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), #ifdef USE_SDCARD @@ -3811,13 +4247,11 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif }; -#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t)) - static void cliHelp(char *cmdline) { UNUSED(cmdline); - for (uint32_t i = 0; i < CMD_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) { cliPrint(cmdTable[i].name); #ifndef MINIMAL_CLI if (cmdTable[i].description) { @@ -3846,7 +4280,7 @@ void cliProcess(void) // do tab completion const clicmd_t *cmd, *pstart = NULL, *pend = NULL; uint32_t i = bufferIndex; - for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0)) continue; if (!pstart) @@ -3907,12 +4341,12 @@ void cliProcess(void) const clicmd_t *cmd; char *options; - for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { if ((options = checkCommand(cliBuffer, cmd->name))) { break; - } + } } - if(cmd < cmdTable + CMD_COUNT) + if(cmd < cmdTable + ARRAYLEN(cmdTable)) cmd->func(options); else cliPrint("Unknown command, try 'help'"); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 6a78dbe837..e7f5628d2b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -29,12 +29,18 @@ #include "cms/cms.h" -#include "common/color.h" #include "common/axis.h" -#include "common/maths.h" +#include "common/color.h" #include "common/filter.h" +#include "common/maths.h" + +#include "config/config_eeprom.h" +#include "config/config_master.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" -#include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/io.h" @@ -45,6 +51,7 @@ #include "drivers/rx_pwm.h" #include "drivers/rx_spi.h" #include "drivers/sdcard.h" +#include "drivers/sensor.h" #include "drivers/serial.h" #include "drivers/sound_beeper.h" #include "drivers/system.h" @@ -56,42 +63,37 @@ #include "fc/fc_rc.h" #include "fc/runtime_config.h" -#include "sensors/sensors.h" -#include "sensors/gyro.h" -#include "sensors/compass.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" -#include "sensors/boardalignment.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/navigation.h" +#include "flight/pid.h" +#include "flight/servos.h" #include "io/beeper.h" -#include "io/serial.h" #include "io/gimbal.h" -#include "io/motors.h" -#include "io/servos.h" -#include "io/ledstrip.h" #include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" #include "io/osd.h" +#include "io/serial.h" +#include "io/servos.h" #include "io/vtx.h" #include "rx/rx.h" #include "rx/rx_spi.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" + #include "telemetry/telemetry.h" -#include "flight/mixer.h" -#include "flight/servos.h" -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/failsafe.h" -#include "flight/altitudehold.h" -#include "flight/navigation.h" - -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #ifndef DEFAULT_RX_FEATURE #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM #endif @@ -544,7 +546,7 @@ uint8_t getCurrentProfile(void) return masterConfig.current_profile_index; } -void setProfile(uint8_t profileIndex) +static void setProfile(uint8_t profileIndex) { currentProfile = &masterConfig.profile[profileIndex]; currentControlRateProfileIndex = currentProfile->activeRateProfile; @@ -869,11 +871,14 @@ void createDefaultConfig(master_t *config) } } -static void resetConf(void) +void resetConfigs(void) { createDefaultConfig(&masterConfig); + pgResetAll(MAX_PROFILE_COUNT); + pgActivateProfile(0); setProfile(0); + setControlRateProfile(0); #ifdef LED_STRIP reevaluateLedConfig(); @@ -886,32 +891,19 @@ void activateConfig(void) resetAdjustmentStates(); - useRcControlsConfig( - modeActivationProfile()->modeActivationConditions, - &masterConfig.motorConfig, - ¤tProfile->pidProfile - ); - -#ifdef TELEMETRY - telemetryUseConfig(&masterConfig.telemetryConfig); -#endif + useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); + useAdjustmentConfig(¤tProfile->pidProfile); #ifdef GPS gpsUseProfile(&masterConfig.gpsProfile); gpsUsePIDs(¤tProfile->pidProfile); #endif - useFailsafeConfig(&masterConfig.failsafeConfig); - setAccelerationTrims(&accelerometerConfig()->accZero); + failsafeReset(); + setAccelerationTrims(&accelerometerConfigMutable()->accZero); setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); - mixerUseConfigs( - &masterConfig.flight3DConfig, - &masterConfig.motorConfig, - &masterConfig.mixerConfig, - &masterConfig.airplaneConfig, - &masterConfig.rxConfig - ); + mixerUseConfigs(&masterConfig.airplaneConfig); #ifdef USE_SERVOS servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoProfile.servoConf, &masterConfig.gimbalConfig, &masterConfig.channelForwardingConfig); @@ -923,22 +915,13 @@ void activateConfig(void) throttleCorrectionConfig()->throttle_correction_angle ); - configureAltitudeHold( - ¤tProfile->pidProfile, - &masterConfig.barometerConfig, - &masterConfig.rcControlsConfig, - &masterConfig.motorConfig - ); - -#ifdef BARO - useBarometerConfig(&masterConfig.barometerConfig); -#endif + configureAltitudeHold(¤tProfile->pidProfile); } void validateAndFixConfig(void) { - if ((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { - motorConfig()->mincommand = 1000; + if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ + motorConfigMutable()->mincommand = 1000; } if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) { @@ -1055,18 +1038,18 @@ void validateAndFixGyroConfig(void) { // Prevent invalid notch cutoff if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { - gyroConfig()->gyro_soft_notch_hz_1 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; } if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) { - gyroConfig()->gyro_soft_notch_hz_2 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } float samplingTime = 0.000125f; if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { - pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - gyroConfig()->gyro_sync_denom = 1; - gyroConfig()->gyro_use_32khz = false; + pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + gyroConfigMutable()->gyro_sync_denom = 1; + gyroConfigMutable()->gyro_use_32khz = false; samplingTime = 0.001f; } @@ -1074,18 +1057,18 @@ void validateAndFixGyroConfig(void) samplingTime = 0.00003125; // F1 and F3 can't handle high sample speed. #if defined(STM32F1) - gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); #elif defined(STM32F3) - gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); #endif } else { #if defined(STM32F1) - gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); #endif } #if !defined(GYRO_USES_SPI) || !defined(USE_MPU_DATA_READY_SIGNAL) - gyroConfig()->gyro_isr_update = false; + gyroConfigMutable()->gyro_isr_update = false; #endif // check for looptime restrictions based on motor protocol. Motor times have safety margin @@ -1115,7 +1098,7 @@ void validateAndFixGyroConfig(void) if (pidLooptime < motorUpdateRestriction) { const uint8_t maxPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM); - pidConfig()->pid_process_denom = MIN(pidConfig()->pid_process_denom, maxPidProcessDenom); + pidConfigMutable()->pid_process_denom = MIN(pidConfigMutable()->pid_process_denom, maxPidProcessDenom); } // Prevent overriding the max rate of motors @@ -1123,25 +1106,57 @@ void validateAndFixGyroConfig(void) uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); if(motorConfig()->motorPwmRate > maxEscRate) - motorConfig()->motorPwmRate = maxEscRate; + motorConfigMutable()->motorPwmRate = maxEscRate; } } +void readEEPROM(void) +{ + suspendRxSignal(); + + // Sanity check, read flash + if (!loadEEPROM()) { + failureMode(FAILURE_INVALID_EEPROM_CONTENTS); + } + +// pgActivateProfile(getCurrentProfile()); +// setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); + + if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check + masterConfig.current_profile_index = 0; + } + + setProfile(masterConfig.current_profile_index); + + validateAndFixConfig(); + activateConfig(); + + resumeRxSignal(); +} + +void writeEEPROM(void) +{ + suspendRxSignal(); + + writeConfigToEEPROM(); + + resumeRxSignal(); +} + +void resetEEPROM(void) +{ + resetConfigs(); + writeEEPROM(); +} + void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { return; } - resetEEPROM(); } -void resetEEPROM(void) -{ - resetConf(); - writeEEPROM(); -} - void saveConfigAndNotify(void) { writeEEPROM(); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 0fe4f9e616..b092f15f80 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -17,8 +17,11 @@ #pragma once +#include #include +#include "config/parameter_group.h" + #if FLASH_SIZE <= 128 #define MAX_PROFILE_COUNT 2 #else @@ -58,6 +61,16 @@ typedef enum { FEATURE_ESC_SENSOR = 1 << 27, } features_e; +typedef struct systemConfig_s { + uint8_t debug_mode; +} systemConfig_t; + +//!!TODOPG_DECLARE(systemConfig_t, systemConfig); +struct profile_s; +extern struct profile_s *currentProfile; +struct controlRateConfig_s; +extern struct controlRateConfig_s *currentControlRateProfile; + void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); void beeperOffClear(uint32_t mask); @@ -69,7 +82,10 @@ void setPreferredBeeperOffMask(uint32_t mask); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); +void initEEPROM(void); void resetEEPROM(void); +void readEEPROM(void); +void writeEEPROM(); void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); @@ -79,14 +95,16 @@ void activateConfig(void); uint8_t getCurrentProfile(void); void changeProfile(uint8_t profileIndex); -void setProfile(uint8_t profileIndex); +struct profile_s; +void resetProfile(struct profile_s *profile); uint8_t getCurrentControlRateProfile(void); void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); -struct master_s; +void resetConfigs(void); +struct master_s; void targetConfiguration(struct master_s *config); void targetValidateConfiguration(struct master_s *config); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 304e98de27..47535bb6f3 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -24,20 +24,26 @@ #include "blackbox/blackbox.h" -#include "common/maths.h" #include "common/axis.h" -#include "common/utils.h" #include "common/filter.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/light_led.h" #include "drivers/system.h" #include "drivers/gyro_sync.h" -#include "sensors/sensors.h" -#include "sensors/boardalignment.h" #include "sensors/acceleration.h" -#include "sensors/gyro.h" +#include "sensors/barometer.h" #include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -59,15 +65,15 @@ #include "scheduler/scheduler.h" +#include "telemetry/telemetry.h" + #include "flight/mixer.h" #include "flight/servos.h" #include "flight/pid.h" #include "flight/failsafe.h" #include "flight/altitudehold.h" +#include "flight/imu.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" // June 2013 V2.2-dev @@ -95,8 +101,8 @@ static bool armingCalibrationWasInitialised; void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { - accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; - accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; + accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; + accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; saveConfigAndNotify(); } @@ -183,15 +189,6 @@ void mwArm(void) ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); -#ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); - if (sharedBlackboxAndMspPort) { - mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); - } - startBlackbox(); - } -#endif disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero //beep to indicate arming @@ -291,7 +288,7 @@ void processRx(timeUs_t currentTimeUs) failsafeUpdateState(); } - throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset @@ -357,7 +354,7 @@ void processRx(timeUs_t currentTimeUs) } } - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch); + processRcStickPositions(throttleStatus); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); @@ -367,7 +364,7 @@ void processRx(timeUs_t currentTimeUs) if (!cliMode) { updateAdjustmentStates(adjustmentProfile()->adjustmentRanges); - processRcAdjustments(currentControlRateProfile, rxConfig()); + processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; @@ -488,7 +485,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { - applyAltHold(&masterConfig.airplaneConfig); + applyAltHold(); } } #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7d0dfbc86e..5abdcaee74 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -28,6 +28,12 @@ #include "common/maths.h" #include "common/printf.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "cms/cms.h" #include "cms/cms_types.h" @@ -115,10 +121,6 @@ #include "flight/failsafe.h" #include "flight/navigation.h" -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -257,16 +259,16 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated #if defined(AVOID_UART1_FOR_PWM_PPM) - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), + serialInit(feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); #elif defined(AVOID_UART2_FOR_PWM_PPM) - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), + serialInit(feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #elif defined(AVOID_UART3_FOR_PWM_PPM) - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), + serialInit(feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); + serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer); @@ -401,12 +403,7 @@ void init(void) } #endif -#ifdef SONAR - const sonarConfig_t *sonarConfig = sonarConfig(); -#else - const void *sonarConfig = NULL; -#endif - if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) { + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } @@ -446,7 +443,7 @@ void init(void) cliInit(serialConfig()); #endif - failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle); + failsafeInit(); rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions); @@ -554,7 +551,7 @@ void init(void) // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) - batteryInit(batteryConfig()); + batteryInit(); #ifdef USE_DASHBOARD if (feature(FEATURE_DASHBOARD)) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6e7adf2027..d882b9eb5d 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -34,20 +34,27 @@ #include "common/maths.h" #include "common/streambuf.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/accgyro.h" -#include "drivers/compass.h" -#include "drivers/serial.h" #include "drivers/bus_i2c.h" -#include "drivers/io.h" +#include "drivers/compass.h" #include "drivers/flash.h" -#include "drivers/sdcard.h" -#include "drivers/vcd.h" +#include "drivers/io.h" #include "drivers/max7456.h" -#include "drivers/vtx_soft_spi_rtc6705.h" #include "drivers/pwm_output.h" +#include "drivers/sdcard.h" +#include "drivers/serial.h" #include "drivers/serial_escserial.h" +#include "drivers/system.h" +#include "drivers/vcd.h" #include "drivers/vtx_common.h" +#include "drivers/vtx_soft_spi_rtc6705.h" #include "fc/config.h" #include "fc/fc_core.h" @@ -56,17 +63,25 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/navigation.h" +#include "flight/pid.h" +#include "flight/servos.h" + +#include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" -#include "io/motors.h" -#include "io/servos.h" +#include "io/flashfs.h" #include "io/gps.h" #include "io/gimbal.h" -#include "io/serial.h" #include "io/ledstrip.h" -#include "io/flashfs.h" -#include "io/transponder_ir.h" -#include "io/asyncfatfs/asyncfatfs.h" +#include "io/motors.h" +#include "io/serial.h" #include "io/serial_4way.h" +#include "io/servos.h" +#include "io/transponder_ir.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -77,36 +92,22 @@ #include "scheduler/scheduler.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" -#include "sensors/battery.h" -#include "sensors/sonar.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" #include "sensors/compass.h" #include "sensors/gyro.h" +#include "sensors/sensors.h" +#include "sensors/sonar.h" #include "telemetry/telemetry.h" -#include "flight/mixer.h" -#include "flight/servos.h" -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" -#include "flight/altitudehold.h" - -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif extern uint16_t cycleTime; // FIXME dependency on mw.c -extern void resetProfile(profile_t *profile); static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; @@ -150,6 +151,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXAIRMODE, "AIR MODE;", 28 }, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29}, { BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30}, + { BOXBLACKBOXERASE, "BLACKBOX ERASE;", 31 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -378,6 +380,7 @@ void initActiveBoxIds(void) #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; + activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE; } #endif @@ -442,6 +445,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX; @@ -675,25 +679,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_SERVO_CONFIGURATIONS: for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - sbufWriteU16(dst, servoProfile()->servoConf[i].min); - sbufWriteU16(dst, servoProfile()->servoConf[i].max); - sbufWriteU16(dst, servoProfile()->servoConf[i].middle); - sbufWriteU8(dst, servoProfile()->servoConf[i].rate); - sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin); - sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax); - sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel); - sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources); + sbufWriteU16(dst, servoParams(i)->min); + sbufWriteU16(dst, servoParams(i)->max); + sbufWriteU16(dst, servoParams(i)->middle); + sbufWriteU8(dst, servoParams(i)->rate); + sbufWriteU8(dst, servoParams(i)->angleAtMin); + sbufWriteU8(dst, servoParams(i)->angleAtMax); + sbufWriteU8(dst, servoParams(i)->forwardFromChannel); + sbufWriteU32(dst, servoParams(i)->reversedSources); } break; case MSP_SERVO_MIX_RULES: for (int i = 0; i < MAX_SERVO_RULES; i++) { - sbufWriteU8(dst, customServoMixer(i)->targetChannel); - sbufWriteU8(dst, customServoMixer(i)->inputSource); - sbufWriteU8(dst, customServoMixer(i)->rate); - sbufWriteU8(dst, customServoMixer(i)->speed); - sbufWriteU8(dst, customServoMixer(i)->min); - sbufWriteU8(dst, customServoMixer(i)->max); - sbufWriteU8(dst, customServoMixer(i)->box); + sbufWriteU8(dst, customServoMixers(i)->targetChannel); + sbufWriteU8(dst, customServoMixers(i)->inputSource); + sbufWriteU8(dst, customServoMixers(i)->rate); + sbufWriteU8(dst, customServoMixers(i)->speed); + sbufWriteU8(dst, customServoMixers(i)->min); + sbufWriteU8(dst, customServoMixers(i)->max); + sbufWriteU8(dst, customServoMixers(i)->box); } break; #endif @@ -800,7 +804,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_ADJUSTMENT_RANGES: for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + const adjustmentRange_t *adjRange = adjustmentRanges(i); sbufWriteU8(dst, adjRange->adjustmentIndex); sbufWriteU8(dst, adjRange->auxChannelIndex); sbufWriteU8(dst, adjRange->range.startStep); @@ -967,8 +971,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_RXFAIL_CONFIG: for (int i = 0; i < rxRuntimeConfig.channelCount; i++) { - sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode); - sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step)); + sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode); + sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step)); } break; @@ -1014,7 +1018,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #ifdef LED_STRIP case MSP_LED_COLORS: for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &ledStripConfig()->colors[i]; + const hsvColor_t *color = &ledStripConfig()->colors[i]; sbufWriteU16(dst, color->h); sbufWriteU8(dst, color->s); sbufWriteU8(dst, color->v); @@ -1023,7 +1027,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_LED_STRIP_CONFIG: for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; sbufWriteU32(dst, *ledConfig); } break; @@ -1091,13 +1095,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #else sbufWriteU8(dst, 0); #endif - sbufWriteU8(dst, osdProfile()->units); - sbufWriteU8(dst, osdProfile()->rssi_alarm); - sbufWriteU16(dst, osdProfile()->cap_alarm); - sbufWriteU16(dst, osdProfile()->time_alarm); - sbufWriteU16(dst, osdProfile()->alt_alarm); + sbufWriteU8(dst, osdConfig()->units); + sbufWriteU8(dst, osdConfig()->rssi_alarm); + sbufWriteU16(dst, osdConfig()->cap_alarm); + sbufWriteU16(dst, osdConfig()->time_alarm); + sbufWriteU16(dst, osdConfig()->alt_alarm); for (int i = 0; i < OSD_ITEM_COUNT; i++) { - sbufWriteU16(dst, osdProfile()->item_pos[i]); + sbufWriteU16(dst, osdConfig()->item_pos[i]); } #else sbufWriteU8(dst, 0); // OSD not supported @@ -1146,6 +1150,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); //!!TODO gyro_isr_update to be added pending decision //sbufWriteU8(dst, gyroConfig()->gyro_isr_update); + sbufWriteU8(dst, motorConfig()->motorPwmInversion); break; case MSP_FILTER_CONFIG : @@ -1316,12 +1321,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif break; case MSP_SET_ACC_TRIM: - accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src); - accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src); + accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src); + accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src); break; case MSP_SET_ARMING_CONFIG: - armingConfig()->auto_disarm_delay = sbufReadU8(src); - armingConfig()->disarm_kill_switch = sbufReadU8(src); + armingConfigMutable()->auto_disarm_delay = sbufReadU8(src); + armingConfigMutable()->disarm_kill_switch = sbufReadU8(src); break; case MSP_SET_LOOP_TIME: @@ -1343,7 +1348,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_MODE_RANGE: i = sbufReadU8(src); if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i]; + modeActivationCondition_t *mac = modeActivationConditionsMutable(i); i = sbufReadU8(src); const box_t *box = findBoxByPermenantId(i); if (box) { @@ -1352,7 +1357,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), ¤tProfile->pidProfile); } else { return MSP_RESULT_ERROR; } @@ -1364,7 +1369,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_ADJUSTMENT_RANGE: i = sbufReadU8(src); if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + adjustmentRange_t *adjRange = adjustmentRangesMutable(i); i = sbufReadU8(src); if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { adjRange->adjustmentIndex = i; @@ -1407,32 +1412,32 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_MISC: - rxConfig()->midrc = sbufReadU16(src); - motorConfig()->minthrottle = sbufReadU16(src); - motorConfig()->maxthrottle = sbufReadU16(src); - motorConfig()->mincommand = sbufReadU16(src); + rxConfigMutable()->midrc = sbufReadU16(src); + motorConfigMutable()->minthrottle = sbufReadU16(src); + motorConfigMutable()->maxthrottle = sbufReadU16(src); + motorConfigMutable()->mincommand = sbufReadU16(src); - failsafeConfig()->failsafe_throttle = sbufReadU16(src); + failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); #ifdef GPS - gpsConfig()->provider = sbufReadU8(src); // gps_type + gpsConfigMutable()->provider = sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate - gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas + gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas #else sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate sbufReadU8(src); // gps_ubx_sbas #endif - batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src); - rxConfig()->rssi_channel = sbufReadU8(src); + batteryConfigMutable()->multiwiiCurrentMeterOutput = sbufReadU8(src); + rxConfigMutable()->rssi_channel = sbufReadU8(src); sbufReadU8(src); - compassConfig()->mag_declination = sbufReadU16(src) * 10; + compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; - batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended - batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert break; case MSP_SET_MOTOR: @@ -1450,14 +1455,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= MAX_SUPPORTED_SERVOS) { return MSP_RESULT_ERROR; } else { - servoProfile()->servoConf[i].min = sbufReadU16(src); - servoProfile()->servoConf[i].max = sbufReadU16(src); - servoProfile()->servoConf[i].middle = sbufReadU16(src); - servoProfile()->servoConf[i].rate = sbufReadU8(src); - servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src); - servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src); - servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src); - servoProfile()->servoConf[i].reversedSources = sbufReadU32(src); + servoParamsMutable(i)->min = sbufReadU16(src); + servoParamsMutable(i)->max = sbufReadU16(src); + servoParamsMutable(i)->middle = sbufReadU16(src); + servoParamsMutable(i)->rate = sbufReadU8(src); + servoParamsMutable(i)->angleAtMin = sbufReadU8(src); + servoParamsMutable(i)->angleAtMax = sbufReadU8(src); + servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src); + servoParamsMutable(i)->reversedSources = sbufReadU32(src); } #endif break; @@ -1468,76 +1473,80 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= MAX_SERVO_RULES) { return MSP_RESULT_ERROR; } else { - customServoMixer(i)->targetChannel = sbufReadU8(src); - customServoMixer(i)->inputSource = sbufReadU8(src); - customServoMixer(i)->rate = sbufReadU8(src); - customServoMixer(i)->speed = sbufReadU8(src); - customServoMixer(i)->min = sbufReadU8(src); - customServoMixer(i)->max = sbufReadU8(src); - customServoMixer(i)->box = sbufReadU8(src); + customServoMixersMutable(i)->targetChannel = sbufReadU8(src); + customServoMixersMutable(i)->inputSource = sbufReadU8(src); + customServoMixersMutable(i)->rate = sbufReadU8(src); + customServoMixersMutable(i)->speed = sbufReadU8(src); + customServoMixersMutable(i)->min = sbufReadU8(src); + customServoMixersMutable(i)->max = sbufReadU8(src); + customServoMixersMutable(i)->box = sbufReadU8(src); loadCustomServoMixer(); } #endif break; case MSP_SET_3D: - flight3DConfig()->deadband3d_low = sbufReadU16(src); - flight3DConfig()->deadband3d_high = sbufReadU16(src); - flight3DConfig()->neutral3d = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_low = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_high = sbufReadU16(src); + flight3DConfigMutable()->neutral3d = sbufReadU16(src); break; case MSP_SET_RC_DEADBAND: - rcControlsConfig()->deadband = sbufReadU8(src); - rcControlsConfig()->yaw_deadband = sbufReadU8(src); - rcControlsConfig()->alt_hold_deadband = sbufReadU8(src); - flight3DConfig()->deadband3d_throttle = sbufReadU16(src); + rcControlsConfigMutable()->deadband = sbufReadU8(src); + rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); + rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src); + flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RESET_CURR_PID: resetProfile(currentProfile); break; case MSP_SET_SENSOR_ALIGNMENT: - gyroConfig()->gyro_align = sbufReadU8(src); - accelerometerConfig()->acc_align = sbufReadU8(src); - compassConfig()->mag_align = sbufReadU8(src); + gyroConfigMutable()->gyro_align = sbufReadU8(src); + accelerometerConfigMutable()->acc_align = sbufReadU8(src); + compassConfigMutable()->mag_align = sbufReadU8(src); break; case MSP_SET_ADVANCED_CONFIG: - gyroConfig()->gyro_sync_denom = sbufReadU8(src); - pidConfig()->pid_process_denom = sbufReadU8(src); - motorConfig()->useUnsyncedPwm = sbufReadU8(src); + gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src); + pidConfigMutable()->pid_process_denom = sbufReadU8(src); + motorConfigMutable()->useUnsyncedPwm = sbufReadU8(src); #ifdef USE_DSHOT - motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); + motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); #else - motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); + motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); #endif - motorConfig()->motorPwmRate = sbufReadU16(src); - if (dataSize > 7) { - motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; + motorConfigMutable()->motorPwmRate = sbufReadU16(src); + if (sbufBytesRemaining(src) >= 2) { + motorConfigMutable()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; } if (sbufBytesRemaining(src)) { - gyroConfig()->gyro_use_32khz = sbufReadU8(src); + gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src); } //!!TODO gyro_isr_update to be added pending decision /*if (sbufBytesRemaining(src)) { - gyroConfig()->gyro_isr_update = sbufReadU8(src); + gyroConfigMutable()->gyro_isr_update = sbufReadU8(src); }*/ validateAndFixGyroConfig(); + + if (sbufBytesRemaining(src)) { + motorConfigMutable()->motorPwmInversion = sbufReadU8(src); + } break; case MSP_SET_FILTER_CONFIG: - gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src); + gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src); currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src); currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src); if (dataSize > 5) { - gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src); - gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src); } if (dataSize > 13) { - gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src); - gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src); } // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); @@ -1567,9 +1576,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - accelerometerConfig()->acc_hardware = sbufReadU8(src); - barometerConfig()->baro_hardware = sbufReadU8(src); - compassConfig()->mag_hardware = sbufReadU8(src); + accelerometerConfigMutable()->acc_hardware = sbufReadU8(src); + barometerConfigMutable()->baro_hardware = sbufReadU8(src); + compassConfigMutable()->mag_hardware = sbufReadU8(src); break; case MSP_RESET_CONF: @@ -1601,9 +1610,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_BLACKBOX_CONFIG: // Don't allow config to be updated while Blackbox is logging if (blackboxMayEditConfig()) { - blackboxConfig()->device = sbufReadU8(src); - blackboxConfig()->rate_num = sbufReadU8(src); - blackboxConfig()->rate_denom = sbufReadU8(src); + blackboxConfigMutable()->device = sbufReadU8(src); + blackboxConfigMutable()->rate_num = sbufReadU8(src); + blackboxConfigMutable()->rate_denom = sbufReadU8(src); } break; #endif @@ -1759,85 +1768,85 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_BOARD_ALIGNMENT: - boardAlignment()->rollDegrees = sbufReadU16(src); - boardAlignment()->pitchDegrees = sbufReadU16(src); - boardAlignment()->yawDegrees = sbufReadU16(src); + boardAlignmentMutable()->rollDegrees = sbufReadU16(src); + boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); + boardAlignmentMutable()->yawDegrees = sbufReadU16(src); break; case MSP_SET_VOLTAGE_METER_CONFIG: - batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended - batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert if (dataSize > 4) { - batteryConfig()->batteryMeterType = sbufReadU8(src); + batteryConfigMutable()->batteryMeterType = sbufReadU8(src); } break; case MSP_SET_CURRENT_METER_CONFIG: - batteryConfig()->currentMeterScale = sbufReadU16(src); - batteryConfig()->currentMeterOffset = sbufReadU16(src); - batteryConfig()->currentMeterType = sbufReadU8(src); - batteryConfig()->batteryCapacity = sbufReadU16(src); + batteryConfigMutable()->currentMeterScale = sbufReadU16(src); + batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); + batteryConfigMutable()->currentMeterType = sbufReadU8(src); + batteryConfigMutable()->batteryCapacity = sbufReadU16(src); break; #ifndef USE_QUAD_MIXER_ONLY case MSP_SET_MIXER: - mixerConfig()->mixerMode = sbufReadU8(src); + mixerConfigMutable()->mixerMode = sbufReadU8(src); break; #endif case MSP_SET_RX_CONFIG: - rxConfig()->serialrx_provider = sbufReadU8(src); - rxConfig()->maxcheck = sbufReadU16(src); - rxConfig()->midrc = sbufReadU16(src); - rxConfig()->mincheck = sbufReadU16(src); - rxConfig()->spektrum_sat_bind = sbufReadU8(src); + rxConfigMutable()->serialrx_provider = sbufReadU8(src); + rxConfigMutable()->maxcheck = sbufReadU16(src); + rxConfigMutable()->midrc = sbufReadU16(src); + rxConfigMutable()->mincheck = sbufReadU16(src); + rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src); if (dataSize > 8) { - rxConfig()->rx_min_usec = sbufReadU16(src); - rxConfig()->rx_max_usec = sbufReadU16(src); + rxConfigMutable()->rx_min_usec = sbufReadU16(src); + rxConfigMutable()->rx_max_usec = sbufReadU16(src); } if (dataSize > 12) { - rxConfig()->rcInterpolation = sbufReadU8(src); - rxConfig()->rcInterpolationInterval = sbufReadU8(src); - rxConfig()->airModeActivateThreshold = sbufReadU16(src); + rxConfigMutable()->rcInterpolation = sbufReadU8(src); + rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src); + rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src); } if (dataSize > 16) { - rxConfig()->rx_spi_protocol = sbufReadU8(src); - rxConfig()->rx_spi_id = sbufReadU32(src); - rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src); + rxConfigMutable()->rx_spi_protocol = sbufReadU8(src); + rxConfigMutable()->rx_spi_id = sbufReadU32(src); + rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src); } if (dataSize > 22) { - rxConfig()->fpvCamAngleDegrees = sbufReadU8(src); + rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src); } break; case MSP_SET_FAILSAFE_CONFIG: - failsafeConfig()->failsafe_delay = sbufReadU8(src); - failsafeConfig()->failsafe_off_delay = sbufReadU8(src); - failsafeConfig()->failsafe_throttle = sbufReadU16(src); - failsafeConfig()->failsafe_kill_switch = sbufReadU8(src); - failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src); - failsafeConfig()->failsafe_procedure = sbufReadU8(src); + failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); + failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src); + failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src); + failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src); break; case MSP_SET_RXFAIL_CONFIG: i = sbufReadU8(src); if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { - rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src); - rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); + rxConfigMutable()->failsafe_channel_configurations[i].mode = sbufReadU8(src); + rxConfigMutable()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); } else { return MSP_RESULT_ERROR; } break; case MSP_SET_RSSI_CONFIG: - rxConfig()->rssi_channel = sbufReadU8(src); + rxConfigMutable()->rssi_channel = sbufReadU8(src); break; case MSP_SET_RX_MAP: for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - rxConfig()->rcmap[i] = sbufReadU8(src); + rxConfigMutable()->rcmap[i] = sbufReadU8(src); } break; @@ -1845,20 +1854,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_QUAD_MIXER_ONLY sbufReadU8(src); // mixerMode ignored #else - mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode + mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode #endif featureClearAll(); featureSet(sbufReadU32(src)); // features bitmap - rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type + rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type - boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll - boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch - boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw + boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll + boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch + boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw - batteryConfig()->currentMeterScale = sbufReadU16(src); - batteryConfig()->currentMeterOffset = sbufReadU16(src); + batteryConfigMutable()->currentMeterScale = sbufReadU16(src); + batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); break; case MSP_SET_CF_SERIAL_CONFIG: @@ -1892,7 +1901,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef LED_STRIP case MSP_SET_LED_COLORS: for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &ledStripConfig()->colors[i]; + hsvColor_t *color = &ledStripConfigMutable()->colors[i]; color->h = sbufReadU16(src); color->s = sbufReadU8(src); color->v = sbufReadU8(src); @@ -1905,7 +1914,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) { return MSP_RESULT_ERROR; } - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i]; *ledConfig = sbufReadU32(src); reevaluateLedConfig(); } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index ec12ed8090..ecbb07659d 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -27,6 +27,11 @@ #include "common/color.h" #include "common/utils.h" +#include "config/feature.h" +#include "config/config_profile.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/compass.h" @@ -43,8 +48,10 @@ #include "fc/cli.h" #include "fc/fc_dispatch.h" -#include "flight/pid.h" #include "flight/altitudehold.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/pid.h" #include "io/beeper.h" #include "io/dashboard.h" @@ -72,10 +79,6 @@ #include "telemetry/telemetry.h" -#include "config/feature.h" -#include "config/config_profile.h" -#include "config/config_master.h" - #ifdef USE_BST void taskBstMasterProcess(timeUs_t currentTimeUs); #endif @@ -94,7 +97,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - accUpdate(&accelerometerConfig()->accelerometerTrims); + accUpdate(&accelerometerConfigMutable()->accelerometerTrims); } static void taskHandleSerial(timeUs_t currentTimeUs) @@ -128,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs) if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTimeUs; - updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + updateCurrentMeter(ibatTimeSinceLastServiced); } } } @@ -201,7 +204,7 @@ static void taskTelemetry(timeUs_t currentTimeUs) telemetryCheckState(); if (!cliMode && feature(FEATURE_TELEMETRY)) { - telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + telemetryProcess(currentTimeUs); } } #endif diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c new file mode 100644 index 0000000000..62669bb66e --- /dev/null +++ b/src/main/fc/rc_adjustments.c @@ -0,0 +1,457 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "platform.h" + +#include "blackbox/blackbox.h" + +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/system.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "config/feature.h" +#include "config/config_master.h" + +#include "flight/pid.h" + +#include "io/beeper.h" +#include "io/motors.h" + +#include "fc/rc_adjustments.h" +#include "fc/rc_controls.h" +#include "fc/fc_rc.h" +#include "fc/config.h" + +#include "rx/rx.h" + +static pidProfile_t *pidProfile; + +static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) +{ +#ifndef BLACKBOX + UNUSED(adjustmentFunction); + UNUSED(newValue); +#else + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_inflightAdjustment_t eventData; + eventData.adjustmentFunction = adjustmentFunction; + eventData.newValue = newValue; + eventData.floatFlag = false; + blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); + } +#endif +} + +#if 0 +static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) +{ +#ifndef BLACKBOX + UNUSED(adjustmentFunction); + UNUSED(newFloatValue); +#else + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_inflightAdjustment_t eventData; + eventData.adjustmentFunction = adjustmentFunction; + eventData.newFloatValue = newFloatValue; + eventData.floatFlag = true; + blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); + } +#endif +} +#endif + +static uint8_t adjustmentStateMask = 0; + +#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex) +#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex) + +#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex)) + +// sync with adjustmentFunction_e +static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = { + { + .adjustmentFunction = ADJUSTMENT_RC_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_RC_EXPO, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, + .mode = ADJUSTMENT_MODE_SELECT, + .data = { .selectConfig = { .switchPositions = 3 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_D_SETPOINT, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + } +}; + +#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 + +static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; + +static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) +{ + adjustmentState_t *adjustmentState = &adjustmentStates[index]; + + if (adjustmentState->config == adjustmentConfig) { + // already configured + return; + } + adjustmentState->auxChannelIndex = auxSwitchChannelIndex; + adjustmentState->config = adjustmentConfig; + adjustmentState->timeoutAt = 0; + + MARK_ADJUSTMENT_FUNCTION_AS_READY(index); +} + +static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) +{ + int newValue; + + if (delta > 0) { + beeperConfirmationBeeps(2); + } else { + beeperConfirmationBeeps(1); + } + switch(adjustmentFunction) { + case ADJUSTMENT_RC_RATE: + newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcRate8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); + break; + case ADJUSTMENT_RC_EXPO: + newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcExpo8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); + break; + case ADJUSTMENT_THROTTLE_EXPO: + newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c + controlRateConfig->thrExpo8 = newValue; + generateThrottleCurve(); + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_RATE: + case ADJUSTMENT_PITCH_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_PITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE + case ADJUSTMENT_ROLL_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_ROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); + break; + case ADJUSTMENT_YAW_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + controlRateConfig->rates[FD_YAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_P: + case ADJUSTMENT_PITCH_P: + newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_P) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_P + case ADJUSTMENT_ROLL_P: + newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_I: + case ADJUSTMENT_PITCH_I: + newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_I) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_I + case ADJUSTMENT_ROLL_I: + newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_D: + case ADJUSTMENT_PITCH_D: + newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_D) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_D + case ADJUSTMENT_ROLL_D: + newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); + break; + case ADJUSTMENT_YAW_P: + newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); + break; + case ADJUSTMENT_YAW_I: + newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); + break; + case ADJUSTMENT_YAW_D: + newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); + break; + case ADJUSTMENT_RC_RATE_YAW: + newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcYawRate8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); + break; + case ADJUSTMENT_D_SETPOINT: + newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c + pidProfile->dtermSetpointWeight = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); + break; + case ADJUSTMENT_D_SETPOINT_TRANSITION: + newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c + pidProfile->setpointRelaxRatio = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); + break; + default: + break; + }; +} + +static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) +{ + bool applied = false; + + switch(adjustmentFunction) { + case ADJUSTMENT_RATE_PROFILE: + if (getCurrentControlRateProfile() != position) { + changeControlRateProfile(position); + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); + applied = true; + } + break; + } + + if (applied) { + beeperConfirmationBeeps(position + 1); + } +} + +#define RESET_FREQUENCY_2HZ (1000 / 2) + +void processRcAdjustments(controlRateConfig_t *controlRateConfig) +{ + const uint32_t now = millis(); + + const bool canUseRxData = rxIsReceivingSignal(); + + for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) { + adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex]; + + if (!adjustmentState->config) { + continue; + } + const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction; + if (adjustmentFunction == ADJUSTMENT_NONE) { + continue; + } + + const int32_t signedDiff = now - adjustmentState->timeoutAt; + const bool canResetReadyStates = signedDiff >= 0L; + + if (canResetReadyStates) { + adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; + MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); + } + + if (!canUseRxData) { + continue; + } + + const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex; + + if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { + int delta; + if (rcData[channelIndex] > rxConfig()->midrc + 200) { + delta = adjustmentState->config->data.stepConfig.step; + } else if (rcData[channelIndex] < rxConfig()->midrc - 200) { + delta = 0 - adjustmentState->config->data.stepConfig.step; + } else { + // returning the switch to the middle immediately resets the ready state + MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); + adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; + continue; + } + if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) { + continue; + } + + applyStepAdjustment(controlRateConfig,adjustmentFunction,delta); + pidInitConfig(pidProfile); + } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { + const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); + const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; + applySelectAdjustment(adjustmentFunction, position); + } + MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex); + } +} + +void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) +{ + for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { + adjustmentRange_t *adjustmentRange = &adjustmentRanges[index]; + if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { + const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; + configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); + } + } +} + +void resetAdjustmentStates(void) +{ + memset(adjustmentStates, 0, sizeof(adjustmentStates)); +} + +void useAdjustmentConfig(pidProfile_t *pidProfileToUse) +{ + pidProfile = pidProfileToUse; +} diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h new file mode 100644 index 0000000000..ad3fc47451 --- /dev/null +++ b/src/main/fc/rc_adjustments.h @@ -0,0 +1,116 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#include +#include "config/parameter_group.h" +#include "fc/rc_controls.h" + +typedef enum { + ADJUSTMENT_NONE = 0, + ADJUSTMENT_RC_RATE, + ADJUSTMENT_RC_EXPO, + ADJUSTMENT_THROTTLE_EXPO, + ADJUSTMENT_PITCH_ROLL_RATE, + ADJUSTMENT_YAW_RATE, + ADJUSTMENT_PITCH_ROLL_P, + ADJUSTMENT_PITCH_ROLL_I, + ADJUSTMENT_PITCH_ROLL_D, + ADJUSTMENT_YAW_P, + ADJUSTMENT_YAW_I, + ADJUSTMENT_YAW_D, + ADJUSTMENT_RATE_PROFILE, + ADJUSTMENT_PITCH_RATE, + ADJUSTMENT_ROLL_RATE, + ADJUSTMENT_PITCH_P, + ADJUSTMENT_PITCH_I, + ADJUSTMENT_PITCH_D, + ADJUSTMENT_ROLL_P, + ADJUSTMENT_ROLL_I, + ADJUSTMENT_ROLL_D, + ADJUSTMENT_RC_RATE_YAW, + ADJUSTMENT_D_SETPOINT, + ADJUSTMENT_D_SETPOINT_TRANSITION, + ADJUSTMENT_FUNCTION_COUNT +} adjustmentFunction_e; + + +typedef enum { + ADJUSTMENT_MODE_STEP, + ADJUSTMENT_MODE_SELECT +} adjustmentMode_e; + +typedef struct adjustmentStepConfig_s { + uint8_t step; +} adjustmentStepConfig_t; + +typedef struct adjustmentSelectConfig_s { + uint8_t switchPositions; +} adjustmentSelectConfig_t; + +typedef union adjustmentConfig_u { + adjustmentStepConfig_t stepConfig; + adjustmentSelectConfig_t selectConfig; +} adjustmentData_t; + +typedef struct adjustmentConfig_s { + uint8_t adjustmentFunction; + uint8_t mode; + adjustmentData_t data; +} adjustmentConfig_t; + +typedef struct adjustmentRange_s { + // when aux channel is in range... + uint8_t auxChannelIndex; + channelRange_t range; + + // ..then apply the adjustment function to the auxSwitchChannel ... + uint8_t adjustmentFunction; + uint8_t auxSwitchChannelIndex; + + // ... via slot + uint8_t adjustmentIndex; +} adjustmentRange_t; + +#define ADJUSTMENT_INDEX_OFFSET 1 + +typedef struct adjustmentState_s { + uint8_t auxChannelIndex; + const adjustmentConfig_t *config; + uint32_t timeoutAt; +} adjustmentState_t; + + +#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT +#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel +#endif + +#define MAX_ADJUSTMENT_RANGE_COUNT 15 + +PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges); + +typedef struct adjustmentProfile_s { + adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; +} adjustmentProfile_t; + +void resetAdjustmentStates(void); +void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); +struct controlRateConfig_s; +void processRcAdjustments(struct controlRateConfig_s *controlRateConfig); +struct pidProfile_s; +void useAdjustmentConfig(struct pidProfile_s *pidProfileToUse); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0f8a79b01e..e7784610e3 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -31,6 +31,8 @@ #include "common/maths.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" @@ -59,7 +61,6 @@ #include "flight/failsafe.h" -static motorConfig_t *motorConfig; static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) @@ -73,37 +74,6 @@ bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } -void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { -#ifndef BLACKBOX -#define UNUSED(x) (void)(x) - UNUSED(adjustmentFunction); - UNUSED(newValue); -#else - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_inflightAdjustment_t eventData; - eventData.adjustmentFunction = adjustmentFunction; - eventData.newValue = newValue; - eventData.floatFlag = false; - blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); - } -#endif -} - -void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) { -#ifndef BLACKBOX - UNUSED(adjustmentFunction); - UNUSED(newFloatValue); -#else - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_inflightAdjustment_t eventData; - eventData.adjustmentFunction = adjustmentFunction; - eventData.newFloatValue = newFloatValue; - eventData.floatFlag = true; - blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); - } -#endif -} - bool isUsingSticksForArming(void) { return isUsingSticksToArm; @@ -114,20 +84,20 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(void) { if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle))) + if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) return THROTTLE_LOW; } else { - if (rcData[THROTTLE] < rxConfig->mincheck) + if (rcData[THROTTLE] < rxConfig()->mincheck) return THROTTLE_LOW; } return THROTTLE_HIGH; } -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) +void processRcStickPositions(throttleStatus_e throttleStatus) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos @@ -139,9 +109,9 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; - if (rcData[i] > rxConfig->mincheck) + if (rcData[i] > rxConfig()->mincheck) stTmp |= 0x80; // check for MIN - if (rcData[i] < rxConfig->maxcheck) + if (rcData[i] < rxConfig()->maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { @@ -168,7 +138,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; if (rcDisarmTicks > 3) { - if (disarm_kill_switch) { + if (armingConfig()->disarm_kill_switch) { mwDisarm(); } else if (throttleStatus == THROTTLE_LOW) { mwDisarm(); @@ -317,12 +287,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat } -bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId) +bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId) { uint8_t index; for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; + const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { return true; @@ -357,386 +327,13 @@ void updateActivatedModes(modeActivationCondition_t *modeActivationConditions) } } -uint8_t adjustmentStateMask = 0; - -#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex) -#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex) - -#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex)) - -// sync with adjustmentFunction_e -static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = { - { - .adjustmentFunction = ADJUSTMENT_RC_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_RC_EXPO, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, - .mode = ADJUSTMENT_MODE_SELECT, - .data = { .selectConfig = { .switchPositions = 3 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_D_SETPOINT, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - } -}; - -#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 - -adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; - -static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) { - adjustmentState_t *adjustmentState = &adjustmentStates[index]; - - if (adjustmentState->config == adjustmentConfig) { - // already configured - return; - } - adjustmentState->auxChannelIndex = auxSwitchChannelIndex; - adjustmentState->config = adjustmentConfig; - adjustmentState->timeoutAt = 0; - - MARK_ADJUSTMENT_FUNCTION_AS_READY(index); -} - -static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { - int newValue; - - if (delta > 0) { - beeperConfirmationBeeps(2); - } else { - beeperConfirmationBeeps(1); - } - switch(adjustmentFunction) { - case ADJUSTMENT_RC_RATE: - newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcRate8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); - break; - case ADJUSTMENT_RC_EXPO: - newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); - break; - case ADJUSTMENT_THROTTLE_EXPO: - newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c - controlRateConfig->thrExpo8 = newValue; - generateThrottleCurve(); - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_RATE: - case ADJUSTMENT_PITCH_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_PITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); - if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE - case ADJUSTMENT_ROLL_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_ROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); - break; - case ADJUSTMENT_YAW_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); - controlRateConfig->rates[FD_YAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_P: - case ADJUSTMENT_PITCH_P: - newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_P) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_P - case ADJUSTMENT_ROLL_P: - newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_I: - case ADJUSTMENT_PITCH_I: - newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_I) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_I - case ADJUSTMENT_ROLL_I: - newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_D: - case ADJUSTMENT_PITCH_D: - newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_D) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_D - case ADJUSTMENT_ROLL_D: - newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); - break; - case ADJUSTMENT_YAW_P: - newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); - break; - case ADJUSTMENT_YAW_I: - newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); - break; - case ADJUSTMENT_YAW_D: - newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); - break; - case ADJUSTMENT_RC_RATE_YAW: - newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcYawRate8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); - break; - case ADJUSTMENT_D_SETPOINT: - newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c - pidProfile->dtermSetpointWeight = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); - break; - case ADJUSTMENT_D_SETPOINT_TRANSITION: - newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c - pidProfile->setpointRelaxRatio = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); - break; - default: - break; - }; -} - -static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) -{ - bool applied = false; - - switch(adjustmentFunction) { - case ADJUSTMENT_RATE_PROFILE: - if (getCurrentControlRateProfile() != position) { - changeControlRateProfile(position); - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); - applied = true; - } - break; - } - - if (applied) { - beeperConfirmationBeeps(position + 1); - } -} - -#define RESET_FREQUENCY_2HZ (1000 / 2) - -void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig) -{ - uint8_t adjustmentIndex; - uint32_t now = millis(); - - bool canUseRxData = rxIsReceivingSignal(); - - - for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) { - adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex]; - - if (!adjustmentState->config) { - continue; - } - uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction; - if (adjustmentFunction == ADJUSTMENT_NONE) { - continue; - } - - int32_t signedDiff = now - adjustmentState->timeoutAt; - bool canResetReadyStates = signedDiff >= 0L; - - if (canResetReadyStates) { - adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; - MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); - } - - if (!canUseRxData) { - continue; - } - - uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex; - - if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { - int delta; - if (rcData[channelIndex] > rxConfig->midrc + 200) { - delta = adjustmentState->config->data.stepConfig.step; - } else if (rcData[channelIndex] < rxConfig->midrc - 200) { - delta = 0 - adjustmentState->config->data.stepConfig.step; - } else { - // returning the switch to the middle immediately resets the ready state - MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); - adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; - continue; - } - if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) { - continue; - } - - applyStepAdjustment(controlRateConfig,adjustmentFunction,delta); - pidInitConfig(pidProfile); - } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { - uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); - uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; - - applySelectAdjustment(adjustmentFunction, position); - } - MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex); - } -} - -void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) -{ - uint8_t index; - - for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { - adjustmentRange_t *adjustmentRange = &adjustmentRanges[index]; - - if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { - - const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; - - configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); - } - } -} - int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse) { - motorConfig = motorConfigToUse; pidProfile = pidProfileToUse; isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); } - -void resetAdjustmentStates(void) -{ - memset(adjustmentStates, 0, sizeof(adjustmentStates)); -} - diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index e9c88f874b..bd23a30752 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -19,6 +19,8 @@ #include +#include "config/parameter_group.h" + typedef enum { BOXARM = 0, BOXANGLE, @@ -51,6 +53,7 @@ typedef enum { BOXAIRMODE, BOX3DDISABLESWITCH, BOXFPVANGLEMIX, + BOXBLACKBOXERASE, CHECKBOX_ITEM_COUNT } boxId_e; @@ -140,6 +143,8 @@ typedef struct modeActivationCondition_s { channelRange_t range; } modeActivationCondition_t; +PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); + typedef struct modeActivationProfile_s { modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; } modeActivationProfile_t; @@ -158,6 +163,8 @@ typedef struct controlRateConfig_s { uint16_t tpa_breakpoint; // Breakpoint where TPA is activated } controlRateConfig_t; +//!!TODO PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); + extern int16_t rcCommand[4]; typedef struct rcControlsConfig_s { @@ -168,117 +175,39 @@ typedef struct rcControlsConfig_s { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) } rcControlsConfig_t; +PG_DECLARE(rcControlsConfig_t, rcControlsConfig); + +typedef struct flight3DConfig_s { + uint16_t deadband3d_low; // min 3d value + uint16_t deadband3d_high; // max 3d value + uint16_t neutral3d; // center 3d value + uint16_t deadband3d_throttle; // default throttle deadband from MIDRC +} flight3DConfig_t; + +PG_DECLARE(flight3DConfig_t, flight3DConfig); + typedef struct armingConfig_s { uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 } armingConfig_t; +PG_DECLARE(armingConfig_t, armingConfig); + bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -struct rxConfig_s; -throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +throttleStatus_e calculateThrottleStatus(void); +void processRcStickPositions(throttleStatus_e throttleStatus); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); - -typedef enum { - ADJUSTMENT_NONE = 0, - ADJUSTMENT_RC_RATE, - ADJUSTMENT_RC_EXPO, - ADJUSTMENT_THROTTLE_EXPO, - ADJUSTMENT_PITCH_ROLL_RATE, - ADJUSTMENT_YAW_RATE, - ADJUSTMENT_PITCH_ROLL_P, - ADJUSTMENT_PITCH_ROLL_I, - ADJUSTMENT_PITCH_ROLL_D, - ADJUSTMENT_YAW_P, - ADJUSTMENT_YAW_I, - ADJUSTMENT_YAW_D, - ADJUSTMENT_RATE_PROFILE, - ADJUSTMENT_PITCH_RATE, - ADJUSTMENT_ROLL_RATE, - ADJUSTMENT_PITCH_P, - ADJUSTMENT_PITCH_I, - ADJUSTMENT_PITCH_D, - ADJUSTMENT_ROLL_P, - ADJUSTMENT_ROLL_I, - ADJUSTMENT_ROLL_D, - ADJUSTMENT_RC_RATE_YAW, - ADJUSTMENT_D_SETPOINT, - ADJUSTMENT_D_SETPOINT_TRANSITION, - ADJUSTMENT_FUNCTION_COUNT -} adjustmentFunction_e; - - -typedef enum { - ADJUSTMENT_MODE_STEP, - ADJUSTMENT_MODE_SELECT -} adjustmentMode_e; - -typedef struct adjustmentStepConfig_s { - uint8_t step; -} adjustmentStepConfig_t; - -typedef struct adjustmentSelectConfig_s { - uint8_t switchPositions; -} adjustmentSelectConfig_t; - -typedef union adjustmentConfig_u { - adjustmentStepConfig_t stepConfig; - adjustmentSelectConfig_t selectConfig; -} adjustmentData_t; - -typedef struct adjustmentConfig_s { - uint8_t adjustmentFunction; - uint8_t mode; - adjustmentData_t data; -} adjustmentConfig_t; - -typedef struct adjustmentRange_s { - // when aux channel is in range... - uint8_t auxChannelIndex; - channelRange_t range; - - // ..then apply the adjustment function to the auxSwitchChannel ... - uint8_t adjustmentFunction; - uint8_t auxSwitchChannelIndex; - - // ... via slot - uint8_t adjustmentIndex; -} adjustmentRange_t; - -#define ADJUSTMENT_INDEX_OFFSET 1 - -typedef struct adjustmentState_s { - uint8_t auxChannelIndex; - const adjustmentConfig_t *config; - uint32_t timeoutAt; -} adjustmentState_t; - - -#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel -#endif - -#define MAX_ADJUSTMENT_RANGE_COUNT 15 - -typedef struct adjustmentProfile_s { - adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; -} adjustmentProfile_t; - bool isAirmodeActive(void); -void resetAdjustmentStates(void); -void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); -bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); +bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; -struct motorConfig_s; -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 954ae59b11..54b3aa2310 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -21,25 +21,26 @@ #include #include - #include "platform.h" + #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/maths.h" -#include "sensors/barometer.h" -#include "sensors/sonar.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/pid.h" #include "rx/rx.h" -#include "fc/rc_controls.h" -#include "io/motors.h" - -#include "flight/pid.h" -#include "flight/imu.h" - -#include "fc/runtime_config.h" +#include "sensors/barometer.h" +#include "sensors/sonar.h" int32_t setVelocity = 0; @@ -50,22 +51,11 @@ int32_t AltHold; int32_t vario = 0; // variometer in cm/s -static barometerConfig_t *barometerConfig; static pidProfile_t *pidProfile; -static rcControlsConfig_t *rcControlsConfig; -static motorConfig_t *motorConfig; -void configureAltitudeHold( - pidProfile_t *initialPidProfile, - barometerConfig_t *intialBarometerConfig, - rcControlsConfig_t *initialRcControlsConfig, - motorConfig_t *initialMotorConfig -) +void configureAltitudeHold(pidProfile_t *initialPidProfile) { pidProfile = initialPidProfile; - barometerConfig = intialBarometerConfig; - rcControlsConfig = initialRcControlsConfig; - motorConfig = initialMotorConfig; } #if defined(BARO) || defined(SONAR) @@ -82,12 +72,12 @@ static void applyMultirotorAltHold(void) { static uint8_t isAltHoldChanged = 0; // multirotor alt hold - if (rcControlsConfig->alt_hold_fast_change) { + if (rcControlsConfig()->alt_hold_fast_change) { // rapid alt changes - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { errorVelocityI = 0; isAltHoldChanged = 1; - rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband; + rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband; } else { if (isAltHoldChanged) { AltHold = EstAlt; @@ -97,7 +87,7 @@ static void applyMultirotorAltHold(void) } } else { // slow alt changes, mostly used for aerial photography - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2; velocityControl = 1; @@ -111,19 +101,19 @@ static void applyMultirotorAltHold(void) } } -static void applyFixedWingAltHold(airplaneConfig_t *airplaneConfig) +static void applyFixedWingAltHold(void) { // handle fixedwing-related althold. UNTESTED! and probably wrong // most likely need to check changes on pitch channel and 'reset' althold similar to // how throttle does it on multirotor - rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig->fixedwing_althold_dir; + rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig()->fixedwing_althold_dir; } -void applyAltHold(airplaneConfig_t *airplaneConfig) +void applyAltHold(void) { if (STATE(FIXED_WING)) { - applyFixedWingAltHold(airplaneConfig); + applyFixedWingAltHold(); } else { applyMultirotorAltHold(); } @@ -272,7 +262,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) + accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; #ifdef DEBUG_ALT_HOLD @@ -308,7 +298,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay - vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel); + vel = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel); vel_tmp = lrintf(vel); // set vario diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 3201f71b9a..8dceceb093 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -23,13 +23,9 @@ extern int32_t vario; void calculateEstimatedAltitude(timeUs_t currentTimeUs); struct pidProfile_s; -struct barometerConfig_s; -struct rcControlsConfig_s; -struct motorConfig_s; -void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct motorConfig_s *initialMotorConfig); +void configureAltitudeHold(struct pidProfile_s *initialPidProfile); -struct airplaneConfig_s; -void applyAltHold(struct airplaneConfig_s *airplaneConfig); +void applyAltHold(void); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index c0603b4f01..ba604c309c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -24,6 +24,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "fc/config.h" @@ -40,9 +43,9 @@ /* * Usage: * - * failsafeInit() and useFailsafeConfig() must be called before the other methods are used. + * failsafeInit() and resetFailsafe() must be called before the other methods are used. * - * failsafeInit() and useFailsafeConfig() can be called in any order. + * failsafeInit() and resetFailsafe() can be called in any order. * failsafeInit() should only be called once. * * enable() should be called after system initialisation. @@ -50,15 +53,12 @@ static failsafeState_t failsafeState; -static failsafeConfig_t *failsafeConfig; - -static rxConfig_t *rxConfig; - -static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC - -static void failsafeReset(void) +/* + * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. + */ +void failsafeReset(void) { - failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; failsafeState.validRxDataReceivedAt = 0; failsafeState.validRxDataFailedAt = 0; failsafeState.throttleLowPeriod = 0; @@ -69,20 +69,8 @@ static void failsafeReset(void) failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; } -/* - * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. - */ -void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse) +void failsafeInit(void) { - failsafeConfig = failsafeConfigToUse; - failsafeReset(); -} - -void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle) -{ - rxConfig = intialRxConfig; - - deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; failsafeState.monitoring = false; @@ -119,7 +107,7 @@ static void failsafeActivate(void) failsafeState.active = true; failsafeState.phase = FAILSAFE_LANDING; ENABLE_FLIGHT_MODE(FAILSAFE_MODE); - failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; failsafeState.events++; } @@ -127,9 +115,9 @@ static void failsafeActivate(void) static void failsafeApplyControlInput(void) { for (int i = 0; i < 3; i++) { - rcData[i] = rxConfig->midrc; + rcData[i] = rxConfig()->midrc; } - rcData[THROTTLE] = failsafeConfig->failsafe_throttle; + rcData[THROTTLE] = failsafeConfig()->failsafe_throttle; } bool failsafeIsReceivingRxData(void) @@ -189,11 +177,11 @@ void failsafeUpdateState(void) case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time - if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) { - failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; + if (THROTTLE_HIGH == calculateThrottleStatus()) { + failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) - if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) { + if (failsafeSwitchIsOn && failsafeConfig()->failsafe_kill_switch) { // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure @@ -226,7 +214,7 @@ void failsafeUpdateState(void) if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; } else { - switch (failsafeConfig->failsafe_procedure) { + switch (failsafeConfig()->failsafe_procedure) { default: case FAILSAFE_PROCEDURE_AUTO_LANDING: // Stabilize, and set Throttle to specified level @@ -288,7 +276,7 @@ void failsafeUpdateState(void) // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. // This is to prevent that JustDisarm is activated on the next iteration. // Because that would have the effect of shutting down failsafe handling on intermittent connections. - failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; DISABLE_FLIGHT_MODE(FAILSAFE_MODE); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d166e1c360..d11d61421a 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + #define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5) #define MILLIS_PER_TENTH_SECOND 100 #define MILLIS_PER_SECOND 1000 @@ -36,6 +38,8 @@ typedef struct failsafeConfig_s { uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it } failsafeConfig_t; +PG_DECLARE(failsafeConfig_t, failsafeConfig); + typedef enum { FAILSAFE_IDLE = 0, FAILSAFE_RX_LOSS_DETECTED, @@ -70,9 +74,8 @@ typedef struct failsafeState_s { failsafeRxLinkState_e rxLinkState; } failsafeState_t; -struct rxConfig_s; -void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle); -void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse); +void failsafeInit(void); +void failsafeReset(void); void failsafeStartMonitoring(void); void failsafeUpdateState(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a25d992285..fadaf5e782 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -30,6 +30,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "sensors/sensors.h" diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9525c251ea..0deec6c89d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -21,6 +21,8 @@ #include "common/maths.h" #include "common/time.h" +#include "config/parameter_group.h" + #include "sensors/acceleration.h" // Exported symbols @@ -30,11 +32,6 @@ extern float accVelScale; extern int32_t accSum[XYZ_AXIS_COUNT]; -#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) -#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) -#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) -#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f) - typedef union { int16_t raw[XYZ_AXIS_COUNT]; struct { @@ -57,6 +54,8 @@ typedef struct throttleCorrectionConfig_s { uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. } throttleCorrectionConfig_t; +PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig); + typedef struct imuConfig_s { uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) @@ -65,6 +64,8 @@ typedef struct imuConfig_s { accDeadband_t accDeadband; } imuConfig_t; +PG_DECLARE(imuConfig_t, imuConfig); + typedef struct imuRuntimeConfig_s { float dcm_ki; float dcm_kp; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index accc55505e..fd36ad76ea 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -28,6 +28,10 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/pwm_output.h" @@ -46,9 +50,6 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "config/feature.h" -#include "config/config_master.h" - #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 @@ -61,11 +62,7 @@ static float motorMixRange; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; -static mixerConfig_t *mixerConfig; -static flight3DConfig_t *flight3DConfig; -static motorConfig_t *motorConfig; static airplaneConfig_t *airplaneConfig; -rxConfig_t *rxConfig; mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -253,7 +250,7 @@ float getMotorMixRange() bool isMotorProtocolDshot(void) { #ifdef USE_DSHOT - switch(motorConfig->motorPwmProtocol) { + switch(motorConfig()->motorPwmProtocol) { case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: @@ -273,39 +270,30 @@ void initEscEndpoints(void) { if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; if (feature(FEATURE_3D)) - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); else - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; } else #endif { - disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand; - motorOutputLow = motorConfig->minthrottle; - motorOutputHigh = motorConfig->maxthrottle; - deadbandMotor3dHigh = flight3DConfig->deadband3d_high; - deadbandMotor3dLow = flight3DConfig->deadband3d_low; + disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; + motorOutputLow = motorConfig()->minthrottle; + motorOutputHigh = motorConfig()->maxthrottle; + deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; + deadbandMotor3dLow = flight3DConfig()->deadband3d_low; } - rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck); - rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle; - rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle; + rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck); + rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle; + rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; } -void mixerUseConfigs( - flight3DConfig_t *flight3DConfigToUse, - motorConfig_t *motorConfigToUse, - mixerConfig_t *mixerConfigToUse, - airplaneConfig_t *airplaneConfigToUse, - rxConfig_t *rxConfigToUse) +void mixerUseConfigs(airplaneConfig_t *airplaneConfigToUse) { - flight3DConfig = flight3DConfigToUse; - motorConfig = motorConfigToUse; - mixerConfig = mixerConfigToUse; airplaneConfig = airplaneConfigToUse; - rxConfig = rxConfigToUse; } void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) @@ -440,25 +428,25 @@ void mixTable(pidProfile_t *pidProfile) // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { - if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. + if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. - if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling + if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; - } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling + } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling motorOutputMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle; + throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive + } else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; - throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle; + throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; } else { // Deadband handling from positive to negative @@ -468,7 +456,7 @@ void mixTable(pidProfile_t *pidProfile) currentThrottleInputRange = rcCommandThrottleRange3dHigh; } } else { - throttle = rcCommand[THROTTLE] - rxConfig->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange; motorOutputMin = motorOutputLow; motorOutputMax = motorOutputHigh; @@ -484,7 +472,7 @@ void mixTable(pidProfile_t *pidProfile) } // Calculate voltage compensation - const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; + const float vbatCompensationFactor = pidProfile->vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f; // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; @@ -493,7 +481,7 @@ void mixTable(pidProfile_t *pidProfile) motorMix[i] = scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[ROLL] * currentMixer[i].roll + - scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction); + scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction); if (vbatCompensationFactor > 1.0f) { motorMix[i] *= vbatCompensationFactor; // Add voltage compensation @@ -541,7 +529,7 @@ void mixTable(pidProfile_t *pidProfile) // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { - if (((rcData[THROTTLE]) < rxConfig->mincheck)) { + if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { motor[i] = disarmMotorOutput; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8a84ea8acb..86fd8f3d2c 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -17,6 +17,9 @@ #pragma once +#include "config/parameter_group.h" +#include "drivers/io_types.h" + #define MAX_SUPPORTED_MOTORS 12 #define QUAD_MOTOR_COUNT 4 @@ -85,6 +88,8 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; +PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer); + // Custom mixer configuration typedef struct mixer_s { uint8_t motorCount; @@ -97,12 +102,21 @@ typedef struct mixerConfig_s { int8_t yaw_motor_direction; } mixerConfig_t; -typedef struct flight3DConfig_s { - uint16_t deadband3d_low; // min 3d value - uint16_t deadband3d_high; // max 3d value - uint16_t neutral3d; // center 3d value - uint16_t deadband3d_throttle; // default throttle deadband from MIDRC -} flight3DConfig_t; +PG_DECLARE(mixerConfig_t, mixerConfig); + +typedef struct motorConfig_s { + uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. + uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 + uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs + uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) + uint8_t motorPwmProtocol; // Pwm Protocol + uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation + uint8_t useUnsyncedPwm; + float digitalIdleOffsetPercent; + ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; +} motorConfig_t; + +PG_DECLARE(motorConfig_t, motorConfig); typedef struct airplaneConfig_s { int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign @@ -113,19 +127,12 @@ typedef struct airplaneConfig_s { extern const mixer_t mixers[]; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; - -struct motorConfig_s; struct rxConfig_s; uint8_t getMotorCount(); float getMotorMixRange(); -void mixerUseConfigs( - flight3DConfig_t *flight3DConfigToUse, - struct motorConfig_s *motorConfigToUse, - mixerConfig_t *mixerConfigToUse, - airplaneConfig_t *airplaneConfigToUse, - struct rxConfig_s *rxConfigToUse); +void mixerUseConfigs(airplaneConfig_t *airplaneConfigToUse); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index dffd456893..0b0229c3de 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -27,30 +27,33 @@ #include "build/debug.h" #include "common/axis.h" +#include "common/gps_conversion.h" #include "common/maths.h" #include "common/time.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "sensors/sensors.h" -#include "sensors/boardalignment.h" -#include "sensors/acceleration.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/pid.h" #include "io/beeper.h" #include "io/serial.h" #include "io/gps.h" -#include "flight/pid.h" -#include "flight/navigation.h" -#include "flight/gps_conversion.h" -#include "flight/imu.h" - #include "rx/rx.h" +#include "sensors/acceleration.h" +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" + extern int16_t magHold; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3ee6ff0963..fa59064ecc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -28,6 +28,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/fc_core.h" #include "fc/fc_rc.h" diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9c045245c6..0d68827eed 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -85,10 +85,14 @@ typedef struct pidProfile_s { float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms } pidProfile_t; +PG_DECLARE_PROFILE(pidProfile_t, pidProfile); + typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate } pidConfig_t; +PG_DECLARE(pidConfig_t, pidConfig); + union rollAndPitchTrims_u; void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 13ab2fc658..066c5db40e 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -28,6 +28,9 @@ #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/pwm_output.h" #include "drivers/system.h" @@ -47,7 +50,6 @@ #include "config/feature.h" extern mixerMode_e currentMixerMode; -extern rxConfig_t *rxConfig; static servoMixerConfig_t *servoMixerConfig; @@ -278,7 +280,7 @@ void writeServos(void) case MIXER_TRI: case MIXER_CUSTOM_TRI: - if (servoMixerConfig->tri_unarmed_servo) { + if (servoMixerConfig()->tri_unarmed_servo) { // if unarmed flag set, we always move servo pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); } else { @@ -348,7 +350,7 @@ STATIC_UNIT_TESTED void servoMixer(void) input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING; // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { input[INPUT_STABILIZED_YAW] *= -1; } } @@ -364,14 +366,14 @@ STATIC_UNIT_TESTED void servoMixer(void) // 2000 - 1500 = +500 // 1500 - 1500 = 0 // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; - input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; - input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; - input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; - input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; - input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; - input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; + input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig()->midrc; + input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig()->midrc; + input[INPUT_RC_YAW] = rcData[YAW] - rxConfig()->midrc; + input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc; + input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig()->midrc; + input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig()->midrc; + input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig()->midrc; + input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig()->midrc; for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) servo[i] = 0; @@ -442,7 +444,7 @@ void servoTable(void) servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { - if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { + if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { @@ -473,10 +475,10 @@ void filterServos(void) uint32_t startTime = micros(); #endif - if (servoMixerConfig->servo_lowpass_enable) { + if (servoMixerConfig()->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime); + biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig()->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index e53b840fb2..74e80e0cdc 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + #define MAX_SUPPORTED_SERVOS 8 // These must be consecutive, see 'reversedSources' @@ -87,6 +89,8 @@ typedef struct servoMixer_s { #define MAX_SERVO_SPEED UINT8_MAX #define MAX_SERVO_BOXES 3 +PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers); + // Custom mixer configuration typedef struct mixerRules_s { uint8_t servoRuleCount; @@ -94,6 +98,7 @@ typedef struct mixerRules_s { } mixerRules_t; typedef struct servoParam_s { + uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted int16_t min; // servo min int16_t max; // servo max int16_t middle; // servo middle @@ -101,8 +106,9 @@ typedef struct servoParam_s { uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED - uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted -} __attribute__ ((__packed__)) servoParam_t; +} servoParam_t; + +PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams); typedef struct servoMixerConfig_s{ uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed @@ -110,6 +116,8 @@ typedef struct servoMixerConfig_s{ int8_t servo_lowpass_enable; // enable/disable lowpass filter } servoMixerConfig_t; +//!!TODO PG_DECLARE(servoConfig_t, servoConfig); + typedef struct servoProfile_s { servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; } servoProfile_t; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 13eefa691a..6f0e0ce6ee 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -171,9 +171,9 @@ typedef struct beeperTableEntry_s { { BEEPER_ENTRY(BEEPER_ARMED, 15, beep_armedBeep, "ARMED") }, { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 16, NULL, "SYSTEM_INIT") }, { BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") }, - - { BEEPER_ENTRY(BEEPER_ALL, 18, NULL, "ALL") }, - { BEEPER_ENTRY(BEEPER_PREFERENCE, 19, NULL, "PREFERRED") }, + { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 18, beep_2shortBeeps, "BLACKBOX_ERASE") }, + { BEEPER_ENTRY(BEEPER_ALL, 19, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_PREFERENCE, 20, NULL, "PREFERRED") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 15a40eef5d..645ff45d71 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -41,7 +41,7 @@ typedef enum { BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on BEEPER_USB, // Some boards have beeper powered USB connected - + BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes BEEPER_ALL, // Turn ON or OFF all beeper conditions BEEPER_PREFERENCE // Save preferred beeper configuration // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index 79e1815dae..cb015b0987 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -24,7 +24,8 @@ #include "common/utils.h" -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/display.h" #include "drivers/max7456.h" diff --git a/src/main/io/gimbal.h b/src/main/io/gimbal.h index 914ce05ba5..969242fee3 100644 --- a/src/main/io/gimbal.h +++ b/src/main/io/gimbal.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + typedef enum { GIMBAL_MODE_NORMAL = 0, GIMBAL_MODE_MIXTILT = 1 @@ -27,3 +29,5 @@ typedef enum { typedef struct gimbalConfig_s { uint8_t mode; } gimbalConfig_t; + +PG_DECLARE(gimbalConfig_t, gimbalConfig); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index c4925d8f4e..75f5e3c84d 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -29,27 +29,32 @@ #include "build/build_config.h" #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/gps_conversion.h" +#include "common/maths.h" #include "common/utils.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/light_led.h" -#include "sensors/sensors.h" +#include "drivers/light_led.h" +#include "drivers/system.h" -#include "io/serial.h" #include "io/dashboard.h" #include "io/gps.h" - -#include "flight/gps_conversion.h" -#include "flight/pid.h" -#include "flight/navigation.h" +#include "io/serial.h" #include "fc/config.h" #include "fc/runtime_config.h" -#include "config/feature.h" +#include "flight/navigation.h" +#include "flight/pid.h" + +#include "sensors/sensors.h" #define LOG_ERROR '?' #define LOG_IGNORED '!' diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 484f7bf52a..f776de69e2 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -19,6 +19,8 @@ #include "common/time.h" +#include "config/parameter_group.h" + #define LAT 0 #define LON 1 @@ -68,6 +70,8 @@ typedef struct gpsConfig_s { gpsAutoBaud_e autoBaud; } gpsConfig_t; +PG_DECLARE(gpsConfig_t, gpsConfig); + typedef struct gpsCoordinateDDDMMmmmm_s { int16_t dddmm; int16_t mmmm; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 31734e9fd3..5b0074c9d5 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -31,6 +31,9 @@ #include "common/maths.h" #include "common/typeconversion.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/light_ws2811strip.h" #include "drivers/system.h" #include "drivers/serial.h" @@ -44,6 +47,11 @@ #include "common/axis.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -73,10 +81,6 @@ #include "telemetry/telemetry.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - /* PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 4362a3d555..f90f935792 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -19,6 +19,7 @@ #include "common/color.h" #include "common/time.h" +#include "config/parameter_group.h" #include "drivers/io_types.h" #define LED_MAX_STRIP_LENGTH 32 @@ -147,6 +148,8 @@ typedef struct ledStripConfig_s { ioTag_t ioTag; } ledStripConfig_t; +PG_DECLARE(ledStripConfig_t, ledStripConfig); + ledConfig_t *ledConfigs; hsvColor_t *colors; modeColorIndexes_t *modeColors; diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 8486d507d2..817e950356 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -20,13 +20,4 @@ #include "drivers/io_types.h" #include "flight/mixer.h" -typedef struct motorConfig_s { - uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. - uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 - uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs - uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) - uint8_t motorPwmProtocol; // Pwm Protocol - uint8_t useUnsyncedPwm; - float digitalIdleOffsetPercent; - ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; -} motorConfig_t; + diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9f494ecb5f..f31e4d72e4 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -31,12 +32,19 @@ #ifdef OSD +#include "blackbox/blackbox_io.h" + #include "build/debug.h" #include "build/version.h" #include "common/printf.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/max7456_symbols.h" #include "drivers/display.h" #include "drivers/system.h" @@ -48,20 +56,15 @@ #include "cms/cms_types.h" #include "cms/cms_menu_osd.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" #include "io/osd.h" - #include "io/vtx.h" - #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -610,6 +613,47 @@ static void osdUpdateStats(void) stats.max_altitude = baro.BaroAlt; } +static void osdGetBlackboxStatusString(char * buff, uint8_t len) +{ + bool storageDeviceIsWorking = false; + uint32_t storageUsed = 0; + uint32_t storageTotal = 0; + + switch (blackboxConfig()->device) + { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY); + if (storageDeviceIsWorking) { + storageTotal = sdcard_getMetadata()->numBlocks / 2000; + storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000); + } + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + storageDeviceIsWorking = flashfsIsReady(); + if (storageDeviceIsWorking) { + const flashGeometry_t *geometry = flashfsGetGeometry(); + storageTotal = geometry->totalSize / 1024; + storageUsed = flashfsGetOffset() / 1024; + } + break; +#endif + + default: + storageDeviceIsWorking = true; + } + + if (storageDeviceIsWorking) { + uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal; + snprintf(buff, len, "%d%%", storageUsedPercent); + } else { + snprintf(buff, len, "FAULT"); + } +} + static void osdShowStats(void) { uint8_t top = 2; @@ -650,6 +694,12 @@ static void osdShowStats(void) sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); displayWrite(osdDisplayPort, 22, top++, buff); + if (feature(FEATURE_BLACKBOX) && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { + displayWrite(osdDisplayPort, 2, top, "BLACKBOX :"); + osdGetBlackboxStatusString(buff, 10); + displayWrite(osdDisplayPort, 22, top++, buff); + } + refreshTimeout = 60 * REFRESH_1S; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index c6539b42d7..00b6abc163 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define VISIBLE_FLAG 0x0800 #define VISIBLE(x) (x & VISIBLE_FLAG) @@ -67,6 +68,9 @@ typedef struct osd_profile_s { osd_unit_e units; } osd_profile_t; +// !!TODO change to osdConfig_t +PG_DECLARE(osd_profile_t, osdConfig); + struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); void osdResetConfig(osd_profile_t *osdProfile); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 6c96238aae..cb12d7ff82 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2) @@ -42,7 +45,8 @@ #endif #include "io/serial.h" -#include "fc/cli.h" // for cliEnter() + +#include "fc/cli.h" #include "msp/msp_serial.h" @@ -50,7 +54,6 @@ #include "telemetry/telemetry.h" #endif -static serialConfig_t *serialConfig; static serialPortUsage_t serialPortUsageList[SERIAL_PORT_COUNT]; const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { @@ -157,7 +160,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function) { while (findSerialPortConfigState.lastIndex < SERIAL_PORT_COUNT) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[findSerialPortConfigState.lastIndex++]; + serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[findSerialPortConfigState.lastIndex++]; if (candidate->functionMask & function) { return candidate; @@ -170,7 +173,7 @@ typedef struct findSharedSerialPortState_s { uint8_t lastIndex; } findSharedSerialPortState_t; -portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function) +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) { if (!portConfig || (portConfig->functionMask & function) == 0) { return PORTSHARING_UNUSED; @@ -178,7 +181,7 @@ portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFun return portConfig->functionMask == function ? PORTSHARING_NOT_SHARED : PORTSHARING_SHARED; } -bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) { return (portConfig) && (portConfig->functionMask & sharedWithFunction) && (portConfig->functionMask & functionMask); } @@ -195,10 +198,10 @@ serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e s serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) { while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[findSharedSerialPortState.lastIndex++]; + const serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSharedSerialPortState.lastIndex++]; if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) { - serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); + const serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); if (!serialPortUsage) { continue; } @@ -215,7 +218,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX) #endif -bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) +bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) { UNUSED(serialConfigToCheck); /* @@ -229,9 +232,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) */ uint8_t mspPortCount = 0; - uint8_t index; - for (index = 0; index < SERIAL_PORT_COUNT; index++) { - serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index]; + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + const serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index]; if (portConfig->functionMask & FUNCTION_MSP) { mspPortCount++; @@ -265,9 +267,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier) { - uint8_t index; - for (index = 0; index < SERIAL_PORT_COUNT; index++) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[index]; + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[index]; if (candidate->identifier == identifier) { return candidate; } @@ -394,16 +395,12 @@ void closeSerialPort(serialPort_t *serialPort) serialPortUsage->serialPort = NULL; } -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) +void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) { - uint8_t index; - - serialConfig = initialSerialConfig; - serialPortCount = SERIAL_PORT_COUNT; memset(&serialPortUsageList, 0, sizeof(serialPortUsageList)); - for (index = 0; index < SERIAL_PORT_COUNT; index++) { + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { serialPortUsageList[index].identifier = serialPortIdentifiers[index]; if (serialPortToDisable != SERIAL_PORT_NONE) { @@ -469,7 +466,7 @@ void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar) cliEnter(serialPort); } #endif - if (receivedChar == serialConfig->reboot_character) { + if (receivedChar == serialConfig()->reboot_character) { systemResetToBootloader(); } } @@ -486,8 +483,7 @@ static void nopConsumer(uint8_t data) arbitrary serial passthrough "proxy". Optional callbacks can be given to allow for specialized data processing. */ -void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer - *leftC, serialConsumer *rightC) +void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC) { waitForSerialPortToFinishTransmitting(left); waitForSerialPortToFinishTransmitting(right); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 5a68a18041..0398665a8d 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -17,6 +17,10 @@ #pragma once +#include +#include + +#include "config/parameter_group.h" #include "drivers/serial.h" typedef enum { @@ -111,23 +115,25 @@ typedef struct serialConfig_s { uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. } serialConfig_t; +PG_DECLARE(serialConfig_t, serialConfig); + typedef void serialConsumer(uint8_t); // // configuration // -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); +void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); -bool isSerialConfigValid(serialConfig_t *serialConfig); +bool isSerialConfigValid(const serialConfig_t *serialConfig); serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier); bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier); serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function); -portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function); -bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function); +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier); int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier); diff --git a/src/main/io/servos.h b/src/main/io/servos.h index 711ceec755..804b77b0b0 100644 --- a/src/main/io/servos.h +++ b/src/main/io/servos.h @@ -26,3 +26,5 @@ typedef struct servoConfig_s { uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz) ioTag_t ioTags[MAX_SUPPORTED_SERVOS]; } servoConfig_t; + +PG_DECLARE(servoConfig_t, servoConfig); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 8db4c4279b..eb44e3f0b4 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -26,10 +26,14 @@ #include "io/osd.h" //External dependencies -#include "config/config_master.h" #include "config/config_eeprom.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/vtx_rtc6705.h" + #include "fc/runtime_config.h" + #include "io/beeper.h" diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 9d675c721f..c5e6467a21 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -19,32 +19,36 @@ #include #include +#include #include #include "platform.h" #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) +#include "build/build_config.h" + #include "cms/cms.h" #include "cms/cms_types.h" -#include "string.h" #include "common/printf.h" #include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/vtx_common.h" -#include "io/serial.h" -#include "io/vtx_smartaudio.h" -#include "io/vtx_string.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "flight/pid.h" -#include "config/config_master.h" -#include "build/build_config.h" +#include "io/serial.h" +#include "io/vtx_smartaudio.h" +#include "io/vtx_string.h" //#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 63d2b70bdb..d2c368f28a 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 31 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MINOR 32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) #define API_VERSION_LENGTH 2 diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index ba85e9bd55..72b236e1a4 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -407,10 +407,8 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uin ----------------------------------------------- */ -void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initJetiExBusTelemetry(void) { - UNUSED(initialTelemetryConfig); - // Init Ex Bus Frame header jetiExBusTelemetryFrame[EXBUS_HEADER_SYNC] = 0x3B; // Startbytes jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01; @@ -427,7 +425,6 @@ void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig) jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 } - void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor) { uint8_t labelLength = strlen(sensor->label); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 897e8bc0bd..adf8939f7c 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -30,6 +30,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/adc.h" #include "drivers/rx_pwm.h" @@ -90,7 +92,6 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) rxRuntimeConfig_t rxRuntimeConfig; -static const rxConfig_t *rxConfig; static uint8_t rcSampleIndex = 0; static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) @@ -108,7 +109,7 @@ static uint8_t nullFrameStatus(void) void useRxConfig(const rxConfig_t *rxConfigToUse) { - rxConfig = rxConfigToUse; + (void)(rxConfigToUse); } #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels @@ -126,8 +127,8 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void) STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration) { - return pulseDuration >= rxConfig->rx_min_usec && - pulseDuration <= rxConfig->rx_max_usec; + return pulseDuration >= rxConfig()->rx_min_usec && + pulseDuration <= rxConfig()->rx_max_usec; } // pulse duration is in micro seconds (usec) @@ -204,20 +205,20 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig } #endif -void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions) +void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t *modeActivationConditions) { - useRxConfig(rxConfig); + useRxConfig(initialRxConfig); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rcSampleIndex = 0; needRxSignalMaxDelayUs = DELAY_10_HZ; for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcData[i] = rxConfig->midrc; + rcData[i] = rxConfig()->midrc; rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; } - rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec; + rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; // Initialize ARM switch to OFF position when arming via switch is defined for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -237,7 +238,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { - const bool enabled = serialRxInit(rxConfig, &rxRuntimeConfig); + const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SERIAL); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -248,14 +249,14 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef USE_RX_MSP if (feature(FEATURE_RX_MSP)) { - rxMspInit(rxConfig, &rxRuntimeConfig); + rxMspInit(rxConfig(), &rxRuntimeConfig); needRxSignalMaxDelayUs = DELAY_5_HZ; } #endif #ifdef USE_RX_SPI if (feature(FEATURE_RX_SPI)) { - const bool enabled = rxSpiInit(rxConfig, &rxRuntimeConfig); + const bool enabled = rxSpiInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SPI); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -266,7 +267,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #if defined(USE_PWM) || defined(USE_PPM) if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { - rxPwmInit(rxConfig, &rxRuntimeConfig); + rxPwmInit(rxConfig(), &rxRuntimeConfig); } #endif } @@ -376,7 +377,7 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) static uint16_t getRxfailValue(uint8_t channel) { - const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; + const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig()->failsafe_channel_configurations[channel]; switch(channelFailsafeConfiguration->mode) { case RX_FAILSAFE_MODE_AUTO: @@ -384,12 +385,12 @@ static uint16_t getRxfailValue(uint8_t channel) case ROLL: case PITCH: case YAW: - return rxConfig->midrc; + return rxConfig()->midrc; case THROTTLE: if (feature(FEATURE_3D)) - return rxConfig->midrc; + return rxConfig()->midrc; else - return rxConfig->rx_min_usec; + return rxConfig()->rx_min_usec; } /* no break */ @@ -420,7 +421,7 @@ static uint8_t getRxChannelCount(void) { static uint8_t maxChannelsAllowed; if (!maxChannelsAllowed) { - uint8_t maxChannels = rxConfig->max_aux_channel + NON_AUX_CHANNEL_COUNT; + uint8_t maxChannels = rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT; if (maxChannels > rxRuntimeConfig.channelCount) { maxChannelsAllowed = rxRuntimeConfig.channelCount; } else { @@ -436,14 +437,14 @@ static void readRxChannelsApplyRanges(void) const int channelCount = getRxChannelCount(); for (int channel = 0; channel < channelCount; channel++) { - const uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel); // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { - sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]); + sample = applyRxChannelRangeConfiguraton(sample, rxConfig()->channelRanges[channel]); } rcRaw[channel] = sample; @@ -548,10 +549,10 @@ static void updateRSSIPWM(void) { int16_t pwmRssi = 0; // Read value of AUX channel as rssi - pwmRssi = rcData[rxConfig->rssi_channel - 1]; + pwmRssi = rcData[rxConfig()->rssi_channel - 1]; // RSSI_Invert option - if (rxConfig->rssi_ppm_invert) { + if (rxConfig()->rssi_ppm_invert) { pwmRssi = ((2000 - pwmRssi) + 1000); } @@ -578,7 +579,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) int16_t adcRssiMean = 0; uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale; + uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale; adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT; @@ -599,7 +600,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) void updateRSSI(timeUs_t currentTimeUs) { - if (rxConfig->rssi_channel > 0) { + if (rxConfig()->rssi_channel > 0) { updateRSSIPWM(); } else if (feature(FEATURE_RSSI_ADC)) { updateRSSIADC(currentTimeUs); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 484d65914a..17f8d27763 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define STICK_CHANNEL_COUNT 4 @@ -105,11 +106,17 @@ typedef struct rxFailsafeChannelConfiguration_s { uint8_t step; } rxFailsafeChannelConfiguration_t; +//!!TODO change to rxFailsafeChannelConfig_t +PG_DECLARE_ARRAY(rxFailsafeChannelConfiguration_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs); + typedef struct rxChannelRangeConfiguration_s { uint16_t min; uint16_t max; } rxChannelRangeConfiguration_t; +//!!TODO change to rxChannelRangeConfig_t +PG_DECLARE_ARRAY(rxChannelRangeConfiguration_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs); + typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. @@ -139,6 +146,8 @@ typedef struct rxConfig_s { rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT]; } rxConfig_t; +PG_DECLARE(rxConfig_t, rxConfig); + #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) struct rxRuntimeConfig_s; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index ded23de3cf..0d9289ede8 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -27,6 +27,9 @@ #include "common/axis.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" @@ -243,13 +246,13 @@ retry: return true; } -bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval) +bool accInit(uint32_t gyroSamplingInverval) { memset(&acc, 0, sizeof(acc)); // copy over the common gyro mpu settings acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration; acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult; - if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) { + if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { return false; } acc.dev.acc_1G = 256; // set default diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index fda259e940..35de76374e 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/accgyro.h" #include "sensors/sensors.h" @@ -66,7 +67,9 @@ typedef struct accelerometerConfig_s { rollAndPitchTrims_t accelerometerTrims; } accelerometerConfig_t; -bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime); +PG_DECLARE(accelerometerConfig_t, accelerometerConfig); + +bool accInit(uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index bac86ad16c..4b25477dc5 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -23,6 +23,9 @@ #include "common/maths.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/barometer.h" #include "drivers/barometer_bmp085.h" #include "drivers/barometer_bmp280.h" @@ -51,8 +54,6 @@ static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; -static const barometerConfig_t *barometerConfig; - bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function @@ -119,11 +120,6 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) return true; } -void useBarometerConfig(const barometerConfig_t *barometerConfigToUse) -{ - barometerConfig = barometerConfigToUse; -} - bool isBaroCalibrationComplete(void) { return calibratingB == 0; @@ -160,7 +156,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading) return newPressureReading; } -#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1) +#define PRESSURE_SAMPLE_COUNT (barometerConfig()->baro_sample_count - 1) static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading) { @@ -213,7 +209,7 @@ uint32_t baroUpdate(void) baro.dev.get_up(); baro.dev.start_ut(); baro.dev.calculate(&baroPressure, &baroTemperature); - baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); + baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure); state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; break; @@ -228,7 +224,7 @@ int32_t baroCalculateAltitude(void) // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; - baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise + baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise return baro.BaroAlt; } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 536e6c453a..b753ef5eaa 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/barometer.h" typedef enum { @@ -37,6 +38,8 @@ typedef struct barometerConfig_s { float baro_cf_alt; // apply CF to use ACC for height estimation } barometerConfig_t; +PG_DECLARE(barometerConfig_t, barometerConfig); + typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; @@ -46,7 +49,6 @@ typedef struct baro_s { extern baro_t baro; bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse); -void useBarometerConfig(const barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); uint32_t baroUpdate(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index f79e581d2e..e54a0a1ae3 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -22,26 +22,23 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/filter.h" +#include "common/utils.h" + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/adc.h" #include "drivers/system.h" -#include "fc/config.h" #include "fc/runtime_config.h" -#include "config/feature.h" +#include "io/beeper.h" #include "sensors/battery.h" #include "sensors/esc_sensor.h" -#include "fc/rc_controls.h" -#include "io/beeper.h" - -#include "rx/rx.h" - -#include "common/utils.h" #define VBAT_LPF_FREQ 0.4f #define IBAT_LPF_FREQ 0.4f @@ -61,7 +58,7 @@ uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) uint16_t vbatLatest = 0; // most recent unsmoothed value int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) -int32_t amperageLatest = 0; // most recent value +int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start @@ -72,7 +69,7 @@ static uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier); + return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier); } static void updateBatteryVoltage(void) @@ -86,7 +83,7 @@ static void updateBatteryVoltage(void) } #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { + if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) { escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0; if (debugMode == DEBUG_BATTERY) { @@ -134,20 +131,20 @@ void updateBattery(void) uint16_t vBatMeasured = vbatLatest; /* battery has just been connected*/ - if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { + if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ vBatState = BATTERY_OK; - unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; + unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; } batteryCellCount = cells; - batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; - batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */ - } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { + batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; + batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */ + } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { vBatState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; @@ -159,16 +156,16 @@ void updateBattery(void) debug[3] = batteryCellCount; } - if (batteryConfig->useVBatAlerts) { + if (batteryConfig()->useVBatAlerts) { switch(vBatState) { case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_WARNING; } break; case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_CRITICAL; } else if (vbat > batteryWarningVoltage) { vBatState = BATTERY_OK; @@ -206,9 +203,8 @@ const char * getBatteryStateString(void) return batteryStateStrings[getBatteryState()]; } -void batteryInit(batteryConfig_t *initialBatteryConfig) +void batteryInit(void) { - batteryConfig = initialBatteryConfig; vBatState = BATTERY_NOT_PRESENT; consumptionState = BATTERY_OK; batteryCellCount = 0; @@ -221,9 +217,9 @@ static int32_t currentSensorToCentiamps(uint16_t src) int32_t millivolts; millivolts = ((uint32_t)src * ADCVREF) / 4096; - millivolts -= batteryConfig->currentMeterOffset; + millivolts -= batteryConfig()->currentMeterOffset; - return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps + return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps } static void updateBatteryCurrent(void) @@ -251,10 +247,10 @@ static void updateCurrentDrawn(int32_t lastUpdateAt) void updateConsumptionWarning(void) { - if (batteryConfig->useConsumptionAlerts && batteryConfig->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { + if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { if (calculateBatteryPercentage() == 0) { vBatState = BATTERY_CRITICAL; - } else if (calculateBatteryPercentage() <= batteryConfig->consumptionWarningPercentage) { + } else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) { consumptionState = BATTERY_WARNING; } else { consumptionState = BATTERY_OK; @@ -264,35 +260,29 @@ void updateConsumptionWarning(void) } } -void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void updateCurrentMeter(int32_t lastUpdateAt) { if (getBatteryState() != BATTERY_NOT_PRESENT) { - switch(batteryConfig->currentMeterType) { + switch(batteryConfig()->currentMeterType) { case CURRENT_SENSOR_ADC: updateBatteryCurrent(); - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_VIRTUAL: - amperageLatest = (int32_t)batteryConfig->currentMeterOffset; + amperageLatest = (int32_t)batteryConfig()->currentMeterOffset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + throttleStatus_e throttleStatus = calculateThrottleStatus(); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000; } amperage = amperageLatest; - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR @@ -328,7 +318,7 @@ float calculateVbatPidCompensation(void) { float batteryScaler = 1.0f; if (feature(FEATURE_VBAT) && batteryCellCount > 0) { // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference - batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); + batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); } return batteryScaler; } @@ -337,11 +327,11 @@ uint8_t calculateBatteryPercentage(void) { uint8_t batteryPercentage = 0; if (batteryCellCount > 0) { - uint16_t batteryCapacity = batteryConfig->batteryCapacity; + uint16_t batteryCapacity = batteryConfig()->batteryCapacity; if (batteryCapacity > 0) { batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100); } else { - batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); + batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100); } } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 3eb68940c8..91b59568ce 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,7 +17,7 @@ #pragma once -#include "common/maths.h" // for fix12_t +#include "config/parameter_group.h" #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 @@ -63,6 +63,8 @@ typedef struct batteryConfig_s { uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning } batteryConfig_t; +PG_DECLARE(batteryConfig_t, batteryConfig); + typedef enum { BATTERY_OK = 0, BATTERY_WARNING, @@ -81,11 +83,10 @@ extern int32_t mAhDrawn; batteryState_e getBatteryState(void); const char * getBatteryStateString(void); void updateBattery(void); -void batteryInit(batteryConfig_t *initialBatteryConfig); -batteryConfig_t *batteryConfig; +void batteryInit(void); struct rxConfig_s; -void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void updateCurrentMeter(int32_t lastUpdateAt); int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index c4f1988274..0b45b8ffe8 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -23,6 +23,9 @@ #include "common/maths.h" #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "boardalignment.h" diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index b1ab74cdf9..8c224f24e2 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -17,11 +17,15 @@ #pragma once +#include "config/parameter_group.h" + typedef struct boardAlignment_s { int32_t rollDegrees; int32_t pitchDegrees; int32_t yawDegrees; } boardAlignment_t; +PG_DECLARE(boardAlignment_t, boardAlignment); + void alignSensors(int32_t *dest, uint8_t rotation); void initBoardAlignment(const boardAlignment_t *boardAlignment); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 9cc1e29fc3..8e84e44b46 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -22,6 +22,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/compass.h" #include "drivers/compass_ak8975.h" #include "drivers/compass_ak8963.h" @@ -143,12 +146,12 @@ retry: return true; } -void compassInit(const compassConfig_t *compassConfig) +void compassInit(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // calculate magnetic declination - const int16_t deg = compassConfig->mag_declination / 100; - const int16_t min = compassConfig->mag_declination % 100; + const int16_t deg = compassConfig()->mag_declination / 100; + const int16_t min = compassConfig()->mag_declination % 100; mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units LED1_ON; mag.dev.init(); diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 4e534fea08..4f95e9db55 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -17,8 +17,8 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/compass.h" - #include "sensors/sensors.h" @@ -47,8 +47,10 @@ typedef struct compassConfig_s { flightDynamicsTrims_t magZero; } compassConfig_t; +PG_DECLARE(compassConfig_t, compassConfig); + bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); -void compassInit(const compassConfig_t *compassConfig); +void compassInit(void); union flightDynamicsTrims_u; void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero); diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index aad7b1ea4a..4fe0876ba1 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -21,26 +21,28 @@ #include -#include "fc/config.h" +#include "build/debug.h" + #include "config/feature.h" -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "common/utils.h" -#include "drivers/system.h" +#include "drivers/pwm_output.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" -#include "drivers/pwm_output.h" +#include "drivers/system.h" -#include "io/serial.h" +#include "esc_sensor.h" + +#include "fc/config.h" #include "flight/mixer.h" #include "sensors/battery.h" -#include "build/debug.h" - -#include "esc_sensor.h" +#include "io/serial.h" /* KISS ESC TELEMETRY PROTOCOL diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9732a96859..677bce6a84 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -28,6 +28,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" @@ -69,7 +72,6 @@ gyro_t gyro; // gyro access functions static int32_t gyroADC[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; -static const gyroConfig_t *gyroConfig; static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -81,6 +83,29 @@ static void *notchFilter2[3]; #define DEBUG_GYRO_CALIBRATION 3 +#ifdef STM32F10X +#define GYRO_SYNC_DENOM_DEFAULT 8 +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) +#define GYRO_SYNC_DENOM_DEFAULT 1 +#else +#define GYRO_SYNC_DENOM_DEFAULT 4 +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); + +PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, + .gyro_lpf = GYRO_LPF_256HZ, + .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT, + .gyro_soft_lpf_type = FILTER_PT1, + .gyro_soft_lpf_hz = 90, + .gyro_soft_notch_hz_1 = 400, + .gyro_soft_notch_cutoff_1 = 300, + .gyro_soft_notch_hz_2 = 200, + .gyro_soft_notch_cutoff_2 = 100, + .gyro_align = ALIGN_DEFAULT, + .gyroMovementCalibrationThreshold = 32 +); + static const extiConfig_t *selectMPUIntExtiConfig(void) { #if defined(MPU_INT_EXTI) @@ -231,14 +256,13 @@ static bool gyroDetect(gyroDev_t *dev) return true; } -bool gyroInit(const gyroConfig_t *gyroConfigToUse) +bool gyroInit(void) { - gyroConfig = gyroConfigToUse; memset(&gyro, 0, sizeof(gyro)); #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); mpuDetect(&gyro.dev); - mpuReset = gyro.dev.mpuConfiguration.reset; + mpuResetFn = gyro.dev.mpuConfiguration.resetFn; #endif if (!gyroDetect(&gyro.dev)) { @@ -248,8 +272,7 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) switch (detectedSensors[SENSOR_INDEX_GYRO]) { default: // gyro does not support 32kHz - // cast away constness, legitimate as this is cross-validation - ((gyroConfig_t*)gyroConfig)->gyro_use_32khz = false; + gyroConfigMutable()->gyro_use_32khz = false; break; case GYRO_MPU6500: case GYRO_MPU9250: @@ -261,8 +284,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) } // Must set gyro sample rate before initialisation - gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz); - gyro.dev.lpf = gyroConfig->gyro_lpf; + gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); + gyro.dev.lpf = gyroConfig()->gyro_lpf; gyro.dev.init(&gyro.dev); gyroInitFilters(); return true; @@ -280,43 +303,43 @@ void gyroInitFilters(void) notchFilter1ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply; - if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known - if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { + if (gyroConfig()->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known + if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } - } else if (gyroConfig->gyro_soft_lpf_type == FILTER_PT1) { + } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterPt1[axis]; - pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt); + pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt); } } else { softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroDenoiseState[axis]; - firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + firFilterDenoiseInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } } } - if (gyroConfig->gyro_soft_notch_hz_1) { + if (gyroConfig()->gyro_soft_notch_hz_1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); + const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + biquadFilterInit(notchFilter1[axis], gyroConfig()->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } - if (gyroConfig->gyro_soft_notch_hz_2) { + if (gyroConfig()->gyro_soft_notch_hz_2) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); + const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); + biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } @@ -441,7 +464,7 @@ void gyroUpdate(void) if (calibrationComplete) { #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) // SPI-based gyro so can read and update in ISR - if (gyroConfig->gyro_isr_update) { + if (gyroConfig()->gyro_isr_update) { mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR); return; } @@ -450,7 +473,7 @@ void gyroUpdate(void) debug[3] = (uint16_t)(micros() & 0xffff); #endif } else { - performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); + performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold); } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 1fdf8820a6..94e86269bb 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/accgyro.h" #include "common/axis.h" @@ -59,8 +60,10 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_cutoff_2; } gyroConfig_t; +PG_DECLARE(gyroConfig_t, gyroConfig); + void gyroSetCalibrationCycles(void); -bool gyroInit(const gyroConfig_t *gyroConfigToUse); +bool gyroInit(void); void gyroInitFilters(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 01b396b2c1..4220fab3be 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -52,50 +54,40 @@ static bool sonarDetect(void) } #endif -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accelerometerConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *barometerConfig, - const sonarConfig_t *sonarConfig) +bool sensorsAutodetect(void) { // gyro must be initialised before accelerometer - if (!gyroInit(gyroConfig)) { + if (!gyroInit()) { return false; } - accInit(accelerometerConfig, gyro.targetLooptime); + accInit(gyro.targetLooptime); mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG - if (compassDetect(&mag.dev, compassConfig->mag_hardware)) { - compassInit(compassConfig); + if (compassDetect(&mag.dev, compassConfig()->mag_hardware)) { + compassInit(); } -#else - UNUSED(compassConfig); #endif #ifdef BARO - baroDetect(&baro.dev, barometerConfig->baro_hardware); -#else - UNUSED(barometerConfig); + baroDetect(&baro.dev, barometerConfig()->baro_hardware); #endif #ifdef SONAR if (sonarDetect()) { - sonarInit(sonarConfig); + sonarInit(sonarConfig()); } -#else - UNUSED(sonarConfig); #endif - if (gyroConfig->gyro_align != ALIGN_DEFAULT) { - gyro.dev.gyroAlign = gyroConfig->gyro_align; + if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { + gyro.dev.gyroAlign = gyroConfig()->gyro_align; } - if (accelerometerConfig->acc_align != ALIGN_DEFAULT) { - acc.dev.accAlign = accelerometerConfig->acc_align; + if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig()->acc_align; } - if (compassConfig->mag_align != ALIGN_DEFAULT) { - mag.dev.magAlign = compassConfig->mag_align; + if (compassConfig()->mag_align != ALIGN_DEFAULT) { + mag.dev.magAlign = compassConfig()->mag_align; } return true; diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index e656294c6f..9b5d045046 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,8 +17,4 @@ #pragma once -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *baroConfig, - const sonarConfig_t *sonarConfig); +bool sensorsAutodetect(void); diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index e8c78c6087..0e3d0768a0 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -29,6 +29,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/runtime_config.h" diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 16151fc2f9..d123679394 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,10 +17,9 @@ #pragma once +#include "config/parameter_group.h" #include "common/time.h" - #include "drivers/sonar_hcsr04.h" - #include "sensors/battery.h" #define SONAR_OUT_OF_RANGE (-1) diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index a6b455f81b..d39e00200e 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -21,17 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), // PWM1 - PA4 - *TIM3_CH2 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index eb848b4532..8b873e6358 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -85,9 +85,6 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) -#define SENSORS_SET (SENSOR_ACC) - #undef GPS #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index ab0fa2ada6..7173268a47 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -135,8 +135,6 @@ #define I2C_DEVICE (I2CDEV_4) //#define I2C_DEVICE_EXT (I2CDEV_2) -#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) - #define USE_ADC #define VBAT_ADC_PIN PC0 #define CURRENT_METER_ADC_PIN PC1 diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index b65b136e04..e7b417c39e 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -20,25 +20,11 @@ #include -#include "common/utils.h" - -#include "drivers/io.h" - -#include "fc/rc_controls.h" - -#include "flight/failsafe.h" -#include "flight/mixer.h" -#include "flight/pid.h" - -#include "rx/rx.h" - -#include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" void targetConfiguration(master_t *config) { - UNUSED(config); - - batteryConfig->currentMeterScale = 235; + config->batteryConfig.currentMeterScale = 235; } diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index cfd8ef165a..71f4ce0b08 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -117,8 +117,6 @@ #define I2C3_SCL PA8 #define I2C3_SDA PC9 -#define SENSORS_SET (SENSOR_ACC) - #define LED_STRIP // alternative defaults for Colibri/Gemini target diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 18717103cd..550f69b97c 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -77,8 +77,6 @@ #include "bus_bst.h" #include "i2c_bst.h" -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse); - #define BST_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made @@ -640,13 +638,13 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_SERVO_MIX_RULES: for (i = 0; i < MAX_SERVO_RULES; i++) { - bstWrite8(customServoMixer(i)->targetChannel); - bstWrite8(customServoMixer(i)->inputSource); - bstWrite8(customServoMixer(i)->rate); - bstWrite8(customServoMixer(i)->speed); - bstWrite8(customServoMixer(i)->min); - bstWrite8(customServoMixer(i)->max); - bstWrite8(customServoMixer(i)->box); + bstWrite8(customServoMixers(i)->targetChannel); + bstWrite8(customServoMixers(i)->inputSource); + bstWrite8(customServoMixers(i)->rate); + bstWrite8(customServoMixers(i)->speed); + bstWrite8(customServoMixers(i)->min); + bstWrite8(customServoMixers(i)->max); + bstWrite8(customServoMixers(i)->box); } break; #endif @@ -1067,7 +1065,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); } else { ret = BST_FAILED; } @@ -1179,13 +1177,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (i >= MAX_SERVO_RULES) { ret = BST_FAILED; } else { - customServoMixer(i)->targetChannel = bstRead8(); - customServoMixer(i)->inputSource = bstRead8(); - customServoMixer(i)->rate = bstRead8(); - customServoMixer(i)->speed = bstRead8(); - customServoMixer(i)->min = bstRead8(); - customServoMixer(i)->max = bstRead8(); - customServoMixer(i)->box = bstRead8(); + customServoMixers(i)->targetChannel = bstRead8(); + customServoMixers(i)->inputSource = bstRead8(); + customServoMixers(i)->rate = bstRead8(); + customServoMixers(i)->speed = bstRead8(); + customServoMixers(i)->min = bstRead8(); + customServoMixers(i)->max = bstRead8(); + customServoMixers(i)->box = bstRead8(); loadCustomServoMixer(); } #endif diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 2cbffeaedf..2206a57e6a 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -134,8 +134,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 62a4de9952..4d429b641b 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -84,8 +84,6 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define SENSORS_SET (SENSOR_ACC) - #undef GPS #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index b9cc396a3a..315faf2fe5 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -101,8 +101,7 @@ #define ACC_MPU6500_ALIGN CW0_DEG #define BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 +#define USE_BARO_MS5611 // needed for Flip32 board #define USE_BARO_BMP280 /* diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index a1cf620a80..c910194056 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -136,8 +136,6 @@ //#define I2C_DEVICE_EXT (I2CDEV_2) -#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) - #define USE_ADC #define VBAT_ADC_PIN PA3 #define CURRENT_METER_ADC_PIN PC0 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 6d4289da5a..0ad2aff3b4 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -164,8 +164,6 @@ #define LED_STRIP -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_VBAT) @@ -173,10 +171,10 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) +#define TARGET_IO_PORTB (0xffff & ~(BIT(2))) +#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13))) +#define TARGET_IO_PORTD BIT(2) #define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index a55e4eca26..9e8357581f 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -68,8 +68,6 @@ #define USE_SPI #define USE_SPI_DEVICE_2 -#define SENSORS_SET (SENSOR_ACC) - #define TELEMETRY #define BLACKBOX #define SERIAL_RX diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 5e910d8ff6..28bec2ccea 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -26,11 +26,11 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM3 - PA8 - DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1), // PWM2 - PA7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM4 - PB0 - DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), // PWM1 - PA4 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM5 - PB1 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), // PWM6 - PPM - DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PWM6 - PPM + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), }; diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index f01f07a096..dcb58933f8 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -92,8 +92,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) - #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index a93fdc3813..f5ad7a5a79 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -186,8 +186,6 @@ #define LED_STRIP -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #if defined(PODIUMF4) #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index ebff516b0e..3334b77397 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -28,9 +28,6 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -// early prototype had slightly different pin mappings. -//#define SPRACINGF3MINI_MKII_REVA - #define LED0 PB3 #endif @@ -39,36 +36,35 @@ #define USE_EXTI #define MPU_INT_EXTI PC13 -#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO #define ACC -#define BARO -#define USE_BARO_BMP280 - #ifdef TINYBEEF3 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT + #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW270_DEG #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW270_DEG - -#define MAG_AK8963_ALIGN CW90_DEG_FLIP #else -//#define USE_FAKE_GYRO +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + #define USE_GYRO_MPU6500 #define GYRO_MPU6500_ALIGN CW180_DEG -//#define USE_FAKE_ACC #define USE_ACC_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG +#define BARO +#define USE_BARO_BMP280 + #define MAG #define USE_MPU9250_MAG // Enables bypass configuration #define USE_MAG_AK8975 @@ -80,24 +76,20 @@ //#define SONAR_ECHO_PIN PB1 //#define SONAR_TRIGGER_PIN PB0 +#define BRUSHED_ESC_AUTODETECT + #define USB_IO -#ifndef TINYBEEF3 -#define USB_CABLE_DETECTION - -#define USB_DETECT_PIN PB5 -#endif - #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 5 #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 @@ -107,21 +99,26 @@ #define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7) +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#ifdef TINYBEEF3 +#define SERIAL_PORT_COUNT 4 +#else +#define USB_CABLE_DETECTION +#define USB_DETECT_PIN PB5 + +#define USE_SOFTSERIAL1 #define SOFTSERIAL_1_TIMER TIM2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 #define SONAR_SOFTSERIAL1_EXCLUSIVE -#define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define SERIAL_PORT_COUNT 5 +#endif #define USE_SPI -#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 #ifdef TINYBEEF3 #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -131,9 +128,18 @@ #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 -#define MPU6500_CS_PIN PB9 +#define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 -#endif +#else +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 #define USE_SDCARD #define USE_SDCARD_SPI2 @@ -156,6 +162,9 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#endif + #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC #define ADC_INSTANCE ADC2 @@ -169,16 +178,11 @@ #define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#ifdef TINYBEEF3 -#define BRUSHED_ESC_AUTODETECT -#else -#define DEFAULT_FEATURES FEATURE_BLACKBOX -#endif #ifndef TINYBEEF3 +#define DEFAULT_FEATURES FEATURE_BLACKBOX + #define BUTTONS #define BUTTON_A_PIN PB1 #define BUTTON_B_PIN PB0 @@ -187,12 +191,6 @@ #define BINDPLUG_PIN PB0 #endif -#define SPEKTRUM_BIND -// USART3, -#define BIND_PIN PB11 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) diff --git a/src/main/target/common.h b/src/main/target/common.h index 38e562c186..2117ca1478 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,6 +17,7 @@ #pragma once +//#define USE_PARAMETER_GROUPS // type conversion warnings. // -Wconversion can be turned on to enable the process of eliminating these warnings //#pragma GCC diagnostic warning "-Wconversion" diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld index 2cc81c9fd5..eaf65e7b9a 100644 --- a/src/main/target/link/stm32_flash.ld +++ b/src/main/target/link/stm32_flash.ld @@ -81,6 +81,19 @@ SECTIONS KEEP (*(SORT(.fini_array.*))) PROVIDE_HIDDEN (__fini_array_end = .); } >FLASH + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH /* used by the startup to initialize data */ _sidata = .; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d62326e113..baa00554a5 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -32,10 +32,8 @@ #define CLEANFLIGHT #endif -#ifdef CLEANFLIGHT #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#endif #include "common/streambuf.h" #include "common/utils.h" @@ -184,7 +182,7 @@ void crsfFrameBatterySensor(sbuf_t *dst) const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage(); #else crsfSerialize16(dst, amperage / 10); - const uint32_t batteryCapacity = batteryConfig->batteryCapacity; + const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); #endif crsfSerialize8(dst, (batteryCapacity >> 16)); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index f696419016..ef1877e82e 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -33,6 +33,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -72,13 +74,10 @@ static serialPortConfig_t *portConfig; #define FRSKY_BAUDRATE 9600 #define FRSKY_INITIAL_PORT_MODE MODE_TX -static telemetryConfig_t *telemetryConfig; static bool frskyTelemetryEnabled = false; static portSharing_e frskyPortSharing; -extern batteryConfig_t *batteryConfig; - #define CYCLETIME 125 #define PROTOCOL_HEADER 0x5E @@ -196,24 +195,21 @@ static void sendGpsAltitude(void) } #endif -static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +static void sendThrottleOrBatterySizeAsRpm(void) { sendDataHead(ID_RPM); #ifdef USE_ESC_SENSOR - UNUSED(rxConfig); - UNUSED(deadband3d_throttle); - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0); #else if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); } else { - serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER)); + serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER)); } #endif } @@ -240,7 +236,7 @@ static void sendSatalliteSignalQualityAsTemperature2(void) } sendDataHead(ID_TEMPRATURE2); - if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) { + if (telemetryConfig()->frsky_unit == FRSKY_UNIT_METRICS) { serialize16(satellite); } else { float tmp = (satellite - 32) / 1.8f; @@ -286,7 +282,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left - if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) { + if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { result->dddmm = deg * 100 + min; } else { result->dddmm = deg * 60 + min; @@ -332,8 +328,8 @@ static void sendFakeLatLong(void) // Heading is only displayed on OpenTX if non-zero lat/long is also sent int32_t coord[2] = {0,0}; - coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); - coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); + coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); + coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); sendLatLong(coord); } @@ -409,7 +405,7 @@ static void sendVoltage(void) */ static void sendVoltageAmp(void) { - if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { + if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { /* * Use new ID 0x39 to send voltage directly in 0.1 volts resolution */ @@ -418,7 +414,7 @@ static void sendVoltageAmp(void) } else { uint16_t voltage = (getVbat() * 110) / 21; uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = voltage / batteryCellCount; } else { vfasVoltage = voltage; @@ -440,7 +436,7 @@ static void sendFuelLevel(void) { sendDataHead(ID_FUEL_LEVEL); - if (batteryConfig->batteryCapacity > 0) { + if (batteryConfig()->batteryCapacity > 0) { serialize16((uint16_t)calculateBatteryPercentage()); } else { serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); @@ -455,9 +451,8 @@ static void sendHeading(void) serialize16(0); } -void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initFrSkyTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); } @@ -475,7 +470,7 @@ void configureFrSkyTelemetryPort(void) return; } - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); + frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); if (!frskyPort) { return; } @@ -509,7 +504,7 @@ void checkFrSkyTelemetryState(void) } } -void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void handleFrSkyTelemetry(void) { if (!frskyTelemetryEnabled) { return; @@ -540,7 +535,7 @@ void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); - sendThrottleOrBatterySizeAsRpm(rxConfig, deadband3d_throttle); + sendThrottleOrBatterySizeAsRpm(); if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) { sendVoltage(); diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 89ba617c8f..255dae90de 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -22,12 +22,10 @@ typedef enum { FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; -struct rxConfig_s; -void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void handleFrSkyTelemetry(void); void checkFrSkyTelemetryState(void); -struct telemetryConfig_s; -void initFrSkyTelemetry(struct telemetryConfig_s *telemetryConfig); +void initFrSkyTelemetry(void); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index c43822b2a3..dfae08c7e8 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -65,6 +65,10 @@ #include "common/axis.h" #include "common/time.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" @@ -108,7 +112,6 @@ static uint8_t hottMsgCrc; static serialPort_t *hottPort = NULL; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; static bool hottTelemetryEnabled = false; static portSharing_e hottPortSharing; @@ -219,7 +222,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) static bool shouldTriggerBatteryAlarmNow(void) { - return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); + return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig()->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); } static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) @@ -289,9 +292,8 @@ void freeHoTTTelemetryPort(void) hottTelemetryEnabled = false; } -void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initHoTTTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 5370dc24f3..660c4266ab 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -490,7 +490,7 @@ typedef struct HOTT_AIRESC_MSG_s { void handleHoTTTelemetry(timeUs_t currentTimeUs); void checkHoTTTelemetryState(void); -void initHoTTTelemetry(telemetryConfig_t *telemetryConfig); +void initHoTTTelemetry(void); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 8e6341e90b..9bb193454c 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -32,7 +32,8 @@ #if defined(TELEMETRY) && defined(TELEMETRY_IBUS) -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "common/axis.h" diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h index 8bbd30c5d8..3688ea2ffc 100644 --- a/src/main/telemetry/ibus.h +++ b/src/main/telemetry/ibus.h @@ -17,13 +17,11 @@ #pragma once -/* typedef struct ibusTelemetryConfig_s { uint8_t report_cell_voltage; // report vbatt divided with cellcount } ibusTelemetryConfig_t; PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig); -*/ void initIbusTelemetry(void); @@ -31,4 +29,4 @@ void handleIbusTelemetry(void); bool checkIbusTelemetryState(void); void configureIbusTelemetryPort(void); -void freeIbusTelemetryPort(void); \ No newline at end of file +void freeIbusTelemetryPort(void); diff --git a/src/main/telemetry/jetiexbus.h b/src/main/telemetry/jetiexbus.h index 79440c0df2..9c8327db4a 100644 --- a/src/main/telemetry/jetiexbus.h +++ b/src/main/telemetry/jetiexbus.h @@ -17,7 +17,6 @@ #pragma once -struct telemetryConfig_s; -void initJetiExBusTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initJetiExBusTelemetry(void); void checkJetiExBusTelemetryState(void); void handleJetiExBusTelemetry(void); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 038bbf497c..fec48d7b75 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -40,6 +40,7 @@ #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -80,7 +81,6 @@ static serialPort_t *ltmPort; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; static bool ltmEnabled; static portSharing_e ltmPortSharing; static uint8_t ltm_crc; @@ -268,9 +268,8 @@ void freeLtmTelemetryPort(void) ltmEnabled = false; } -void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initLtmTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM); ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM); } diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index e5b4790c5a..892d0efdd8 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -19,8 +19,7 @@ #pragma once -struct telemetryConfig_s; -void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initLtmTelemetry(void); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 7dbf92a1e2..5423919613 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -32,26 +32,18 @@ #include "common/axis.h" #include "common/color.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "fc/config.h" #include "fc/rc_controls.h" - -#include "io/serial.h" -#include "io/gimbal.h" -#include "io/gps.h" -#include "io/ledstrip.h" -#include "io/motors.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" -#include "sensors/gyro.h" -#include "sensors/barometer.h" -#include "sensors/boardalignment.h" -#include "sensors/battery.h" - -#include "rx/rx.h" +#include "fc/runtime_config.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -60,16 +52,24 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + #include "telemetry/telemetry.h" #include "telemetry/mavlink.h" -#include "fc/config.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#include "fc/runtime_config.h" - // mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used // until this is resolved in mavlink library - ignore -Wpedantic for mavlink code #pragma GCC diagnostic push diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5b047861e6..7698b4e385 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -16,6 +16,11 @@ #include "common/maths.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -52,9 +57,6 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -#include "config/config_profile.h" -#include "config/feature.h" - #include "msp/msp.h" extern profile_t *currentProfile; @@ -148,7 +150,6 @@ const uint16_t frSkyDataIdTable[] = { static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port. static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; static bool smartPortTelemetryEnabled = false; static portSharing_e smartPortPortSharing; @@ -302,9 +303,8 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) smartPortSendPackageEx(FSSP_DATA_FRAME,payload); } -void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initSmartPortTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT); smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT); } @@ -326,11 +326,11 @@ void configureSmartPortTelemetryPort(void) portOptions_t portOptions = 0; - if (telemetryConfig->sportHalfDuplex) { + if (telemetryConfig()->sportHalfDuplex) { portOptions |= SERIAL_BIDIR; } - if (telemetryConfig->telemetry_inversion) { + if (telemetryConfig()->telemetry_inversion) { portOptions |= SERIAL_INVERTED; } @@ -622,7 +622,7 @@ void handleSmartPortTelemetry(void) case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT) && batteryCellCount > 0) { uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = getVbat() / batteryCellCount; } else { vfasVoltage = getVbat(); @@ -752,7 +752,7 @@ void handleSmartPortTelemetry(void) } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } else if (telemetryConfig->pidValuesAsTelemetry){ + } else if (telemetryConfig()->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index d50c1fd575..35f18fc4ae 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,7 +7,7 @@ #pragma once -void initSmartPortTelemetry(telemetryConfig_t *); +void initSmartPortTelemetry(void); void handleSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 928acdc2b7..f1cda235a9 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" @@ -50,29 +53,23 @@ #include "telemetry/srxl.h" #include "telemetry/ibus.h" -static telemetryConfig_t *telemetryConfig; - -void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse) -{ - telemetryConfig = telemetryConfigToUse; -} void telemetryInit(void) { #ifdef TELEMETRY_FRSKY - initFrSkyTelemetry(telemetryConfig); + initFrSkyTelemetry(); #endif #ifdef TELEMETRY_HOTT - initHoTTTelemetry(telemetryConfig); + initHoTTTelemetry(); #endif #ifdef TELEMETRY_SMARTPORT - initSmartPortTelemetry(telemetryConfig); + initSmartPortTelemetry(); #endif #ifdef TELEMETRY_LTM - initLtmTelemetry(telemetryConfig); + initLtmTelemetry(); #endif #ifdef TELEMETRY_JETIEXBUS - initJetiExBusTelemetry(telemetryConfig); + initJetiExBusTelemetry(); #endif #ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); @@ -95,7 +92,7 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) bool enabled = portSharing == PORTSHARING_NOT_SHARED; if (portSharing == PORTSHARING_SHARED) { - if (telemetryConfig->telemetry_switch) + if (telemetryConfig()->telemetry_switch) enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY); else enabled = ARMING_FLAG(ARMED); @@ -142,13 +139,10 @@ void telemetryCheckState(void) #endif } -void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void telemetryProcess(uint32_t currentTime) { #ifdef TELEMETRY_FRSKY - handleFrSkyTelemetry(rxConfig, deadband3d_throttle); -#else - UNUSED(rxConfig); - UNUSED(deadband3d_throttle); + handleFrSkyTelemetry(); #endif #ifdef TELEMETRY_HOTT handleHoTTTelemetry(currentTime); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index aa60912147..0fc00cae57 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -24,6 +24,9 @@ #pragma once +#include "config/parameter_group.h" +#include "io/serial.h" + typedef enum { FRSKY_FORMAT_DMS = 0, FRSKY_FORMAT_NMEA @@ -35,11 +38,11 @@ typedef enum { } frskyUnit_e; typedef struct telemetryConfig_s { + float gpsNoFixLatitude; + float gpsNoFixLongitude; uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inversion; // also shared with smartport inversion uint8_t sportHalfDuplex; - float gpsNoFixLatitude; - float gpsNoFixLongitude; frskyGpsCoordFormat_e frsky_coordinate_format; frskyUnit_e frsky_unit; uint8_t frsky_vfas_precision; @@ -49,18 +52,18 @@ typedef struct telemetryConfig_s { uint8_t report_cell_voltage; } telemetryConfig_t; -void telemetryInit(void); -bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); -extern serialPort_t *telemetrySharedPort; - -void telemetryCheckState(void); -struct rxConfig_s; -void telemetryProcess(uint32_t currentTime, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); - -bool telemetryDetermineEnabledState(portSharing_e portSharing); - -void telemetryUseConfig(telemetryConfig_t *telemetryConfig); +PG_DECLARE(telemetryConfig_t, telemetryConfig); #define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) +extern serialPort_t *telemetrySharedPort; + +void telemetryInit(void); +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); + +void telemetryCheckState(void); +void telemetryProcess(uint32_t currentTime); + +bool telemetryDetermineEnabledState(portSharing_e portSharing); + void releaseSharedTelemetryPorts(void); diff --git a/src/test/Makefile b/src/test/Makefile index 9f1e83940d..26b95987ce 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -256,24 +256,24 @@ $(OBJECT_DIR)/altitude_hold_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/flight/gps_conversion.o : \ - $(USER_DIR)/flight/gps_conversion.c \ - $(USER_DIR)/flight/gps_conversion.h \ +$(OBJECT_DIR)/common/gps_conversion.o : \ + $(USER_DIR)/common/gps_conversion.c \ + $(USER_DIR)/common/gps_conversion.h \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/gps_conversion.c -o $@ $(OBJECT_DIR)/gps_conversion_unittest.o : \ $(TEST_DIR)/gps_conversion_unittest.cc \ - $(USER_DIR)/flight/gps_conversion.h \ + $(USER_DIR)/common/gps_conversion.h \ $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@ $(OBJECT_DIR)/gps_conversion_unittest : \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/gps_conversion_unittest.o \ $(OBJECT_DIR)/gtest_main.a @@ -359,7 +359,7 @@ $(OBJECT_DIR)/telemetry_hott_unittest.o : \ $(OBJECT_DIR)/telemetry_hott_unittest : \ $(OBJECT_DIR)/telemetry/hott.o \ $(OBJECT_DIR)/telemetry_hott_unittest.o \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/gtest_main.a $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ @@ -537,7 +537,7 @@ $(OBJECT_DIR)/telemetry_crsf_unittest : \ $(OBJECT_DIR)/telemetry_crsf_unittest.o \ $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/common/streambuf.o \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/fc/runtime_config.o \ $(OBJECT_DIR)/gtest_main.a diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index b839912c00..c422e24731 100644 --- a/src/test/unit/cms_unittest.cc +++ b/src/test/unit/cms_unittest.cc @@ -26,6 +26,7 @@ #define BARO extern "C" { + #include "platform.h" #include "target.h" #include "drivers/display.h" #include "cms/cms.h" diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc index df45a1829c..82c857e38a 100644 --- a/src/test/unit/parameter_groups_unittest.cc +++ b/src/test/unit/parameter_groups_unittest.cc @@ -22,16 +22,14 @@ #include extern "C" { - #include "build/debug.h" - #include - + #include "build/debug.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" #include "io/motors.h" -PG_DECLARE(motorConfig_t, motorConfig); +//PG_DECLARE(motorConfig_t, motorConfig); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); @@ -49,7 +47,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, TEST(ParameterGroupsfTest, Test_pgResetAll) { - memset(motorConfig(), 0, sizeof(motorConfig_t)); + memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); pgResetAll(0); EXPECT_EQ(1150, motorConfig()->minthrottle); EXPECT_EQ(1850, motorConfig()->maxthrottle); @@ -59,7 +57,7 @@ TEST(ParameterGroupsfTest, Test_pgResetAll) TEST(ParameterGroupsfTest, Test_pgFind) { - memset(motorConfig(), 0, sizeof(motorConfig_t)); + memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); const pgRegistry_t *pgRegistry = pgFind(PG_MOTOR_CONFIG); pgResetCurrent(pgRegistry); EXPECT_EQ(1150, motorConfig()->minthrottle); @@ -69,7 +67,7 @@ TEST(ParameterGroupsfTest, Test_pgFind) motorConfig_t motorConfig2; memset(&motorConfig2, 0, sizeof(motorConfig_t)); - motorConfig()->motorPwmRate = 500; + motorConfigMutable()->motorPwmRate = 500; pgStore(pgRegistry, &motorConfig2, sizeof(motorConfig_t), 0); EXPECT_EQ(1150, motorConfig2.minthrottle); EXPECT_EQ(1850, motorConfig2.maxthrottle); diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index c5c400aa85..6b5f77acff 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -17,6 +17,8 @@ #pragma once +#define USE_PARAMETER_GROUPS + #define U_ID_0 0 #define U_ID_1 1 #define U_ID_2 2 @@ -30,8 +32,6 @@ #define USE_SERVOS #define TRANSPONDER -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 - typedef enum { Mode_TEST = 0x0, diff --git a/src/test/unit/target.h b/src/test/unit/target.h index cc8149c790..741529284a 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -56,8 +56,6 @@ #define SERIAL_PORT_COUNT 8 -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 - #define TARGET_BOARD_IDENTIFIER "TEST" #define LED_STRIP_TIMER 1 diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index bb1a526d7e..4fb0cea52f 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -22,40 +22,42 @@ #include extern "C" { - #include "build/debug.h" - #include + #include "build/debug.h" + #include "common/axis.h" #include "common/filter.h" + #include "common/gps_conversion.h" #include "common/maths.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" - #include "drivers/system.h" #include "drivers/serial.h" + #include "drivers/system.h" #include "fc/runtime_config.h" + #include "flight/pid.h" + #include "flight/imu.h" + #include "io/gps.h" #include "io/serial.h" #include "rx/crsf.h" - #include "sensors/sensors.h" #include "sensors/battery.h" + #include "sensors/sensors.h" - #include "telemetry/telemetry.h" #include "telemetry/crsf.h" - - #include "flight/pid.h" - #include "flight/imu.h" - #include "flight/gps_conversion.h" + #include "telemetry/telemetry.h" bool airMode; uint16_t vbat; serialPort_t *telemetrySharedPort; + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h" @@ -135,10 +137,7 @@ TEST(TelemetryCrsfTest, TestGPS) TEST(TelemetryCrsfTest, TestBattery) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - batteryConfig_t workingBatteryConfig; - batteryConfig = &workingBatteryConfig; - memset(batteryConfig, 0, sizeof(batteryConfig_t)); vbat = 0; // 0.1V units int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); @@ -157,7 +156,7 @@ TEST(TelemetryCrsfTest, TestBattery) vbat = 33; // 3.3V = 3300 mv amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps - batteryConfig->batteryCapacity = 1234; + batteryConfigMutable()->batteryCapacity = 1234; frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 EXPECT_EQ(33, voltage); @@ -307,7 +306,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} bool telemetryDetermineEnabledState(portSharing_e) {return true;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} -portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} +portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} uint8_t batteryCapacityRemainingPercentage(void) {return 67;} uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;} diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index ff984b097b..29d9cd59cf 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -27,24 +27,30 @@ extern "C" { #include "build/debug.h" #include "common/axis.h" + #include "common/gps_conversion.h" + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" #include "drivers/system.h" #include "drivers/serial.h" + #include "drivers/system.h" - #include "sensors/sensors.h" - #include "sensors/battery.h" - #include "sensors/barometer.h" + #include "fc/runtime_config.h" + + #include "flight/pid.h" - #include "io/serial.h" #include "io/gps.h" + #include "io/serial.h" + + #include "sensors/barometer.h" + #include "sensors/battery.h" + #include "sensors/sensors.h" #include "telemetry/telemetry.h" #include "telemetry/hott.h" - #include "flight/pid.h" - #include "flight/gps_conversion.h" - - #include "fc/runtime_config.h" + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h" @@ -246,7 +252,7 @@ bool telemetryDetermineEnabledState(portSharing_e) return true; } -portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) +portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) { return PORTSHARING_NOT_SHARED; }