diff --git a/Makefile b/Makefile index 1a0e09d36a..5d4c39945f 100644 --- a/Makefile +++ b/Makefile @@ -665,6 +665,7 @@ COMMON_SRC = \ build/version.c \ $(TARGET_DIR_SRC) \ main.c \ + common/bitarray.c \ common/encoding.c \ common/filter.c \ common/maths.c \ @@ -677,6 +678,7 @@ COMMON_SRC = \ config/config_streamer.c \ drivers/adc.c \ drivers/buf_writer.c \ + drivers/bus_i2c_config.c \ drivers/bus_i2c_soft.c \ drivers/bus_spi.c \ drivers/bus_spi_soft.c \ @@ -735,6 +737,7 @@ FC_SRC = \ fc/fc_rc.c \ fc/rc_adjustments.c \ fc/rc_controls.c \ + fc/rc_modes.c \ fc/cli.c \ fc/settings.c \ flight/altitude.c \ @@ -896,7 +899,11 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ io/osd_slave.c SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ + drivers/bus_i2c_config.c \ drivers/serial_escserial.c \ + drivers/serial_pinconfig.c \ + drivers/serial_uart_init.c \ + drivers/serial_uart_pinconfig.c \ drivers/vtx_rtc6705_soft_spi.c \ drivers/vtx_rtc6705.c \ drivers/vtx_common.c \ @@ -907,9 +914,6 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ config/feature.c \ config/parameter_group.c \ config/config_streamer.c \ - drivers/serial_pinconfig.c \ - drivers/serial_uart_init.c \ - drivers/serial_uart_pinconfig.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ @@ -1014,7 +1018,8 @@ STM32F7xx_COMMON_SRC = \ drivers/serial_uart_stm32f7xx.c \ drivers/serial_uart_hal.c -F7EXCLUDES = drivers/bus_spi.c \ +F7EXCLUDES = \ + drivers/bus_spi.c \ drivers/bus_i2c.c \ drivers/timer.c \ drivers/serial_uart.c @@ -1023,6 +1028,7 @@ SITLEXCLUDES = \ drivers/adc.c \ drivers/bus_spi.c \ drivers/bus_i2c.c \ + drivers/bus_i2c_config.c \ drivers/dma.c \ drivers/pwm_output.c \ drivers/timer.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c665bddb48..40cf9c3bad 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -47,6 +47,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" @@ -322,10 +323,10 @@ typedef struct blackboxSlowState_s { } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From mixer.c: -extern uint16_t motorOutputHigh, motorOutputLow; +extern float motorOutputHigh, motorOutputLow; //From rc_controls.c -extern uint32_t rcModeActivationMask; +extern boxBitmask_t rcModeActivationMask; static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; @@ -753,7 +754,7 @@ static void writeSlowFrame(void) */ static void loadSlowState(blackboxSlowState_t *slow) { - slow->flightModeFlags = rcModeActivationMask; //was flightModeFlags; + memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; slow->stateFlags = stateFlags; slow->failsafePhase = failsafePhase(); slow->rxSignalReceived = rxIsReceivingSignal(); @@ -861,7 +862,7 @@ static void blackboxStart(void) */ blackboxBuildConditionCache(); - blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(BOXBLACKBOX); blackboxResetIterationTimers(); @@ -870,7 +871,7 @@ static void blackboxStart(void) * it finally plays the beep for this arming event. */ blackboxLastArmingBeep = getArmingBeepTimeMicros(); - blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status + memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); } @@ -1187,6 +1188,9 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v */ static bool blackboxWriteSysinfo(void) { + const uint16_t motorOutputLowInt = lrintf(motorOutputLow); + const uint16_t motorOutputHighInt = lrintf(motorOutputHigh); + // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line) if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) { return false; @@ -1202,7 +1206,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); - BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLow,motorOutputHigh); + BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( @@ -1383,10 +1387,10 @@ static void blackboxCheckAndLogFlightMode(void) flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags // Use != so that we can still detect a change if the counter wraps - if (rcModeActivationMask != blackboxLastFlightModeFlags) { + if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) { eventData.lastFlags = blackboxLastFlightModeFlags; - blackboxLastFlightModeFlags = rcModeActivationMask; - eventData.flags = rcModeActivationMask; + memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); + memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags)); blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData); } diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index bca4df0538..2a811bc7b3 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -137,7 +137,7 @@ OSD_Entry menuOsdActiveElemsEntries[] = {"YAW PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_YAW_PIDS], 0}, {"PROFILES", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PIDRATE_PROFILE], 0}, {"DEBUG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_DEBUG], 0}, - {"BATT WARN", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_MAIN_BATT_WARNING], 0}, + {"WARNINGS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_WARNINGS], 0}, {"DISARMED", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_DISARMED], 0}, {"PIT ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_ANGLE], 0}, {"ROL ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_ANGLE], 0}, diff --git a/src/main/target/ALIENFLIGHTF3/initialisation.c b/src/main/common/bitarray.c similarity index 57% rename from src/main/target/ALIENFLIGHTF3/initialisation.c rename to src/main/common/bitarray.c index f435cb994c..53ca99127c 100644 --- a/src/main/target/ALIENFLIGHTF3/initialisation.c +++ b/src/main/common/bitarray.c @@ -15,34 +15,25 @@ * along with Cleanflight. If not, see . */ -#include #include +#include -#include "platform.h" -#include "drivers/bus_i2c.h" -#include "drivers/bus_spi.h" -#include "hardware_revision.h" +#include "bitarray.h" -void targetBusInit(void) +#define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op (1 << ((bit) % (sizeof((array)[0]) * 8)))) + +bool bitArrayGet(const void *array, unsigned bit) { - #ifdef USE_SPI - #ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); - #endif - #ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); - #endif - #ifdef USE_SPI_DEVICE_3 - if (hardwareRevision == AFF3_REV_2) { - spiInit(SPIDEV_3); - } - #endif - #ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4); - #endif - #endif + return BITARRAY_BIT_OP((uint32_t*)array, bit, &); +} + +void bitArraySet(void *array, unsigned bit) +{ + BITARRAY_BIT_OP((uint32_t*)array, bit, |=); +} + +void bitArrayClr(void *array, unsigned bit) +{ + BITARRAY_BIT_OP((uint32_t*)array, bit, &=~); +} - #ifdef USE_I2C - i2cInit(I2C_DEVICE); - #endif -} \ No newline at end of file diff --git a/src/main/common/bitarray.h b/src/main/common/bitarray.h new file mode 100644 index 0000000000..48032bb297 --- /dev/null +++ b/src/main/common/bitarray.h @@ -0,0 +1,20 @@ +/* + * 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 . + */ + +bool bitArrayGet(const void *array, unsigned bit); +void bitArraySet(void *array, unsigned bit); +void bitArrayClr(void *array, unsigned bit); diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 983bd6879d..001181977f 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -20,6 +20,8 @@ #include #include +#define NOOP do {} while (0) + #define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0])) #define ARRAYEND(x) (&(x)[ARRAYLEN(x)]) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index b94b3512d5..0993ac50d5 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -38,10 +38,7 @@ static uint16_t eepromConfigSize; 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, + CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_SYSTEM, } configRecordFlags_e; #define CR_CLASSIFICATION_MASK (0x3) @@ -178,23 +175,12 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla bool loadEEPROM(void) { PG_FOREACH(reg) { - configRecordFlags_e cls_start, cls_end; - if (pgIsSystem(reg)) { - cls_start = CR_CLASSICATION_SYSTEM; - cls_end = CR_CLASSICATION_SYSTEM; + const configRecord_t *rec = findEEPROM(reg, CR_CLASSICATION_SYSTEM); + if (rec) { + // config from EEPROM is available, use it to initialize PG. pgLoad will handle version mismatch + pgLoad(reg, rec->pg, rec->size - offsetof(configRecord_t, pg), rec->version); } 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); - } + pgReset(reg); } } return true; @@ -225,25 +211,11 @@ static bool writeSettingsToEEPROM(void) .flags = 0 }; - if (pgIsSystem(reg)) { - // write the only instance - record.flags |= CR_CLASSICATION_SYSTEM; - config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); - crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record)); - config_streamer_write(&streamer, reg->address, regSize); - crc = crc16_ccitt_update(crc, reg->address, regSize); - } else { - // write one instance for each profile - for (uint8_t profileIndex = 0; profileIndex < PG_PROFILE_COUNT; profileIndex++) { - record.flags = 0; - record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK); - config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); - crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record)); - const uint8_t *address = reg->address + (regSize * profileIndex); - config_streamer_write(&streamer, address, regSize); - crc = crc16_ccitt_update(crc, address, regSize); - } - } + record.flags |= CR_CLASSICATION_SYSTEM; + config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); + crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record)); + config_streamer_write(&streamer, reg->address, regSize); + crc = crc16_ccitt_update(crc, reg->address, regSize); } configFooter_t footer = { diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index 58dbe51e1a..a61b454820 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -34,15 +34,9 @@ const pgRegistry_t* pgFind(pgn_t pgn) return NULL; } -static uint8_t *pgOffset(const pgRegistry_t* reg, uint8_t profileIndex) +static uint8_t *pgOffset(const pgRegistry_t* reg) { - const uint16_t regSize = pgSize(reg); - - uint8_t *base = reg->address; - if (!pgIsSystem(reg)) { - base += (regSize * profileIndex); - } - return base; + return reg->address; } static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base) @@ -59,18 +53,9 @@ static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base) } } -void pgReset(const pgRegistry_t* reg, int profileIndex) +void pgReset(const pgRegistry_t* reg) { - pgResetInstance(reg, pgOffset(reg, profileIndex)); -} - -void pgResetCurrent(const pgRegistry_t *reg) -{ - if (pgIsSystem(reg)) { - pgResetInstance(reg, reg->address); - } else { - pgResetInstance(reg, *reg->ptr); - } + pgResetInstance(reg, pgOffset(reg)); } bool pgResetCopy(void *copy, pgn_t pgn) @@ -83,44 +68,26 @@ bool pgResetCopy(void *copy, pgn_t pgn) return false; } -void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int size, int version) +void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version) { - pgResetInstance(reg, pgOffset(reg, profileIndex)); + pgResetInstance(reg, pgOffset(reg)); // restore only matching version, keep defaults otherwise if (version == pgVersion(reg)) { const int take = MIN(size, pgSize(reg)); - memcpy(pgOffset(reg, profileIndex), from, take); + memcpy(pgOffset(reg), from, take); } } -int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex) +int pgStore(const pgRegistry_t* reg, void *to, int size) { const int take = MIN(size, pgSize(reg)); - memcpy(to, pgOffset(reg, profileIndex), take); + memcpy(to, pgOffset(reg), take); return take; } - -void pgResetAll(int profileCount) +void pgResetAll() { PG_FOREACH(reg) { - if (pgIsSystem(reg)) { - pgReset(reg, 0); - } else { - // reset one instance for each profile - for (int profileIndex = 0; profileIndex < profileCount; profileIndex++) { - pgReset(reg, profileIndex); - } - } - } -} - -void pgActivateProfile(int profileIndex) -{ - PG_FOREACH(reg) { - if (!pgIsSystem(reg)) { - uint8_t *ptr = pgOffset(reg, profileIndex); - *(reg->ptr) = ptr; - } + pgReset(reg); } } diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index cbfe84de82..4890a95f13 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -34,14 +34,9 @@ typedef enum { PGR_PGN_MASK = 0x0fff, PGR_PGN_VERSION_MASK = 0xf000, PGR_SIZE_MASK = 0x0fff, - PGR_SIZE_SYSTEM_FLAG = 0x0000, // documentary - PGR_SIZE_PROFILE_FLAG = 0x8000 // start using flags from the top bit down + PGR_SIZE_SYSTEM_FLAG = 0x0000 // documentary } pgRegistryInternal_e; -enum { - PG_PROFILE_COUNT = 3 -}; - // function that resets a single parameter group instance typedef void (pgResetFunc)(void * /* base */, int /* size */); @@ -60,8 +55,6 @@ typedef struct pgRegistry_s { static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_MASK;} static inline uint8_t pgVersion(const pgRegistry_t* reg) {return (uint8_t)(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)) @@ -89,19 +82,11 @@ extern const uint8_t __pg_resetdata_end[]; #define PG_FOREACH(_name) \ for (const pgRegistry_t *(_name) = __pg_registry_start; (_name) < __pg_registry_end; _name++) -#define PG_FOREACH_PROFILE(_name) \ - PG_FOREACH(_name) \ - if(pgIsSystem(_name)) \ - continue; \ - else \ - /**/ - // Reset configuration to default (by name) -// Only current profile is reset for profile based configs -#define PG_RESET_CURRENT(_name) \ +#define PG_RESET(_name) \ do { \ extern const pgRegistry_t _name ##_Registry; \ - pgResetCurrent(&_name ## _Registry); \ + pgReset(&_name ## _Registry); \ } while(0) \ /**/ @@ -124,15 +109,6 @@ extern const uint8_t __pg_resetdata_end[]; struct _dummy \ /**/ -// Declare profile config -#define PG_DECLARE_PROFILE(_type, _name) \ - extern _type *_name ## _ProfileCurrent; \ - static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \ - static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \ - struct _dummy \ - /**/ - - // Register system config #define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ _type _name ## _System; \ @@ -195,43 +171,6 @@ extern const uint8_t __pg_resetdata_end[]; /**/ #endif -#ifdef UNIT_TEST -# define _PG_PROFILE_CURRENT_DECL(_type, _name) \ - _type *_name ## _ProfileCurrent = &_name ## _Storage[0]; -#else -# define _PG_PROFILE_CURRENT_DECL(_type, _name) \ - _type *_name ## _ProfileCurrent; -#endif - -// register profile config -#define PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, _reset) \ - STATIC_UNIT_TESTED _type _name ## _Storage[PG_PROFILE_COUNT]; \ - _PG_PROFILE_CURRENT_DECL(_type, _name) \ - extern const pgRegistry_t _name ## _Registry; \ - const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = { \ - .pgn = _pgn | (_version << 12), \ - .size = sizeof(_type) | PGR_SIZE_PROFILE_FLAG, \ - .address = (uint8_t*)&_name ## _Storage, \ - .ptr = (uint8_t **)&_name ## _ProfileCurrent, \ - _reset, \ - } \ - /**/ - -#define PG_REGISTER_PROFILE(_type, _name, _pgn, _version) \ - PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ - /**/ - -#define PG_REGISTER_PROFILE_WITH_RESET_FN(_type, _name, _pgn, _version) \ - extern void pgResetFn_ ## _name(_type *); \ - PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ - /**/ - -#define PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \ - extern const _type pgResetTemplate_ ## _name; \ - PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ - /**/ - - // Emit reset defaults for config. // Config must be registered with PG_REGISTER__WITH_RESET_TEMPLATE macro #define PG_RESET_TEMPLATE(_type, _name, ...) \ @@ -245,10 +184,8 @@ extern const uint8_t __pg_resetdata_end[]; const pgRegistry_t* pgFind(pgn_t pgn); -void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int size, int version); -int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex); -void pgResetAll(int profileCount); -void pgResetCurrent(const pgRegistry_t *reg); +void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version); +int pgStore(const pgRegistry_t* reg, void *to, int size); +void pgResetAll(); bool pgResetCopy(void *copy, pgn_t pgn); -void pgReset(const pgRegistry_t* reg, int profileIndex); -void pgActivateProfile(int profileIndex); +void pgReset(const pgRegistry_t* reg); diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 77895849d0..e4b0bb3f59 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -106,7 +106,8 @@ #define PG_VTX_CONFIG 515 #define PG_SONAR_CONFIG 516 #define PG_ESC_SENSOR_CONFIG 517 -#define PG_BETAFLIGHT_END 517 +#define PG_I2C_CONFIG 518 +#define PG_BETAFLIGHT_END 518 // OSD configuration (subject to change) diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h new file mode 100644 index 0000000000..d1c1961896 --- /dev/null +++ b/src/main/drivers/bus.h @@ -0,0 +1,24 @@ +/* + * 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 "platform.h" + +#ifdef TARGET_BUS_INIT +void targetBusInit(void); +#endif diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index dd5ee8a8ec..bd27125994 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -19,14 +19,10 @@ #include "platform.h" +#include "config/parameter_group.h" #include "drivers/io_types.h" #include "drivers/rcc_types.h" -#define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) -#define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) -#define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT - - #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID #endif @@ -36,39 +32,27 @@ typedef enum I2CDevice { I2CDEV_1 = 0, I2CDEV_2, I2CDEV_3, -#ifdef USE_I2C_DEVICE_4 I2CDEV_4, -#endif - I2CDEV_COUNT } I2CDevice; -typedef struct i2cDevice_s { - I2C_TypeDef *dev; - ioTag_t scl; - ioTag_t sda; - rccPeriphTag_t rcc; - bool overClock; -#if !defined(STM32F303xC) - uint8_t ev_irq; - uint8_t er_irq; +#if defined(STM32F1) || defined(STM32F3) +#define I2CDEV_COUNT 2 +#elif defined(STM32F4) +#define I2CDEV_COUNT 3 +#elif defined(STM32F7) +#define I2CDEV_COUNT 4 +#else +#define I2CDEV_COUNT 4 #endif -#if defined(STM32F7) - uint8_t af; -#endif -} i2cDevice_t; -typedef struct i2cState_s { - volatile bool error; - volatile bool busy; - volatile uint8_t addr; - volatile uint8_t reg; - volatile uint8_t bytes; - volatile uint8_t writing; - volatile uint8_t reading; - volatile uint8_t* write_p; - volatile uint8_t* read_p; -} i2cState_t; +typedef struct i2cConfig_s { + ioTag_t ioTagScl[I2CDEV_COUNT]; + ioTag_t ioTagSda[I2CDEV_COUNT]; + bool overClock[I2CDEV_COUNT]; + bool pullUp[I2CDEV_COUNT]; +} i2cConfig_t; +void i2cHardwareConfigure(void); void i2cInit(I2CDevice device); bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data); diff --git a/src/main/drivers/bus_i2c_config.c b/src/main/drivers/bus_i2c_config.c new file mode 100644 index 0000000000..74af3f76fe --- /dev/null +++ b/src/main/drivers/bus_i2c_config.c @@ -0,0 +1,262 @@ +/* + * 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 . + */ + +/* + * Created by jflyper + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" +#include "build/debug.h" + +#include "drivers/io.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_impl.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#if defined(USE_I2C) && !defined(SOFT_I2C) + +#ifdef I2C_FULL_RECONFIGURABILITY +#if I2CDEV_COUNT >= 1 +#ifndef I2C1_SCL +#define I2C1_SCL NONE +#endif +#ifndef I2C1_SDA +#define I2C1_SDA NONE +#endif +#endif + +#if I2CDEV_COUNT >= 2 +#ifndef I2C2_SCL +#define I2C2_SCL NONE +#endif +#ifndef I2C2_SDA +#define I2C2_SDA NONE +#endif +#endif + +#if I2CDEV_COUNT >= 3 +#ifndef I2C3_SCL +#define I2C3_SCL NONE +#endif +#ifndef I2C3_SDA +#define I2C3_SDA NONE +#endif +#endif + +#if I2CDEV_COUNT >= 4 +#ifndef I2C4_SCL +#define I2C4_SCL NONE +#endif +#ifndef I2C4_SDA +#define I2C4_SDA NONE +#endif +#endif + +#else // I2C_FULL_RECONFIGURABILITY + +// Backward compatibility for exisiting targets + +#ifdef STM32F1 +#ifndef I2C1_SCL +#define I2C1_SCL PB8 +#endif +#ifndef I2C1_SDA +#define I2C1_SDA PB9 +#endif +#ifndef I2C2_SCL +#define I2C2_SCL PB10 +#endif +#ifndef I2C2_SDA +#define I2C2_SDA PB11 +#endif +#endif // STM32F1 + +#ifdef STM32F3 +#ifndef I2C1_SCL +#define I2C1_SCL PB6 +#endif +#ifndef I2C1_SDA +#define I2C1_SDA PB7 +#endif +#ifndef I2C2_SCL +#define I2C2_SCL PA9 +#endif +#ifndef I2C2_SDA +#define I2C2_SDA PA10 +#endif +#endif // STM32F3 + +#ifdef STM32F4 +#ifndef I2C1_SCL +#define I2C1_SCL PB6 +#endif +#ifndef I2C1_SDA +#define I2C1_SDA PB7 +#endif +#ifndef I2C2_SCL +#define I2C2_SCL PB10 +#endif +#ifndef I2C2_SDA +#define I2C2_SDA PB11 +#endif +#ifndef I2C3_SCL +#define I2C3_SCL PA8 +#endif +#ifndef I2C3_SDA +#define I2C3_SDA PC9 +#endif +#endif // STM32F4 + +#ifdef STM32F7 +#ifndef I2C1_SCL +#define I2C1_SCL PB6 +#endif +#ifndef I2C1_SDA +#define I2C1_SDA PB7 +#endif +#ifndef I2C2_SCL +#define I2C2_SCL PB10 +#endif +#ifndef I2C2_SDA +#define I2C2_SDA PB11 +#endif +#ifndef I2C3_SCL +#define I2C3_SCL PA8 +#endif +#ifndef I2C3_SDA +#define I2C3_SDA PB4 +#endif +#ifndef I2C4_SCL +#define I2C4_SCL PD12 +#endif +#ifndef I2C4_SDA +#define I2C4_SDA PD13 +#endif +#endif // STM32F7 + +#endif // I2C_FULL_RECONFIGURABILITY + +// Backward compatibility for overclocking and internal pullup. +// These will eventually be configurable through PG-based configurator +// (and/or probably through some cli extension). + +#ifndef I2C1_OVERCLOCK +#define I2C1_OVERCLOCK false +#endif +#ifndef I2C2_OVERCLOCK +#define I2C2_OVERCLOCK false +#endif +#ifndef I2C3_OVERCLOCK +#define I2C3_OVERCLOCK false +#endif +#ifndef I2C4_OVERCLOCK +#define I2C4_OVERCLOCK false +#endif + +// Default values for internal pullup + +#if defined(USE_I2C_PULLUP) +#define I2C1_PULLUP true +#define I2C2_PULLUP true +#define I2C3_PULLUP true +#define I2C4_PULLUP true +#else +#define I2C1_PULLUP false +#define I2C2_PULLUP false +#define I2C3_PULLUP false +#define I2C4_PULLUP false +#endif + +typedef struct i2cDefaultConfig_s { + I2CDevice device; + ioTag_t ioTagScl, ioTagSda; + bool overClock; + bool pullUp; +} i2cDefaultConfig_t; + +static const i2cDefaultConfig_t i2cDefaultConfig[] = { +#ifdef USE_I2C_DEVICE_1 + { I2CDEV_1, IO_TAG(I2C1_SCL), IO_TAG(I2C1_SDA), I2C1_OVERCLOCK, I2C1_PULLUP }, +#endif +#ifdef USE_I2C_DEVICE_2 + { I2CDEV_2, IO_TAG(I2C2_SCL), IO_TAG(I2C2_SDA), I2C2_OVERCLOCK, I2C2_PULLUP }, +#endif +#ifdef USE_I2C_DEVICE_3 + { I2CDEV_3, IO_TAG(I2C3_SCL), IO_TAG(I2C3_SDA), I2C3_OVERCLOCK, I2C3_PULLUP }, +#endif +#ifdef USE_I2C_DEVICE_4 + { I2CDEV_4, IO_TAG(I2C4_SCL), IO_TAG(I2C4_SDA), I2C4_OVERCLOCK, I2C4_PULLUP }, +#endif +}; + +PG_DECLARE(i2cConfig_t, i2cConfig); +PG_REGISTER_WITH_RESET_FN(i2cConfig_t, i2cConfig, PG_I2C_CONFIG, 0); + +void pgResetFn_i2cConfig(i2cConfig_t *i2cConfig) +{ + memset(i2cConfig, 0, sizeof(*i2cConfig)); + + for (size_t index = 0 ; index < ARRAYLEN(i2cDefaultConfig) ; index++) { + const i2cDefaultConfig_t *defconf = &i2cDefaultConfig[index]; + i2cConfig->ioTagScl[defconf->device] = defconf->ioTagScl; + i2cConfig->ioTagSda[defconf->device] = defconf->ioTagSda; + i2cConfig->overClock[defconf->device] = defconf->overClock; + i2cConfig->pullUp[defconf->device] = defconf->pullUp; + } +} + +void i2cHardwareConfigure(void) +{ + const i2cConfig_t *pConfig = i2cConfig(); + + for (int index = 0 ; index < I2CDEV_COUNT ; index++) { + const i2cHardware_t *hardware = &i2cHardware[index]; + + if (!hardware->reg) { + continue; + } + + I2CDevice device = hardware->device; + i2cDevice_t *pDev = &i2cDevice[device]; + + memset(pDev, 0, sizeof(*pDev)); + + for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) { + if (pConfig->ioTagScl[device] == hardware->sclPins[pindex]) + pDev->scl = IOGetByTag(pConfig->ioTagScl[device]); + if (pConfig->ioTagSda[device] == hardware->sdaPins[pindex]) + pDev->sda = IOGetByTag(pConfig->ioTagSda[device]); + } + + if (pDev->scl && pDev->sda) { + pDev->hardware = hardware; + pDev->reg = hardware->reg; + pDev->overClock = pConfig->overClock[device]; + pDev->pullUp = pConfig->pullUp[device]; + } + } +} + +#endif // defined(USE_I2C) && !defined(USE_SOFT_I2C) diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 006a19c1ad..ac8508447a 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -17,114 +17,118 @@ #include #include +#include #include -#include "drivers/bus_i2c.h" #include "drivers/io.h" +#include "drivers/io_impl.h" #include "drivers/nvic.h" #include "drivers/time.h" +#include "drivers/rcc.h" -#include "io_impl.h" -#include "rcc.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_impl.h" - -#if !defined(SOFT_I2C) && defined(USE_I2C) +#if defined(USE_I2C) && !defined(SOFT_I2C) #define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default) static void i2cUnstick(IO_t scl, IO_t sda); -#if defined(USE_I2C_PULLUP) -#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP) -#else -#define IOCFG_I2C IOCFG_AF_OD -#endif +#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP) +#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) -#ifndef I2C1_SCL -#define I2C1_SCL PB6 -#endif +#define GPIO_AF4_I2C GPIO_AF4_I2C1 -#ifndef I2C1_SDA -#define I2C1_SDA PB7 +const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { +#ifdef USE_I2C_DEVICE_1 + { + .device = I2CDEV_1, + .reg = I2C1, + .sclPins = { DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) }, + .sdaPins = { DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) }, + .rcc = RCC_APB1(I2C1), + .ev_irq = I2C1_EV_IRQn, + .er_irq = I2C1_ER_IRQn, + }, #endif - -#ifndef I2C2_SCL -#define I2C2_SCL PB10 +#ifdef USE_I2C_DEVICE_2 + { + .device = I2CDEV_2, + .reg = I2C2, + .sclPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PF1) }, + .sdaPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PF0) }, + .rcc = RCC_APB1(I2C2), + .ev_irq = I2C2_EV_IRQn, + .er_irq = I2C2_ER_IRQn, + }, #endif -#ifndef I2C2_SDA -#define I2C2_SDA PB11 +#ifdef USE_I2C_DEVICE_3 + { + .device = I2CDEV_3, + .reg = I2C3, + .sclPins = { DEFIO_TAG_E(PA8) }, + .sdaPins = { DEFIO_TAG_E(PC9) }, + .rcc = RCC_APB1(I2C3), + .ev_irq = I2C3_EV_IRQn, + .er_irq = I2C3_ER_IRQn, + }, #endif - -#ifndef I2C3_SCL -#define I2C3_SCL PA8 -#endif -#ifndef I2C3_SDA -#define I2C3_SDA PB4 -#endif - -#if defined(USE_I2C_DEVICE_4) -#ifndef I2C4_SCL -#define I2C4_SCL PD12 -#endif -#ifndef I2C4_SDA -#define I2C4_SDA PD13 -#endif -#endif - -static i2cDevice_t i2cHardwareMap[] = { - { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn, .af = GPIO_AF4_I2C1 }, - { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 }, - { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 }, -#if defined(USE_I2C_DEVICE_4) - { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1(I2C4), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 } +#ifdef USE_I2C_DEVICE_4 + { + .device = I2CDEV_4, + .reg = I2C4, + .sclPins = { DEFIO_TAG_E(PD12), DEFIO_TAG_E(PF14) }, + .sdaPins = { DEFIO_TAG_E(PD13), DEFIO_TAG_E(PF15) }, + .rcc = RCC_APB1(I2C4), + .ev_irq = I2C4_EV_IRQn, + .er_irq = I2C4_ER_IRQn, + }, #endif }; -typedef struct{ - I2C_HandleTypeDef Handle; -}i2cHandle_t; -static i2cHandle_t i2cHandle[I2CDEV_COUNT]; +i2cDevice_t i2cDevice[I2CDEV_COUNT]; void I2C1_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_1].Handle); + HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_1].handle); } void I2C1_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_1].Handle); + HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_1].handle); } void I2C2_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_2].Handle); + HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_2].handle); } void I2C2_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_2].Handle); + HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_2].handle); } void I2C3_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_3].Handle); + HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_3].handle); } void I2C3_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_3].Handle); + HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_3].handle); } #ifdef USE_I2C_DEVICE_4 void I2C4_ER_IRQHandler(void) { - HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_4].Handle); + HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_4].handle); } void I2C4_EV_IRQHandler(void) { - HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_4].Handle); + HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_4].handle); } #endif @@ -147,12 +151,22 @@ static bool i2cHandleHardwareFailure(I2CDevice device) bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) { + if (device == I2CINVALID || device > I2CDEV_COUNT) { + return false; + } + + I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle; + + if (!pHandle->Instance) { + return false; + } + HAL_StatusTypeDef status; if(reg_ == 0xFF) - status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle,addr_ << 1,data, len_, I2C_DEFAULT_TIMEOUT); + status = HAL_I2C_Master_Transmit(pHandle ,addr_ << 1, data, len_, I2C_DEFAULT_TIMEOUT); else - status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT); + status = HAL_I2C_Mem_Write(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT); if(status != HAL_OK) return i2cHandleHardwareFailure(device); @@ -167,12 +181,22 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { + if (device == I2CINVALID || device > I2CDEV_COUNT) { + return false; + } + + I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle; + + if (!pHandle->Instance) { + return false; + } + HAL_StatusTypeDef status; if(reg_ == 0xFF) - status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT); + status = HAL_I2C_Master_Receive(pHandle ,addr_ << 1, buf, len, I2C_DEFAULT_TIMEOUT); else - status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT); + status = HAL_I2C_Mem_Read(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT); if(status != HAL_OK) return i2cHandleHardwareFailure(device); @@ -182,87 +206,72 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t void i2cInit(I2CDevice device) { - /*## Configure the I2C clock source. The clock is derived from the SYSCLK #*/ -// RCC_PeriphCLKInitTypeDef RCC_PeriphCLKInitStruct; -// RCC_PeriphCLKInitStruct.PeriphClockSelection = i2cHardwareMap[device].clk; -// RCC_PeriphCLKInitStruct.I2c1ClockSelection = i2cHardwareMap[device].clk_src; -// HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphCLKInitStruct); - - switch (device) { - case I2CDEV_1: - __HAL_RCC_I2C1_CLK_ENABLE(); - break; - case I2CDEV_2: - __HAL_RCC_I2C2_CLK_ENABLE(); - break; - case I2CDEV_3: - __HAL_RCC_I2C3_CLK_ENABLE(); - break; -#ifdef USE_I2C_DEVICE_4 - case I2CDEV_4: - __HAL_RCC_I2C4_CLK_ENABLE(); - break; -#endif - default: - break; + if (device == I2CINVALID) { + return; } - if (device == I2CINVALID) - return; - i2cDevice_t *i2c; - i2c = &(i2cHardwareMap[device]); + i2cDevice_t *pDev = &i2cDevice[device]; - //I2C_InitTypeDef i2cInit; + const i2cHardware_t *hardware = pDev->hardware; - IO_t scl = IOGetByTag(i2c->scl); - IO_t sda = IOGetByTag(i2c->sda); + if (!hardware) { + return; + } - IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device)); - IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device)); + IO_t scl = pDev->scl; + IO_t sda = pDev->sda; - // Enable RCC - RCC_ClockCmd(i2c->rcc, ENABLE); + IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device)); + IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device)); + // Enable RCC + RCC_ClockCmd(hardware->rcc, ENABLE); - i2cUnstick(scl, sda); + i2cUnstick(scl, sda); + + // Init pins +#ifdef STM32F7 + IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C); + IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C); +#else + IOConfigGPIO(scl, IOCFG_AF_OD); + IOConfigGPIO(sda, IOCFG_AF_OD); +#endif - // Init pins - #ifdef STM32F7 - IOConfigGPIOAF(scl, IOCFG_I2C, i2c->af); - IOConfigGPIOAF(sda, IOCFG_I2C, i2c->af); - #else - IOConfigGPIO(scl, IOCFG_AF_OD); - IOConfigGPIO(sda, IOCFG_AF_OD); - #endif // Init I2C peripheral - i2cHandle[device].Handle.Instance = i2cHardwareMap[device].dev; + I2C_HandleTypeDef *pHandle = &pDev->handle; + + memset(pHandle, 0, sizeof(*pHandle)); + + pHandle->Instance = pDev->hardware->reg; + /// TODO: HAL check if I2C timing is correct - if (i2c->overClock) { + if (pDev->overClock) { // 800khz Maximum speed tested on various boards without issues - i2cHandle[device].Handle.Init.Timing = 0x00500D1D; + pHandle->Init.Timing = 0x00500D1D; } else { - //i2cHandle[device].Handle.Init.Timing = 0x00500B6A; - i2cHandle[device].Handle.Init.Timing = 0x00500C6F; + pHandle->Init.Timing = 0x00500C6F; } - //i2cHandle[device].Handle.Init.Timing = 0x00D00E28; /* (Rise time = 120ns, Fall time = 25ns) */ - i2cHandle[device].Handle.Init.OwnAddress1 = 0x0; - i2cHandle[device].Handle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; - i2cHandle[device].Handle.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; - i2cHandle[device].Handle.Init.OwnAddress2 = 0x0; - i2cHandle[device].Handle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; - i2cHandle[device].Handle.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + pHandle->Init.OwnAddress1 = 0x0; + pHandle->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + pHandle->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + pHandle->Init.OwnAddress2 = 0x0; + pHandle->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + pHandle->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; - HAL_I2C_Init(&i2cHandle[device].Handle); - /* Enable the Analog I2C Filter */ - HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE); + HAL_I2C_Init(pHandle); - HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER)); - HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq); - HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV)); - HAL_NVIC_EnableIRQ(i2cHardwareMap[device].ev_irq); + // Enable the Analog I2C Filter + HAL_I2CEx_ConfigAnalogFilter(pHandle, I2C_ANALOGFILTER_ENABLE); + + // Setup interrupt handlers + HAL_NVIC_SetPriority(hardware->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER)); + HAL_NVIC_EnableIRQ(hardware->er_irq); + HAL_NVIC_SetPriority(hardware->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV)); + HAL_NVIC_EnableIRQ(hardware->ev_irq); } uint16_t i2cGetErrorCounter(void) diff --git a/src/main/drivers/bus_i2c_impl.h b/src/main/drivers/bus_i2c_impl.h new file mode 100644 index 0000000000..8395845931 --- /dev/null +++ b/src/main/drivers/bus_i2c_impl.h @@ -0,0 +1,76 @@ +/* + * 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 "platform.h" + +#include "drivers/io_types.h" +#include "drivers/rcc_types.h" + +#define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) +#define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) +#define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT + +#define I2C_PIN_SEL_MAX 3 + +typedef struct i2cHardware_s { + I2CDevice device; + I2C_TypeDef *reg; + ioTag_t sclPins[I2C_PIN_SEL_MAX]; + ioTag_t sdaPins[I2C_PIN_SEL_MAX]; + rccPeriphTag_t rcc; +#if !defined(STM32F303xC) + uint8_t ev_irq; + uint8_t er_irq; +#endif +} i2cHardware_t; + +extern const i2cHardware_t i2cHardware[]; + +#if defined(STM32F1) || defined(STM32F4) +typedef struct i2cState_s { + volatile bool error; + volatile bool busy; + volatile uint8_t addr; + volatile uint8_t reg; + volatile uint8_t bytes; + volatile uint8_t writing; + volatile uint8_t reading; + volatile uint8_t* write_p; + volatile uint8_t* read_p; +} i2cState_t; +#endif + +typedef struct i2cDevice_s { + const i2cHardware_t *hardware; + I2C_TypeDef *reg; + IO_t scl; + IO_t sda; + bool overClock; + bool pullUp; + + // MCU/Driver dependent member follows +#if defined(STM32F1) || defined(STM32F4) + i2cState_t state; +#endif +#ifdef USE_HAL_DRIVER + I2C_HandleTypeDef handle; +#endif +} i2cDevice_t; + +extern i2cDevice_t i2cDevice[]; diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index 7cd909d542..0268642394 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -18,18 +18,19 @@ #include #include #include +#include #include #include "drivers/io.h" #include "drivers/time.h" +#include "drivers/nvic.h" +#include "drivers/rcc.h" #include "drivers/bus_i2c.h" -#include "drivers/nvic.h" -#include "io_impl.h" -#include "rcc.h" +#include "drivers/bus_i2c_impl.h" -#ifndef SOFT_I2C +#if defined(USE_I2C) && !defined(SOFT_I2C) #define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default) @@ -40,65 +41,52 @@ static void i2cUnstick(IO_t scl, IO_t sda); #define GPIO_AF_I2C GPIO_AF_I2C1 #ifdef STM32F4 - -#if defined(USE_I2C_PULLUP) -#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP) -#else -#define IOCFG_I2C IOCFG_AF_OD -#endif - -#ifndef I2C1_SCL -#define I2C1_SCL PB8 -#endif -#ifndef I2C1_SDA -#define I2C1_SDA PB9 -#endif - -#else - -#ifndef I2C1_SCL -#define I2C1_SCL PB6 -#endif -#ifndef I2C1_SDA -#define I2C1_SDA PB7 -#endif +#define IOCFG_I2C_PU IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP) +#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) +#else // STM32F4 #define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz) - #endif -#ifndef I2C2_SCL -#define I2C2_SCL PB10 +const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { +#ifdef USE_I2C_DEVICE_1 + { + .device = I2CDEV_1, + .reg = I2C1, + .sclPins = { DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) }, + .sdaPins = { DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) }, + .rcc = RCC_APB1(I2C1), + .ev_irq = I2C1_EV_IRQn, + .er_irq = I2C1_ER_IRQn, + }, #endif - -#ifndef I2C2_SDA -#define I2C2_SDA PB11 +#ifdef USE_I2C_DEVICE_2 + { + .device = I2CDEV_2, + .reg = I2C2, + .sclPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PF1) }, + .sdaPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PF0) }, + .rcc = RCC_APB1(I2C2), + .ev_irq = I2C2_EV_IRQn, + .er_irq = I2C2_ER_IRQn, + }, #endif - -#ifdef STM32F4 -#ifndef I2C3_SCL -#define I2C3_SCL PA8 -#endif -#ifndef I2C3_SDA -#define I2C3_SDA PC9 -#endif -#endif - -static i2cDevice_t i2cHardwareMap[] = { - { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn }, - { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn }, -#ifdef STM32F4 - { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn } +#ifdef USE_I2C_DEVICE_3 + { + .device = I2CDEV_3, + .reg = I2C3, + .sclPins = { DEFIO_TAG_E(PA8) }, + .sdaPins = { DEFIO_TAG_E(PC9) }, + .rcc = RCC_APB1(I2C3), + .ev_irq = I2C3_EV_IRQn, + .er_irq = I2C3_ER_IRQn, + }, #endif }; +i2cDevice_t i2cDevice[I2CDEV_COUNT]; + static volatile uint16_t i2cErrorCount = 0; -static i2cState_t i2cState[] = { - { false, false, 0, 0, 0, 0, 0, 0, 0 }, - { false, false, 0, 0, 0, 0, 0, 0, 0 }, - { false, false, 0, 0, 0, 0, 0, 0, 0 } -}; - void I2C1_ER_IRQHandler(void) { i2c_er_handler(I2CDEV_1); } @@ -135,18 +123,19 @@ static bool i2cHandleHardwareFailure(I2CDevice device) bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) { - - if (device == I2CINVALID) + if (device == I2CINVALID || device > I2CDEV_COUNT) { return false; + } + I2C_TypeDef *I2Cx = i2cDevice[device].reg; + + if (!I2Cx) { + return false; + } + + i2cState_t *state = &i2cDevice[device].state; uint32_t timeout = I2C_DEFAULT_TIMEOUT; - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; - - i2cState_t *state; - state = &(i2cState[device]); - state->addr = addr_ << 1; state->reg = reg_; state->writing = 1; @@ -182,17 +171,19 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { - if (device == I2CINVALID) + if (device == I2CINVALID || device > I2CDEV_COUNT) { return false; + } + I2C_TypeDef *I2Cx = i2cDevice[device].reg; + + if (!I2Cx) { + return false; + } + + i2cState_t *state = &i2cDevice[device].state; uint32_t timeout = I2C_DEFAULT_TIMEOUT; - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; - - i2cState_t *state; - state = &(i2cState[device]); - state->addr = addr_ << 1; state->reg = reg_; state->writing = 0; @@ -223,11 +214,9 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t static void i2c_er_handler(I2CDevice device) { - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2C_TypeDef *I2Cx = i2cDevice[device].hardware->reg; - i2cState_t *state; - state = &(i2cState[device]); + i2cState_t *state = &i2cDevice[device].state; // Read the I2C1 status register volatile uint32_t SR1Register = I2Cx->SR1; @@ -258,11 +247,9 @@ static void i2c_er_handler(I2CDevice device) { void i2c_ev_handler(I2CDevice device) { - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2C_TypeDef *I2Cx = i2cDevice[device].hardware->reg; - i2cState_t *state; - state = &(i2cState[device]); + i2cState_t *state = &i2cDevice[device].state; static uint8_t subaddress_sent, final_stop; // flag to indicate if subaddess sent, flag to indicate final bus condition static int8_t index; // index is signed -1 == send the subaddress @@ -378,65 +365,72 @@ void i2cInit(I2CDevice device) if (device == I2CINVALID) return; - i2cDevice_t *i2c; - i2c = &(i2cHardwareMap[device]); + i2cDevice_t *pDev = &i2cDevice[device]; + const i2cHardware_t *hw = pDev->hardware; + + if (!hw) { + return; + } + + I2C_TypeDef *I2Cx = hw->reg; + + memset(&pDev->state, 0, sizeof(pDev->state)); NVIC_InitTypeDef nvic; I2C_InitTypeDef i2cInit; - IO_t scl = IOGetByTag(i2c->scl); - IO_t sda = IOGetByTag(i2c->sda); + IO_t scl = pDev->scl; + IO_t sda = pDev->sda; IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device)); IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device)); // Enable RCC - RCC_ClockCmd(i2c->rcc, ENABLE); + RCC_ClockCmd(hw->rcc, ENABLE); - I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE); + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); i2cUnstick(scl, sda); // Init pins #ifdef STM32F4 - IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C); - IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_I2C); + IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_I2C); + IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_I2C); #else IOConfigGPIO(scl, IOCFG_I2C); IOConfigGPIO(sda, IOCFG_I2C); #endif - I2C_DeInit(i2c->dev); + I2C_DeInit(I2Cx); I2C_StructInit(&i2cInit); - I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts - they are enabled by the first request + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts - they are enabled by the first request i2cInit.I2C_Mode = I2C_Mode_I2C; i2cInit.I2C_DutyCycle = I2C_DutyCycle_2; i2cInit.I2C_OwnAddress1 = 0; i2cInit.I2C_Ack = I2C_Ack_Enable; i2cInit.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - if (i2c->overClock) { + if (pDev->overClock) { i2cInit.I2C_ClockSpeed = 800000; // 800khz Maximum speed tested on various boards without issues } else { i2cInit.I2C_ClockSpeed = 400000; // 400khz Operation according specs } - I2C_Cmd(i2c->dev, ENABLE); - I2C_Init(i2c->dev, &i2cInit); - - I2C_StretchClockCmd(i2c->dev, ENABLE); + I2C_Cmd(I2Cx, ENABLE); + I2C_Init(I2Cx, &i2cInit); + I2C_StretchClockCmd(I2Cx, ENABLE); // I2C ER Interrupt - nvic.NVIC_IRQChannel = i2c->er_irq; + nvic.NVIC_IRQChannel = hw->er_irq; nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER); nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER); nvic.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&nvic); // I2C EV Interrupt - nvic.NVIC_IRQChannel = i2c->ev_irq; + nvic.NVIC_IRQChannel = hw->ev_irq; nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV); nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV); NVIC_Init(&nvic); diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 3869c179ab..d1c5c97582 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -17,54 +17,57 @@ #include #include +#include #include +#include "build/debug.h" + #include "drivers/system.h" #include "drivers/io.h" -#include "io_impl.h" -#include "rcc.h" +#include "drivers/io_impl.h" +#include "drivers/rcc.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_impl.h" -#ifndef SOFT_I2C +#if defined(USE_I2C) && !defined(SOFT_I2C) -#if defined(USE_I2C_PULLUP) -#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP) -#else -#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL) -#endif +#define IOCFG_I2C_PU IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP) +#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL) #define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. #define I2C_STANDARD_TIMING 0x00E0257A // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. -#define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) -#define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_GPIO_AF GPIO_AF_4 -#ifndef I2C1_SCL -#define I2C1_SCL PB6 -#endif -#ifndef I2C1_SDA -#define I2C1_SDA PB7 -#endif -#ifndef I2C2_SCL -#define I2C2_SCL PF4 -#endif -#ifndef I2C2_SDA -#define I2C2_SDA PA10 -#endif - static uint32_t i2cTimeout; static volatile uint16_t i2cErrorCount = 0; -//static volatile uint16_t i2c2ErrorCount = 0; -static i2cDevice_t i2cHardwareMap[] = { - { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK }, - { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK } +const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { +#ifdef USE_I2C_DEVICE_1 + { + .device = I2CDEV_1, + .reg = I2C1, + .sclPins = { DEFIO_TAG_E(PA15), DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) }, + .sdaPins = { DEFIO_TAG_E(PA14), DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) }, + .rcc = RCC_APB1(I2C1), + }, +#endif +#ifdef USE_I2C_DEVICE_2 + { + .device = I2CDEV_2, + .reg = I2C2, + .sclPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PF6) }, + .sdaPins = { DEFIO_TAG_E(PA10) }, + .rcc = RCC_APB1(I2C2), + }, +#endif }; +i2cDevice_t i2cDevice[I2CDEV_COUNT]; + /////////////////////////////////////////////////////////////////////////////// // I2C TimeoutUserCallback /////////////////////////////////////////////////////////////////////////////// @@ -77,24 +80,30 @@ uint32_t i2cTimeoutUserCallback(void) void i2cInit(I2CDevice device) { + if (device == I2CINVALID || device > I2CDEV_COUNT) { + return; + } - i2cDevice_t *i2c; - i2c = &(i2cHardwareMap[device]); + i2cDevice_t *pDev = &i2cDevice[device]; + const i2cHardware_t *hw = pDev->hardware; - I2C_TypeDef *I2Cx; - I2Cx = i2c->dev; + if (!hw) { + return; + } - IO_t scl = IOGetByTag(i2c->scl); - IO_t sda = IOGetByTag(i2c->sda); + I2C_TypeDef *I2Cx = pDev->reg; - RCC_ClockCmd(i2c->rcc, ENABLE); + IO_t scl = pDev->scl; + IO_t sda = pDev->sda; + + RCC_ClockCmd(hw->rcc, ENABLE); RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device)); - IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_4); + IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_4); IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device)); - IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_4); + IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_4); I2C_InitTypeDef i2cInit = { .I2C_Mode = I2C_Mode_I2C, @@ -103,7 +112,7 @@ void i2cInit(I2CDevice device) .I2C_OwnAddress1 = 0x00, .I2C_Ack = I2C_Ack_Enable, .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_Timing = (i2c->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING) + .I2C_Timing = (pDev->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING) }; I2C_Init(I2Cx, &i2cInit); @@ -120,10 +129,17 @@ uint16_t i2cGetErrorCounter(void) bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) { - addr_ <<= 1; + if (device == I2CINVALID || device > I2CDEV_COUNT) { + return false; + } - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2C_TypeDef *I2Cx = i2cDevice[device].reg; + + if (!I2Cx) { + return false; + } + + addr_ <<= 1; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; @@ -186,10 +202,17 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) { - addr_ <<= 1; + if (device == I2CINVALID || device > I2CDEV_COUNT) { + return false; + } - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2C_TypeDef *I2Cx = i2cDevice[device].reg; + + if (!I2Cx) { + return false; + } + + addr_ <<= 1; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; diff --git a/src/main/drivers/display_ug2864hsweg01.c b/src/main/drivers/display_ug2864hsweg01.c index 9c14f15620..b9ac8528e5 100644 --- a/src/main/drivers/display_ug2864hsweg01.c +++ b/src/main/drivers/display_ug2864hsweg01.c @@ -24,6 +24,8 @@ #include "display_ug2864hsweg01.h" +#ifdef USE_I2C_OLED_DISPLAY + #if !defined(OLED_I2C_INSTANCE) #if defined(I2C_DEVICE) #define OLED_I2C_INSTANCE I2C_DEVICE @@ -288,3 +290,4 @@ bool ug2864hsweg01InitI2C(void) return true; } +#endif // USE_I2C_OLED_DISPLAY diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 858ee1ff08..a21672640a 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -17,24 +17,49 @@ #include "platform.h" +#include "config/parameter_group_ids.h" + #include "drivers/io.h" #include "io_impl.h" #include "light_led.h" -static IO_t leds[LED_NUMBER]; -static uint8_t ledPolarity = 0; +PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); + +static IO_t leds[STATUS_LED_NUMBER]; +static uint8_t ledInversion = 0; + +void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) +{ +#ifdef LED0_PIN + statusLedConfig->ioTags[0] = IO_TAG(LED0_PIN); +#endif +#ifdef LED1_PIN + statusLedConfig->ioTags[1] = IO_TAG(LED1_PIN); +#endif +#ifdef LED2_PIN + statusLedConfig->ioTags[2] = IO_TAG(LED2_PIN); +#endif + + statusLedConfig->inversion = 0 +#ifdef LED0_INVERTED + | BIT(0) +#endif +#ifdef LED1_INVERTED + | BIT(1) +#endif +#ifdef LED2_INVERTED + | BIT(2) +#endif + ; +} void ledInit(const statusLedConfig_t *statusLedConfig) { - LED0_OFF; - LED1_OFF; - LED2_OFF; - - ledPolarity = statusLedConfig->polarity; - for (int i = 0; i < LED_NUMBER; i++) { - if (statusLedConfig->ledTags[i]) { - leds[i] = IOGetByTag(statusLedConfig->ledTags[i]); + ledInversion = statusLedConfig->inversion; + for (int i = 0; i < STATUS_LED_NUMBER; i++) { + if (statusLedConfig->ioTags[i]) { + leds[i] = IOGetByTag(statusLedConfig->ioTags[i]); IOInit(leds[i], OWNER_LED, RESOURCE_INDEX(i)); IOConfigGPIO(leds[i], IOCFG_OUT_PP); } else { @@ -54,6 +79,6 @@ void ledToggle(int led) void ledSet(int led, bool on) { - const bool inverted = (1 << (led)) & ledPolarity; + const bool inverted = (1 << (led)) & ledInversion; IOWrite(leds[led], on ? inverted : !inverted); } diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 341bbb81d9..198a3f3244 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -19,47 +19,48 @@ #include "config/parameter_group.h" #include "drivers/io_types.h" +#include "common/utils.h" -#define LED_NUMBER 3 +#define STATUS_LED_NUMBER 3 typedef struct statusLedConfig_s { - ioTag_t ledTags[LED_NUMBER]; - uint8_t polarity; + ioTag_t ioTags[STATUS_LED_NUMBER]; + uint8_t inversion; } statusLedConfig_t; PG_DECLARE(statusLedConfig_t, statusLedConfig); // Helpful macros -#ifdef LED0 -# define LED0_TOGGLE ledToggle(0) -# define LED0_OFF ledSet(0, false) -# define LED0_ON ledSet(0, true) -#else -# define LED0_TOGGLE do {} while(0) -# define LED0_OFF do {} while(0) -# define LED0_ON do {} while(0) -#endif +#if defined(UNIT_TEST) || defined(USE_FAKE_LED) -#ifdef LED1 -# define LED1_TOGGLE ledToggle(1) -# define LED1_OFF ledSet(1, false) -# define LED1_ON ledSet(1, true) -#else -# define LED1_TOGGLE do {} while(0) -# define LED1_OFF do {} while(0) -# define LED1_ON do {} while(0) -#endif +#define LED0_TOGGLE NOOP +#define LED0_OFF NOOP +#define LED0_ON NOOP + +#define LED1_TOGGLE NOOP +#define LED1_OFF NOOP +#define LED1_ON NOOP + +#define LED2_TOGGLE NOOP +#define LED2_OFF NOOP +#define LED2_ON NOOP -#ifdef LED2 -# define LED2_TOGGLE ledToggle(2) -# define LED2_OFF ledSet(2, false) -# define LED2_ON ledSet(2, true) #else -# define LED2_TOGGLE do {} while(0) -# define LED2_OFF do {} while(0) -# define LED2_ON do {} while(0) -#endif + +#define LED0_TOGGLE ledToggle(0) +#define LED0_OFF ledSet(0, false) +#define LED0_ON ledSet(0, true) + +#define LED1_TOGGLE ledToggle(1) +#define LED1_OFF ledSet(1, false) +#define LED1_ON ledSet(1, true) + +#define LED2_TOGGLE ledToggle(2) +#define LED2_OFF ledSet(2, false) +#define LED2_ON ledSet(2, true) void ledInit(const statusLedConfig_t *statusLedConfig); void ledToggle(int led); void ledSet(int led, bool state); + +#endif diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 1624598a56..a7da53c8b4 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -125,40 +125,42 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard *port->ccr = 0; } -static void pwmWriteUnused(uint8_t index, uint16_t value) +static void pwmWriteUnused(uint8_t index, float value) { UNUSED(index); UNUSED(value); } -static void pwmWriteBrushed(uint8_t index, uint16_t value) +static void pwmWriteBrushed(uint8_t index, float value) { - *motors[index].ccr = (value - 1000) * motors[index].period / 1000; + *motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000); } -static void pwmWriteStandard(uint8_t index, uint16_t value) +static void pwmWriteStandard(uint8_t index, float value) { - *motors[index].ccr = value; + *motors[index].ccr = lrintf(value); } -static void pwmWriteOneShot125(uint8_t index, uint16_t value) +static void pwmWriteOneShot125(uint8_t index, float value) { - *motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); + *motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f); } -static void pwmWriteOneShot42(uint8_t index, uint16_t value) +static void pwmWriteOneShot42(uint8_t index, float value) { - *motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); + *motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f); } -static void pwmWriteMultiShot(uint8_t index, uint16_t value) +static void pwmWriteMultiShot(uint8_t index, float value) { - *motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); + *motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); } -void pwmWriteMotor(uint8_t index, uint16_t value) +void pwmWriteMotor(uint8_t index, float value) { - pwmWritePtr(index, value); + if (pwmMotorsEnabled) { + pwmWritePtr(index, value); + } } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) @@ -245,11 +247,16 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 idlePulse = 0; break; #ifdef USE_DSHOT + case PWM_TYPE_PROSHOT1000: + pwmWritePtr = pwmWriteProShot; + pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; + isDigital = true; + break; case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - pwmWritePtr = pwmWriteDigital; + pwmWritePtr = pwmWriteDshot; pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; break; @@ -320,15 +327,17 @@ pwmOutputPort_t *pwmGetMotors(void) uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT1200): - return MOTOR_DSHOT1200_MHZ * 1000000; - case(PWM_TYPE_DSHOT600): - return MOTOR_DSHOT600_MHZ * 1000000; - case(PWM_TYPE_DSHOT300): - return MOTOR_DSHOT300_MHZ * 1000000; - default: - case(PWM_TYPE_DSHOT150): - return MOTOR_DSHOT150_MHZ * 1000000; + case(PWM_TYPE_PROSHOT1000): + return MOTOR_PROSHOT1000_MHZ * 1000000; + case(PWM_TYPE_DSHOT1200): + return MOTOR_DSHOT1200_MHZ * 1000000; + case(PWM_TYPE_DSHOT600): + return MOTOR_DSHOT600_MHZ * 1000000; + case(PWM_TYPE_DSHOT300): + return MOTOR_DSHOT300_MHZ * 1000000; + default: + case(PWM_TYPE_DSHOT150): + return MOTOR_DSHOT150_MHZ * 1000000; } } @@ -338,11 +347,21 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) motorDmaOutput_t *const motor = getMotorDmaOutput(index); unsigned repeats; - if ((command >= 7 && command <= 10) || command == 12) { + switch (command) { + case DSHOT_CMD_SPIN_DIRECTION_1: + case DSHOT_CMD_SPIN_DIRECTION_2: + case DSHOT_CMD_3D_MODE_OFF: + case DSHOT_CMD_3D_MODE_ON: + case DSHOT_CMD_SAVE_SETTINGS: + case DSHOT_CMD_SPIN_DIRECTION_NORMAL: + case DSHOT_CMD_SPIN_DIRECTION_REVERSED: repeats = 10; - } else { + break; + default: repeats = 1; + break; } + for (; repeats; repeats--) { motor->requestTelemetry = true; pwmWritePtr(index, command); @@ -355,10 +374,10 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) #endif #ifdef USE_SERVOS -void pwmWriteServo(uint8_t index, uint16_t value) +void pwmWriteServo(uint8_t index, float value) { if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) { - *servos[index].ccr = value; + *servos[index].ccr = lrintf(value); } } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f4249f4adf..1c3e792a49 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,6 +28,26 @@ #define MAX_SUPPORTED_SERVOS 8 #endif +typedef enum { + DSHOT_CMD_MOTOR_STOP = 0, + DSHOT_CMD_BEEP1, + DSHOT_CMD_BEEP2, + DSHOT_CMD_BEEP3, + DSHOT_CMD_BEEP4, + DSHOT_CMD_BEEP5, + DSHOT_CMD_ESC_INFO, + DSHOT_CMD_SPIN_DIRECTION_1, + DSHOT_CMD_SPIN_DIRECTION_2, + DSHOT_CMD_3D_MODE_OFF, + DSHOT_CMD_3D_MODE_ON, + DSHOT_CMD_SETTINGS_REQUEST, + DSHOT_CMD_SAVE_SETTINGS, + DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, //Blheli_S only command + DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, //Blheli_S only command + DSHOT_CMD_MAX = 47 +} dshotCommands_e; + + typedef enum { PWM_TYPE_STANDARD = 0, PWM_TYPE_ONESHOT125, @@ -39,6 +59,7 @@ typedef enum { PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT600, PWM_TYPE_DSHOT1200, + PWM_TYPE_PROSHOT1000, #endif PWM_TYPE_MAX } motorPwmProtocolTypes_e; @@ -56,6 +77,11 @@ typedef enum { #define MOTOR_BIT_0 7 #define MOTOR_BIT_1 14 #define MOTOR_BITLENGTH 19 + +#define MOTOR_PROSHOT1000_MHZ 24 +#define PROSHOT_BASE_SYMBOL 24 // 1uS +#define PROSHOT_BIT_WIDTH 3 +#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS #endif #if defined(STM32F40_41xxx) // must be multiples of timer clock @@ -75,7 +101,8 @@ typedef enum { #define PWM_BRUSHED_TIMER_MHZ 24 #endif -#define MOTOR_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define PROSHOT_DMA_BUFFER_SIZE 6/* resolution + frame reset (2us) */ typedef struct { TIM_TypeDef *timer; @@ -89,9 +116,9 @@ typedef struct { uint16_t timerDmaSource; volatile bool requestTelemetry; #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) - uint32_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE]; + uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; #else - uint8_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE]; + uint8_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; #endif #if defined(STM32F7) TIM_HandleTypeDef TimHandle; @@ -104,7 +131,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index); extern bool pwmMotorsEnabled; struct timerHardware_s; -typedef void(*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors +typedef void(*pwmWriteFuncPtr)(uint8_t index, float value); // function pointer used to write motors typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointer used after motors are written typedef struct { @@ -140,7 +167,8 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDshotCommand(uint8_t index, uint8_t command); -void pwmWriteDigital(uint8_t index, uint16_t value); +void pwmWriteProShot(uint8_t index, float value); +void pwmWriteDshot(uint8_t index, float value); void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); #endif @@ -151,11 +179,11 @@ void pwmToggleBeeper(void); void beeperPwmInit(IO_t io, uint16_t frequency); #endif -void pwmWriteMotor(uint8_t index, uint16_t value); +void pwmWriteMotor(uint8_t index, float value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(uint8_t motorCount); -void pwmWriteServo(uint8_t index, uint16_t value); +void pwmWriteServo(uint8_t index, float value); pwmOutputPort_t *pwmGetMotors(void); bool pwmIsSynced(void); diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 38a4c1ccd5..c75ac74a03 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -54,12 +54,9 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount-1; } -void pwmWriteDigital(uint8_t index, uint16_t value) +void pwmWriteDshot(uint8_t index, float value) { - - if (!pwmMotorsEnabled) { - return; - } + const uint16_t digitalValue = lrintf(value); motorDmaOutput_t * const motor = &dmaMotors[index]; @@ -67,7 +64,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value) return; } - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); + uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row // compute checksum @@ -86,7 +83,41 @@ void pwmWriteDigital(uint8_t index, uint16_t value) packet <<= 1; } - DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, MOTOR_DMA_BUFFER_SIZE); + DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE); + DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); +} + +void pwmWriteProShot(uint8_t index, float value) +{ + const uint16_t digitalValue = lrintf(value); + + motorDmaOutput_t * const motor = &dmaMotors[index]; + + if (!motor->timerHardware || !motor->timerHardware->dmaRef) { + return; + } + + uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); + motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + + // compute checksum + int csum = 0; + int csum_data = packet; + for (int i = 0; i < 3; i++) { + csum ^= csum_data; // xor data by nibbles + csum_data >>= 4; + } + csum &= 0xf; + // append checksum + packet = (packet << 4) | csum; + + // generate pulses for Proshot + for (int i = 0; i < 4; i++) { + motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first + packet <<= 4; // Shift 4 bits + } + + DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, PROSHOT_DMA_BUFFER_SIZE); DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); } @@ -139,7 +170,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_Cmd(timer, DISABLE); TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((timerClock(timer) / getDshotHz(pwmProtocolType)) - 1); - TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; + TIM_TimeBaseStructure.TIM_Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; @@ -205,7 +236,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; #endif DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware); - DMA_InitStructure.DMA_BufferSize = MOTOR_DMA_BUFFER_SIZE; + DMA_InitStructure.DMA_BufferSize = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 01083add6c..c2ff4737e1 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -49,11 +49,9 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount - 1; } -void pwmWriteDigital(uint8_t index, uint16_t value) +void pwmWriteDshot(uint8_t index, float value) { - if (!pwmMotorsEnabled) { - return; - } + const uint16_t digitalValue = lrintf(value); motorDmaOutput_t * const motor = &dmaMotors[index]; @@ -61,7 +59,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value) return; } - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); + uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row // compute checksum @@ -81,12 +79,55 @@ void pwmWriteDigital(uint8_t index, uint16_t value) } if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) { + if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ return; } } else { - if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) { + if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) { + /* Starting PWM generation Error */ + return; + } + } +} + +void pwmWriteProShot(uint8_t index, float value) +{ + const uint16_t digitalValue = lrintf(value); + + motorDmaOutput_t * const motor = &dmaMotors[index]; + + if (!motor->timerHardware || !motor->timerHardware->dmaRef) { + return; + } + + uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); + motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + + // compute checksum + int csum = 0; + int csum_data = packet; + for (int i = 0; i < 3; i++) { + csum ^= csum_data; // xor data by nibbles + csum_data >>= 4; + } + csum &= 0xf; + // append checksum + packet = (packet << 4) | csum; + + // generate pulses for Proshot + for (int i = 0; i < 4; i++) { + motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first + packet <<= 4; // Shift 4 bits + } + + if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) { + /* Starting PWM generation Error */ + return; + } + } else { + if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ return; } @@ -122,8 +163,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); motor->TimHandle.Instance = timerHardware->tim; - motor->TimHandle.Init.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1;; - motor->TimHandle.Init.Period = MOTOR_BITLENGTH; + motor->TimHandle.Init.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1; + motor->TimHandle.Init.Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; motor->TimHandle.Init.RepetitionCounter = 0; motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index a21ab96868..fa213eeab0 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -58,6 +58,8 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { } } +// XXX uartReconfigure does not handle resource management properly. + void uartReconfigure(uartPort_t *uartPort) { /*RCC_PeriphCLKInitTypeDef RCC_PeriphClkInit; diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 8dd877c940..881d634286 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -184,7 +184,7 @@ void serialUARTInitIO(IO_t txIO, IO_t rxIO, portMode_t mode, portOptions_t optio ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP ); - IOInit(txIO, OWNER_SERIAL_TX, index); + IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(index)); IOConfigGPIOAF(txIO, ioCfg, af); if (!(options & SERIAL_INVERTED)) @@ -192,12 +192,12 @@ void serialUARTInitIO(IO_t txIO, IO_t rxIO, portMode_t mode, portOptions_t optio } else { ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP); if ((mode & MODE_TX) && txIO) { - IOInit(txIO, OWNER_SERIAL_TX, index); + IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(index)); IOConfigGPIOAF(txIO, ioCfg, af); } if ((mode & MODE_RX) && rxIO) { - IOInit(rxIO, OWNER_SERIAL_RX, index); + IOInit(rxIO, OWNER_SERIAL_RX, RESOURCE_INDEX(index)); IOConfigGPIOAF(rxIO, ioCfg, af); } } diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index b6f2d423e0..536023fd27 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -83,7 +83,7 @@ void systemInit(void) // Turn on clocks for stuff we use RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; RCC_ClearFlag(); diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index a00abdfba0..f3a349706a 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -90,7 +90,7 @@ void systemInit(void) // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; RCC_ClearFlag(); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index de32333124..5b7e155640 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -152,7 +152,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void) bool isMPUSoftReset(void) { - if (RCC->CSR & RCC_CSR_SFTRSTF) + if (cachedRccCsrValue & RCC_CSR_SFTRSTF) return true; else return false; @@ -167,7 +167,7 @@ void systemInit(void) // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index db4f9085ea..bfe8039dde 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -149,7 +149,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void) bool isMPUSoftReset(void) { - if (RCC->CSR & RCC_CSR_SFTRSTF) + if (cachedRccCsrValue & RCC_CSR_SFTRSTF) return true; else return false; @@ -164,7 +164,7 @@ void systemInit(void) // Configure NVIC preempt/priority groups HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITY_GROUPING); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 97a6d5f706..83bcf40051 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -75,6 +75,7 @@ extern uint8_t __config_end; #include "drivers/time.h" #include "drivers/timer.h" #include "drivers/vcd.h" +#include "drivers/light_led.h" #include "fc/settings.h" #include "fc/cli.h" @@ -126,10 +127,20 @@ extern uint8_t __config_end; static serialPort_t *cliPort; -static bufWriter_t *cliWriter; -static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128]; -static char cliBuffer[64]; +#ifdef STM32F1 +#define CLI_IN_BUFFER_SIZE 128 +#define CLI_OUT_BUFFER_SIZE 64 +#else +// Space required to set array parameters +#define CLI_IN_BUFFER_SIZE 256 +#define CLI_OUT_BUFFER_SIZE 256 +#endif + +static bufWriter_t *cliWriter; +static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_IN_BUFFER_SIZE]; + +static char cliBuffer[CLI_OUT_BUFFER_SIZE]; static uint32_t bufferIndex = 0; static bool configIsInCopy = false; @@ -295,33 +306,44 @@ static void cliPrintLinef(const char *format, ...) static void printValuePointer(const clivalue_t *var, const void *valuePointer, bool full) { - int value = 0; + if ((var->type & VALUE_MODE_MASK) == MODE_ARRAY) { + for (int i = 0; i < var->config.array.length; i++) { + uint8_t value = ((uint8_t *)valuePointer)[i]; - switch (var->type & VALUE_TYPE_MASK) { - case VAR_UINT8: - value = *(uint8_t *)valuePointer; - break; - - case VAR_INT8: - value = *(int8_t *)valuePointer; - break; - - case VAR_UINT16: - case VAR_INT16: - value = *(int16_t *)valuePointer; - break; - } - - switch(var->type & VALUE_MODE_MASK) { - case MODE_DIRECT: - cliPrintf("%d", value); - if (full) { - cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + cliPrintf("%d", value); + if (i < var->config.array.length - 1) { + cliPrint(","); + } + } + } else { + int value = 0; + + switch (var->type & VALUE_TYPE_MASK) { + case VAR_UINT8: + value = *(uint8_t *)valuePointer; + break; + + case VAR_INT8: + value = *(int8_t *)valuePointer; + break; + + case VAR_UINT16: + case VAR_INT16: + value = *(int16_t *)valuePointer; + break; + } + + switch(var->type & VALUE_MODE_MASK) { + case MODE_DIRECT: + cliPrintf("%d", value); + if (full) { + cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + } + break; + case MODE_LOOKUP: + cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); + break; } - break; - case MODE_LOOKUP: - cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); - break; } } @@ -379,14 +401,15 @@ static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) const char *defaultFormat = "#set %s = "; const int valueOffset = getValueOffset(value); const bool equalsDefault = valuePtrEqualsDefault(value->type, pg->copy + valueOffset, pg->address + valueOffset); + if (((dumpMask & DO_DIFF) == 0) || !equalsDefault) { if (dumpMask & SHOW_DEFAULTS && !equalsDefault) { cliPrintf(defaultFormat, value->name); - printValuePointer(value, (uint8_t*)pg->address + valueOffset, 0); + printValuePointer(value, (uint8_t*)pg->address + valueOffset, false); cliPrintLinefeed(); } cliPrintf(format, value->name); - printValuePointer(value, pg->copy + valueOffset, 0); + printValuePointer(value, pg->copy + valueOffset, false); cliPrintLinefeed(); } } @@ -426,26 +449,31 @@ static void cliPrintVarRange(const clivalue_t *var) } cliPrintLinefeed(); } + break; + case (MODE_ARRAY): { + cliPrintLinef("Array length: %d", var->config.array.length); + } + break; } } -static void cliSetVar(const clivalue_t *var, const cliVar_t value) +static void cliSetVar(const clivalue_t *var, const int16_t value) { void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: - *(uint8_t *)ptr = value.uint8; + *(uint8_t *)ptr = value; break; case VAR_INT8: - *(int8_t *)ptr = value.int8; + *(int8_t *)ptr = value; break; case VAR_UINT16: case VAR_INT16: - *(int16_t *)ptr = value.int16; + *(int16_t *)ptr = value; break; } } @@ -2509,18 +2537,34 @@ static void cliGet(char *cmdline) cliPrintLine("Invalid name"); } +static char *skipSpace(char *buffer) +{ + while (*(buffer) == ' ') { + buffer++; + } + + return buffer; +} + +static uint8_t getWordLength(char *bufBegin, char *bufEnd) +{ + while (*(bufEnd - 1) == ' ') { + bufEnd--; + } + + return bufEnd - bufBegin; +} + static void cliSet(char *cmdline) { - uint32_t len; - const clivalue_t *val; - char *eqptr = NULL; - - len = strlen(cmdline); + const uint32_t len = strlen(cmdline); + char *eqptr; if (len == 0 || (len == 1 && cmdline[0] == '*')) { cliPrintLine("Current settings: "); + for (uint32_t i = 0; i < valueTableEntryCount; i++) { - val = &valueTable[i]; + const clivalue_t *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 cliPrintLinefeed(); @@ -2528,52 +2572,81 @@ static void cliSet(char *cmdline) } else if ((eqptr = strstr(cmdline, "=")) != NULL) { // has equals - char *lastNonSpaceCharacter = eqptr; - while (*(lastNonSpaceCharacter - 1) == ' ') { - lastNonSpaceCharacter--; - } - uint8_t variableNameLength = lastNonSpaceCharacter - cmdline; + uint8_t variableNameLength = getWordLength(cmdline, eqptr); // skip the '=' and any ' ' characters eqptr++; - while (*(eqptr) == ' ') { - eqptr++; - } + eqptr = skipSpace(eqptr); for (uint32_t i = 0; i < valueTableEntryCount; i++) { - val = &valueTable[i]; + const clivalue_t *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)) { - bool changeValue = false; - cliVar_t value = { .int16 = 0 }; + bool valueChanged = false; + int16_t value = 0; switch (valueTable[i].type & VALUE_MODE_MASK) { case MODE_DIRECT: { - value.int16 = atoi(eqptr); + int16_t value = atoi(eqptr); - if (value.int16 >= valueTable[i].config.minmax.min && value.int16 <= valueTable[i].config.minmax.max) { - changeValue = true; - } + if (value >= valueTable[i].config.minmax.min && value <= valueTable[i].config.minmax.max) { + cliSetVar(val, value); + valueChanged = true; } - break; + } + + break; case MODE_LOOKUP: { - const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex]; - bool matched = false; - for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { - matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; + const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex]; + bool matched = false; + for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { + matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; - if (matched) { - value.int16 = tableValueIndex; - changeValue = true; - } + if (matched) { + value = tableValueIndex; + + cliSetVar(val, value); + valueChanged = true; } } - break; + } + + break; + case MODE_ARRAY: { + const uint8_t arrayLength = valueTable[i].config.array.length; + char *valPtr = eqptr; + uint8_t array[256]; + char curVal[4]; + for (int i = 0; i < arrayLength; i++) { + valPtr = skipSpace(valPtr); + char *valEnd = strstr(valPtr, ","); + if ((valEnd != NULL) && (i < arrayLength - 1)) { + uint8_t varLength = getWordLength(valPtr, valEnd); + if (varLength <= 3) { + strncpy(curVal, valPtr, getWordLength(valPtr, valEnd)); + curVal[varLength] = '\0'; + array[i] = (uint8_t)atoi((const char *)curVal); + valPtr = valEnd + 1; + } else { + break; + } + } else if ((valEnd == NULL) && (i == arrayLength - 1)) { + array[i] = atoi(valPtr); + + uint8_t *ptr = getValuePointer(val); + memcpy(ptr, array, arrayLength); + valueChanged = true; + } else { + break; + } + } + } + + break; + } - if (changeValue) { - cliSetVar(val, value); - + if (valueChanged) { cliPrintf("%s set to ", valueTable[i].name); cliPrintVar(val, 0); } else { @@ -2984,11 +3057,7 @@ static void backupConfigs(void) { // make copies of configs to do differencing PG_FOREACH(pg) { - if (pgIsProfile(pg)) { - //memcpy(pg->copy, pg->address, pg->size * MAX_PROFILE_COUNT); - } else { - memcpy(pg->copy, pg->address, pg->size); - } + memcpy(pg->copy, pg->address, pg->size); } configIsInCopy = true; @@ -2997,11 +3066,7 @@ static void backupConfigs(void) static void restoreConfigs(void) { PG_FOREACH(pg) { - if (pgIsProfile(pg)) { - //memcpy(pg->address, pg->copy, pg->size * MAX_PROFILE_COUNT); - } else { - memcpy(pg->address, pg->copy, pg->size); - } + memcpy(pg->address, pg->copy, pg->size); } configIsInCopy = false; diff --git a/src/main/fc/cli.c.orig b/src/main/fc/cli.c.orig new file mode 100755 index 0000000000..665991e146 --- /dev/null +++ b/src/main/fc/cli.c.orig @@ -0,0 +1,3509 @@ +/* + * 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 +#include +#include + +#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 +// signal that we're in cli mode +uint8_t cliMode = 0; +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; + +#ifdef USE_CLI + +#include "blackbox/blackbox.h" + +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + +#include "cms/cms.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" +#include "common/printf.h" +#include "common/typeconversion.h" +#include "common/utils.h" + +#include "config/config_eeprom.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/accgyro/accgyro.h" +#include "drivers/buf_writer.h" +#include "drivers/bus_i2c.h" +#include "drivers/compass/compass.h" +#include "drivers/display.h" +#include "drivers/dma.h" +#include "drivers/flash.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/inverter.h" +#include "drivers/rx_pwm.h" +#include "drivers/sdcard.h" +#include "drivers/sensor.h" +#include "drivers/serial.h" +#include "drivers/serial_escserial.h" +#include "drivers/sonar_hcsr04.h" +#include "drivers/stack_check.h" +#include "drivers/system.h" +#include "drivers/transponder_ir.h" +#include "drivers/time.h" +#include "drivers/timer.h" +#include "drivers/vcd.h" +#include "drivers/light_led.h" + +#include "fc/settings.h" +#include "fc/cli.h" +#include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/fc_core.h" +#include "fc/rc_adjustments.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" +#include "fc/fc_msp.h" + +#include "flight/altitude.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/flashfs.h" +#include "io/displayport_max7456.h" +#include "io/displayport_msp.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/osd.h" +#include "io/serial.h" +#include "io/transponder_ir.h" +#include "io/vtx_rtc6705.h" +#include "io/vtx_control.h" + +#include "rx/rx.h" +#include "rx/spektrum.h" + +#include "scheduler/scheduler.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/frsky.h" +#include "telemetry/telemetry.h" + + +static serialPort_t *cliPort; + +#ifdef STM32F1 +#define CLI_IN_BUFFER_SIZE 128 +#define CLI_OUT_BUFFER_SIZE 64 +#else +// Space required to set array parameters +#define CLI_IN_BUFFER_SIZE 256 +#define CLI_OUT_BUFFER_SIZE 256 +#endif + +static bufWriter_t *cliWriter; +static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_IN_BUFFER_SIZE]; + +static char cliBuffer[CLI_OUT_BUFFER_SIZE]; +static uint32_t bufferIndex = 0; + +static bool configIsInCopy = false; + +static const char* const emptyName = "-"; + +#ifndef USE_QUAD_MIXER_ONLY +// sync this with mixerMode_e +static const char * const mixerNames[] = { + "TRI", "QUADP", "QUADX", "BI", + "GIMBAL", "Y6", "HEX6", + "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX", + "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", + "HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER", + "ATAIL4", "CUSTOM", "CUSTOMAIRPLANE", "CUSTOMTRI", "QUADX1234", NULL +}; +#endif + +// sync this with features_e +static const char * const featureNames[] = { + "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", + "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", + "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", + "UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", + "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL +}; + +// sync this with rxFailsafeChannelMode_e +static const char rxFailsafeModeCharacters[] = "ahs"; + +static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT][RX_FAILSAFE_MODE_COUNT] = { + { RX_FAILSAFE_MODE_AUTO, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_INVALID }, + { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } +}; + +#if defined(USE_SENSOR_NAMES) +// sync this with sensors_e +static const char * const sensorTypeNames[] = { + "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL +}; + +#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) + +// sync with gyroSensor_e +static const char * const gyroNames[] = { + "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE" +}; + +static const char * const *sensorHardwareNames[] = { + gyroNames, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware + +}; +#endif // USE_SENSOR_NAMES + +static void cliPrint(const char *str) +{ + while (*str) { + bufWriterAppend(cliWriter, *str++); + } + bufWriterFlush(cliWriter); +} + +static void cliPrintLinefeed() +{ + cliPrint("\r\n"); +} + +static void cliPrintLine(const char *str) +{ + cliPrint(str); + cliPrintLinefeed(); +} + +#ifdef MINIMAL_CLI +#define cliPrintHashLine(str) +#else +static void cliPrintHashLine(const char *str) +{ + cliPrint("\r\n# "); + cliPrintLine(str); +} +#endif + +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 void cliPrintfva(const char *format, va_list va) +{ + tfp_format(cliWriter, cliPutp, format, va); + bufWriterFlush(cliWriter); +} + +static void cliPrintLinefva(const char *format, va_list va) +{ + tfp_format(cliWriter, cliPutp, format, va); + bufWriterFlush(cliWriter); + cliPrintLinefeed(); +} + +static bool cliDumpPrintLinef(uint8_t dumpMask, bool equalsDefault, const char *format, ...) +{ + if (!((dumpMask & DO_DIFF) && equalsDefault)) { + va_list va; + va_start(va, format); + cliPrintLinefva(format, va); + va_end(va); + return true; + } else { + return false; + } +} + +static void cliWrite(uint8_t ch) +{ + bufWriterAppend(cliWriter, ch); +} + +static bool cliDefaultPrintLinef(uint8_t dumpMask, bool equalsDefault, const char *format, ...) +{ + if ((dumpMask & SHOW_DEFAULTS) && !equalsDefault) { + cliWrite('#'); + + va_list va; + va_start(va, format); + cliPrintLinefva(format, va); + va_end(va); + return true; + } else { + return false; + } +} + +static void cliPrintf(const char *format, ...) +{ + va_list va; + va_start(va, format); + cliPrintfva(format, va); + va_end(va); +} + + +static void cliPrintLinef(const char *format, ...) +{ + va_list va; + va_start(va, format); + cliPrintLinefva(format, va); + va_end(va); +} + +static void printValuePointer(const clivalue_t *var, const void *valuePointer, bool full) +{ + if ((var->type & VALUE_MODE_MASK) == MODE_ARRAY) { + for (int i = 0; i < var->config.array.length; i++) { + uint8_t value = ((uint8_t *)valuePointer)[i]; + + cliPrintf("%d", value); + if (i < var->config.array.length - 1) { + cliPrint(","); + } + } + } else { + int value = 0; + + switch (var->type & VALUE_TYPE_MASK) { + case VAR_UINT8: + value = *(uint8_t *)valuePointer; + break; + + case VAR_INT8: + value = *(int8_t *)valuePointer; + break; + + case VAR_UINT16: + case VAR_INT16: + value = *(int16_t *)valuePointer; + break; + } + + switch(var->type & VALUE_MODE_MASK) { + case MODE_DIRECT: + cliPrintf("%d", value); + if (full) { + cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + } + break; + case MODE_LOOKUP: + cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); + break; + } + } +} + +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: + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; + } + + return result; +} + +static uint16_t getValueOffset(const clivalue_t *value) +{ + switch (value->type & VALUE_SECTION_MASK) { + case MASTER_VALUE: + return value->offset; + case PROFILE_VALUE: + return value->offset + sizeof(pidProfile_t) * getCurrentPidProfileIndex(); + case PROFILE_RATE_VALUE: + return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex(); + } + return 0; +} + +static void *getValuePointer(const clivalue_t *value) +{ + const pgRegistry_t* rec = pgFind(value->pgn); + return CONST_CAST(void *, rec->address + getValueOffset(value)); +} + +static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) +{ + const pgRegistry_t *pg = pgFind(value->pgn); +#ifdef DEBUG + if (!pg) { + cliPrintLinef("VALUE %s ERROR", value->name); + return; // if it's not found, the pgn shouldn't be in the value table! + } +#endif + + const char *format = "set %s = "; + const char *defaultFormat = "#set %s = "; + const int valueOffset = getValueOffset(value); + const bool equalsDefault = valuePtrEqualsDefault(value->type, pg->copy + valueOffset, pg->address + valueOffset); + + if (((dumpMask & DO_DIFF) == 0) || !equalsDefault) { + if (dumpMask & SHOW_DEFAULTS && !equalsDefault) { + cliPrintf(defaultFormat, value->name); + printValuePointer(value, (uint8_t*)pg->address + valueOffset, false); + cliPrintLinefeed(); + } + cliPrintf(format, value->name); + printValuePointer(value, pg->copy + valueOffset, false); + cliPrintLinefeed(); + } +} + +static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) +{ + for (uint32_t i = 0; i < valueTableEntryCount; i++) { + const clivalue_t *value = &valueTable[i]; + bufWriterFlush(cliWriter); + if ((value->type & VALUE_SECTION_MASK) == valueSection) { + dumpPgValue(value, dumpMask); + } + } +} + +static void cliPrintVar(const clivalue_t *var, bool full) +{ + const void *ptr = getValuePointer(var); + + printValuePointer(var, ptr, full); +} + +static void cliPrintVarRange(const clivalue_t *var) +{ + switch (var->type & VALUE_MODE_MASK) { + case (MODE_DIRECT): { + cliPrintLinef("Allowed range: %d - %d", var->config.minmax.min, var->config.minmax.max); + } + break; + case (MODE_LOOKUP): { + const lookupTableEntry_t *tableEntry = &lookupTables[var->config.lookup.tableIndex]; + cliPrint("Allowed values:"); + for (uint32_t i = 0; i < tableEntry->valueCount ; i++) { + if (i > 0) + cliPrint(","); + cliPrintf(" %s", tableEntry->values[i]); + } + cliPrintLinefeed(); + } + break; + case (MODE_ARRAY): { + cliPrintLinef("Array length: %d", var->config.array.length); + } + + break; + } +} + +static void cliSetVar(const clivalue_t *var, const int16_t value) +{ + void *ptr = getValuePointer(var); + + switch (var->type & VALUE_TYPE_MASK) { + case VAR_UINT8: + *(uint8_t *)ptr = value; + break; + + case VAR_INT8: + *(int8_t *)ptr = value; + break; + + case VAR_UINT16: + case VAR_INT16: + *(int16_t *)ptr = value; + break; + } +} + +#if defined(USE_RESOURCE_MGMT) && !defined(MINIMAL_CLI) +static void cliRepeat(char ch, uint8_t len) +{ + for (int i = 0; i < len; i++) { + bufWriterAppend(cliWriter, ch); + } + cliPrintLinefeed(); +} +#endif + +static void cliPrompt(void) +{ + cliPrint("\r\n# "); +} + +static void cliShowParseError(void) +{ + cliPrintLine("Parse error"); +} + +static void cliShowArgumentRangeError(char *name, int min, int max) +{ + cliPrintLinef("%s not between %d and %d", name, min, max); +} + +static const char *nextArg(const char *currentArg) +{ + const char *ptr = strchr(currentArg, ' '); + while (ptr && *ptr == ' ') { + ptr++; + } + + return ptr; +} + +static const char *processChannelRangeArgs(const char *ptr, channelRange_t *range, uint8_t *validArgumentCount) +{ + for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { + ptr = nextArg(ptr); + if (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) { + range->startStep = val; + } else { + range->endStep = val; + } + (*validArgumentCount)++; + } + } + } + + return ptr; +} + +// Check if a string's length is zero +static bool isEmpty(const char *string) +{ + return (string == NULL || *string == '\0') ? true : false; +} + +static void printRxFailsafe(uint8_t dumpMask, const rxFailsafeChannelConfig_t *rxFailsafeChannelConfigs, const rxFailsafeChannelConfig_t *defaultRxFailsafeChannelConfigs) +{ + // print out rxConfig failsafe settings + for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { + const rxFailsafeChannelConfig_t *channelFailsafeConfig = &rxFailsafeChannelConfigs[channel]; + const rxFailsafeChannelConfig_t *defaultChannelFailsafeConfig = &defaultRxFailsafeChannelConfigs[channel]; + const bool equalsDefault = channelFailsafeConfig->mode == defaultChannelFailsafeConfig->mode + && channelFailsafeConfig->step == defaultChannelFailsafeConfig->step; + const bool requireValue = channelFailsafeConfig->mode == RX_FAILSAFE_MODE_SET; + if (requireValue) { + const char *format = "rxfail %u %c %d"; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + channel, + rxFailsafeModeCharacters[defaultChannelFailsafeConfig->mode], + RXFAIL_STEP_TO_CHANNEL_VALUE(defaultChannelFailsafeConfig->step) + ); + cliDumpPrintLinef(dumpMask, equalsDefault, format, + channel, + rxFailsafeModeCharacters[channelFailsafeConfig->mode], + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step) + ); + } else { + const char *format = "rxfail %u %c"; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + channel, + rxFailsafeModeCharacters[defaultChannelFailsafeConfig->mode] + ); + cliDumpPrintLinef(dumpMask, equalsDefault, format, + channel, + rxFailsafeModeCharacters[channelFailsafeConfig->mode] + ); + } + } +} + +static void cliRxFailsafe(char *cmdline) +{ + uint8_t channel; + char buf[3]; + + if (isEmpty(cmdline)) { + // print out rxConfig failsafe settings + for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { + cliRxFailsafe(itoa(channel, buf, 10)); + } + } else { + const char *ptr = cmdline; + channel = atoi(ptr++); + if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) { + + rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigsMutable(channel); + + const rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX; + rxFailsafeChannelMode_e mode = channelFailsafeConfig->mode; + bool requireValue = channelFailsafeConfig->mode == RX_FAILSAFE_MODE_SET; + + ptr = nextArg(ptr); + if (ptr) { + const char *p = strchr(rxFailsafeModeCharacters, *(ptr)); + if (p) { + const uint8_t requestedMode = p - rxFailsafeModeCharacters; + mode = rxFailsafeModesTable[type][requestedMode]; + } else { + mode = RX_FAILSAFE_MODE_INVALID; + } + if (mode == RX_FAILSAFE_MODE_INVALID) { + cliShowParseError(); + return; + } + + requireValue = mode == RX_FAILSAFE_MODE_SET; + + ptr = nextArg(ptr); + if (ptr) { + if (!requireValue) { + cliShowParseError(); + return; + } + uint16_t value = atoi(ptr); + value = CHANNEL_VALUE_TO_RXFAIL_STEP(value); + if (value > MAX_RXFAIL_RANGE_STEP) { + cliPrintLine("Value out of range"); + return; + } + + channelFailsafeConfig->step = value; + } else if (requireValue) { + cliShowParseError(); + return; + } + channelFailsafeConfig->mode = mode; + } + + char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfig->mode]; + + // double use of cliPrintf below + // 1. acknowledge interpretation on command, + // 2. query current setting on single item, + + if (requireValue) { + cliPrintLinef("rxfail %u %c %d", + channel, + modeCharacter, + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step) + ); + } else { + cliPrintLinef("rxfail %u %c", + channel, + modeCharacter + ); + } + } else { + cliShowArgumentRangeError("channel", 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1); + } + } +} + +static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActivationConditions, const modeActivationCondition_t *defaultModeActivationConditions) +{ + const char *format = "aux %u %u %u %u %u"; + // print out aux channel settings + for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + const modeActivationCondition_t *mac = &modeActivationConditions[i]; + bool equalsDefault = false; + if (defaultModeActivationConditions) { + const modeActivationCondition_t *macDefault = &defaultModeActivationConditions[i]; + equalsDefault = mac->modeId == macDefault->modeId + && mac->auxChannelIndex == macDefault->auxChannelIndex + && mac->range.startStep == macDefault->range.startStep + && mac->range.endStep == macDefault->range.endStep; + const box_t *box = findBoxByBoxId(macDefault->modeId); + if (box) { + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + box->permanentId, + macDefault->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep) + ); + } + } + const box_t *box = findBoxByBoxId(mac->modeId); + if (box) { + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + box->permanentId, + mac->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) + ); + } + } +} + +static void cliAux(char *cmdline) +{ + int i, val = 0; + const char *ptr; + + if (isEmpty(cmdline)) { + printAux(DUMP_MASTER, modeActivationConditions(0), NULL); + } else { + ptr = cmdline; + i = atoi(ptr++); + if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { + modeActivationCondition_t *mac = modeActivationConditionsMutable(i); + uint8_t validArgumentCount = 0; + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + const box_t *box = findBoxByPermanentId(val); + if (box) { + mac->modeId = box->boxId; + validArgumentCount++; + } + } + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { + mac->auxChannelIndex = val; + validArgumentCount++; + } + } + ptr = processChannelRangeArgs(ptr, &mac->range, &validArgumentCount); + + if (validArgumentCount != 4) { + memset(mac, 0, sizeof(modeActivationCondition_t)); + } + } else { + cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1); + } + } +} + +static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, const serialConfig_t *serialConfigDefault) +{ + const char *format = "serial %d %d %ld %ld %ld %ld"; + for (uint32_t i = 0; i < SERIAL_PORT_COUNT; i++) { + if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { + continue; + }; + bool equalsDefault = false; + if (serialConfigDefault) { + equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier + && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask + && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex + && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex + && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex + && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + serialConfigDefault->portConfigs[i].identifier, + serialConfigDefault->portConfigs[i].functionMask, + baudRates[serialConfigDefault->portConfigs[i].msp_baudrateIndex], + baudRates[serialConfigDefault->portConfigs[i].gps_baudrateIndex], + baudRates[serialConfigDefault->portConfigs[i].telemetry_baudrateIndex], + baudRates[serialConfigDefault->portConfigs[i].blackbox_baudrateIndex] + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + serialConfig->portConfigs[i].identifier, + serialConfig->portConfigs[i].functionMask, + baudRates[serialConfig->portConfigs[i].msp_baudrateIndex], + baudRates[serialConfig->portConfigs[i].gps_baudrateIndex], + baudRates[serialConfig->portConfigs[i].telemetry_baudrateIndex], + baudRates[serialConfig->portConfigs[i].blackbox_baudrateIndex] + ); + } +} + +static void cliSerial(char *cmdline) +{ + if (isEmpty(cmdline)) { + printSerial(DUMP_MASTER, serialConfig(), NULL); + return; + } + serialPortConfig_t portConfig; + memset(&portConfig, 0 , sizeof(portConfig)); + + serialPortConfig_t *currentConfig; + + uint8_t validArgumentCount = 0; + + const char *ptr = cmdline; + + int val = atoi(ptr++); + currentConfig = serialFindPortConfiguration(val); + if (currentConfig) { + portConfig.identifier = val; + validArgumentCount++; + } + + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + portConfig.functionMask = val & 0xFFFF; + validArgumentCount++; + } + + for (int i = 0; i < 4; i ++) { + ptr = nextArg(ptr); + if (!ptr) { + break; + } + + val = atoi(ptr); + + uint8_t baudRateIndex = lookupBaudRateIndex(val); + if (baudRates[baudRateIndex] != (uint32_t) val) { + break; + } + + switch(i) { + case 0: + if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_1000000) { + continue; + } + portConfig.msp_baudrateIndex = baudRateIndex; + break; + case 1: + if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_115200) { + continue; + } + portConfig.gps_baudrateIndex = baudRateIndex; + break; + case 2: + if (baudRateIndex != BAUD_AUTO && baudRateIndex > BAUD_115200) { + continue; + } + portConfig.telemetry_baudrateIndex = baudRateIndex; + break; + case 3: + if (baudRateIndex < BAUD_19200 || baudRateIndex > BAUD_2470000) { + continue; + } + portConfig.blackbox_baudrateIndex = baudRateIndex; + break; + } + + validArgumentCount++; + } + + if (validArgumentCount < 6) { + cliShowParseError(); + return; + } + + memcpy(currentConfig, &portConfig, sizeof(portConfig)); +} + +#ifndef SKIP_SERIAL_PASSTHROUGH +static void cliSerialPassthrough(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliShowParseError(); + return; + } + + int id = -1; + uint32_t baud = 0; + unsigned mode = 0; + char *saveptr; + char* tok = strtok_r(cmdline, " ", &saveptr); + int index = 0; + + while (tok != NULL) { + switch(index) { + case 0: + id = atoi(tok); + break; + case 1: + baud = atoi(tok); + break; + case 2: + if (strstr(tok, "rx") || strstr(tok, "RX")) + mode |= MODE_RX; + if (strstr(tok, "tx") || strstr(tok, "TX")) + mode |= MODE_TX; + break; + } + index++; + tok = strtok_r(NULL, " ", &saveptr); + } + + tfp_printf("Port %d ", id); + serialPort_t *passThroughPort; + serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id); + if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) { + if (!baud) { + tfp_printf("closed, specify baud.\r\n"); + return; + } + if (!mode) + mode = MODE_RXTX; + + passThroughPort = openSerialPort(id, FUNCTION_NONE, NULL, + baud, mode, + SERIAL_NOT_INVERTED); + if (!passThroughPort) { + tfp_printf("could not be opened.\r\n"); + return; + } + tfp_printf("opened, baud = %d.\r\n", baud); + } else { + passThroughPort = passThroughPortUsage->serialPort; + // If the user supplied a mode, override the port's mode, otherwise + // leave the mode unchanged. serialPassthrough() handles one-way ports. + tfp_printf("already open.\r\n"); + if (mode && passThroughPort->mode != mode) { + tfp_printf("mode changed from %d to %d.\r\n", + passThroughPort->mode, mode); + serialSetMode(passThroughPort, mode); + } + // If this port has a rx callback associated we need to remove it now. + // Otherwise no data will be pushed in the serial port buffer! + if (passThroughPort->rxCallback) { + passThroughPort->rxCallback = 0; + } + } + + tfp_printf("forwarding, power cycle to exit.\r\n"); + + serialPassthrough(cliPort, passThroughPort, NULL, NULL); +} +#endif + +static void printAdjustmentRange(uint8_t dumpMask, const adjustmentRange_t *adjustmentRanges, const adjustmentRange_t *defaultAdjustmentRanges) +{ + const char *format = "adjrange %u %u %u %u %u %u %u"; + // print out adjustment ranges channel settings + for (uint32_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + const adjustmentRange_t *ar = &adjustmentRanges[i]; + bool equalsDefault = false; + if (defaultAdjustmentRanges) { + const adjustmentRange_t *arDefault = &defaultAdjustmentRanges[i]; + equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex + && ar->range.startStep == arDefault->range.startStep + && ar->range.endStep == arDefault->range.endStep + && ar->adjustmentFunction == arDefault->adjustmentFunction + && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex + && ar->adjustmentIndex == arDefault->adjustmentIndex; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + arDefault->adjustmentIndex, + arDefault->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(arDefault->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(arDefault->range.endStep), + arDefault->adjustmentFunction, + arDefault->auxSwitchChannelIndex + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + ar->adjustmentIndex, + ar->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), + ar->adjustmentFunction, + ar->auxSwitchChannelIndex + ); + } +} + +static void cliAdjustmentRange(char *cmdline) +{ + int i, val = 0; + const char *ptr; + + if (isEmpty(cmdline)) { + printAdjustmentRange(DUMP_MASTER, adjustmentRanges(0), NULL); + } else { + ptr = cmdline; + i = atoi(ptr++); + if (i < MAX_ADJUSTMENT_RANGE_COUNT) { + adjustmentRange_t *ar = adjustmentRangesMutable(i); + uint8_t validArgumentCount = 0; + + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + if (val >= 0 && val < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { + ar->adjustmentIndex = val; + validArgumentCount++; + } + } + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { + ar->auxChannelIndex = val; + validArgumentCount++; + } + } + + ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount); + + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + if (val >= 0 && val < ADJUSTMENT_FUNCTION_COUNT) { + ar->adjustmentFunction = val; + validArgumentCount++; + } + } + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { + ar->auxSwitchChannelIndex = val; + validArgumentCount++; + } + } + + if (validArgumentCount != 6) { + memset(ar, 0, sizeof(adjustmentRange_t)); + cliShowParseError(); + } + } else { + cliShowArgumentRangeError("index", 0, MAX_ADJUSTMENT_RANGE_COUNT - 1); + } + } +} + +#ifndef USE_QUAD_MIXER_ONLY +static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer, const motorMixer_t *defaultCustomMotorMixer) +{ + const char *format = "mmix %d %s %s %s %s"; + char buf0[FTOA_BUFFER_LENGTH]; + char buf1[FTOA_BUFFER_LENGTH]; + char buf2[FTOA_BUFFER_LENGTH]; + char buf3[FTOA_BUFFER_LENGTH]; + for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (customMotorMixer[i].throttle == 0.0f) + break; + const float thr = customMotorMixer[i].throttle; + const float roll = customMotorMixer[i].roll; + const float pitch = customMotorMixer[i].pitch; + const float yaw = customMotorMixer[i].yaw; + bool equalsDefault = false; + if (defaultCustomMotorMixer) { + const float thrDefault = defaultCustomMotorMixer[i].throttle; + const float rollDefault = defaultCustomMotorMixer[i].roll; + const float pitchDefault = defaultCustomMotorMixer[i].pitch; + const float yawDefault = defaultCustomMotorMixer[i].yaw; + const bool equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + ftoa(thrDefault, buf0), + ftoa(rollDefault, buf1), + ftoa(pitchDefault, buf2), + ftoa(yawDefault, buf3)); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + ftoa(thr, buf0), + ftoa(roll, buf1), + ftoa(pitch, buf2), + ftoa(yaw, buf3)); + } +} +#endif // USE_QUAD_MIXER_ONLY + +static void cliMotorMix(char *cmdline) +{ +#ifdef USE_QUAD_MIXER_ONLY + UNUSED(cmdline); +#else + int check = 0; + uint8_t len; + 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++) { + customMotorMixerMutable(i)->throttle = 0.0f; + } + } else if (strncasecmp(cmdline, "load", 4) == 0) { + ptr = nextArg(cmdline); + if (ptr) { + len = strlen(ptr); + for (uint32_t i = 0; ; i++) { + if (mixerNames[i] == NULL) { + cliPrintLine("Invalid name"); + break; + } + if (strncasecmp(ptr, mixerNames[i], len) == 0) { + mixerLoadMix(i, customMotorMixerMutable(0)); + cliPrintLinef("Loaded %s", mixerNames[i]); + cliMotorMix(""); + break; + } + } + } + } else { + ptr = cmdline; + uint32_t i = atoi(ptr); // get motor number + if (i < MAX_SUPPORTED_MOTORS) { + ptr = nextArg(ptr); + if (ptr) { + customMotorMixerMutable(i)->throttle = fastA2F(ptr); + check++; + } + ptr = nextArg(ptr); + if (ptr) { + customMotorMixerMutable(i)->roll = fastA2F(ptr); + check++; + } + ptr = nextArg(ptr); + if (ptr) { + customMotorMixerMutable(i)->pitch = fastA2F(ptr); + check++; + } + ptr = nextArg(ptr); + if (ptr) { + customMotorMixerMutable(i)->yaw = fastA2F(ptr); + check++; + } + if (check != 4) { + cliShowParseError(); + } else { + printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); + } + } else { + cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); + } + } +#endif +} + +static void printRxRange(uint8_t dumpMask, const rxChannelRangeConfig_t *channelRangeConfigs, const rxChannelRangeConfig_t *defaultChannelRangeConfigs) +{ + const char *format = "rxrange %u %u %u"; + for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { + bool equalsDefault = false; + if (defaultChannelRangeConfigs) { + equalsDefault = channelRangeConfigs[i].min == defaultChannelRangeConfigs[i].min + && channelRangeConfigs[i].max == defaultChannelRangeConfigs[i].max; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + defaultChannelRangeConfigs[i].min, + defaultChannelRangeConfigs[i].max + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + channelRangeConfigs[i].min, + channelRangeConfigs[i].max + ); + } +} + +static void cliRxRange(char *cmdline) +{ + int i, validArgumentCount = 0; + const char *ptr; + + if (isEmpty(cmdline)) { + printRxRange(DUMP_MASTER, rxChannelRangeConfigs(0), NULL); + } else if (strcasecmp(cmdline, "reset") == 0) { + resetAllRxChannelRangeConfigurations(rxChannelRangeConfigsMutable(0)); + } else { + ptr = cmdline; + i = atoi(ptr); + if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) { + int rangeMin, rangeMax; + + ptr = nextArg(ptr); + if (ptr) { + rangeMin = atoi(ptr); + validArgumentCount++; + } + + ptr = nextArg(ptr); + if (ptr) { + rangeMax = atoi(ptr); + validArgumentCount++; + } + + if (validArgumentCount != 2) { + cliShowParseError(); + } else if (rangeMin < PWM_PULSE_MIN || rangeMin > PWM_PULSE_MAX || rangeMax < PWM_PULSE_MIN || rangeMax > PWM_PULSE_MAX) { + cliShowParseError(); + } else { + rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigsMutable(i); + channelRangeConfig->min = rangeMin; + channelRangeConfig->max = rangeMax; + } + } else { + cliShowArgumentRangeError("channel", 0, NON_AUX_CHANNEL_COUNT - 1); + } + } +} + +#ifdef LED_STRIP +static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledConfig_t *defaultLedConfigs) +{ + const char *format = "led %u %s"; + char ledConfigBuffer[20]; + char ledConfigDefaultBuffer[20]; + for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + ledConfig_t ledConfig = ledConfigs[i]; + generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer)); + bool equalsDefault = false; + if (defaultLedConfigs) { + ledConfig_t ledConfigDefault = defaultLedConfigs[i]; + equalsDefault = ledConfig == ledConfigDefault; + generateLedConfig(&ledConfigDefault, ledConfigDefaultBuffer, sizeof(ledConfigDefaultBuffer)); + cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, ledConfigDefaultBuffer); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, ledConfigBuffer); + } +} + +static void cliLed(char *cmdline) +{ + int i; + const char *ptr; + + if (isEmpty(cmdline)) { + printLed(DUMP_MASTER, ledStripConfig()->ledConfigs, NULL); + } else { + ptr = cmdline; + i = atoi(ptr); + if (i < LED_MAX_STRIP_LENGTH) { + ptr = nextArg(cmdline); + if (!parseLedStripConfig(i, ptr)) { + cliShowParseError(); + } + } else { + cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); + } + } +} + +static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColor_t *defaultColors) +{ + const char *format = "color %u %d,%u,%u"; + for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + const hsvColor_t *color = &colors[i]; + bool equalsDefault = false; + if (defaultColors) { + const hsvColor_t *colorDefault = &defaultColors[i]; + equalsDefault = color->h == colorDefault->h + && color->s == colorDefault->s + && color->v == colorDefault->v; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,colorDefault->h, colorDefault->s, colorDefault->v); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, color->h, color->s, color->v); + } +} + +static void cliColor(char *cmdline) +{ + if (isEmpty(cmdline)) { + printColor(DUMP_MASTER, ledStripConfig()->colors, NULL); + } else { + const char *ptr = cmdline; + const int i = atoi(ptr); + if (i < LED_CONFIGURABLE_COLOR_COUNT) { + ptr = nextArg(cmdline); + if (!parseColor(i, ptr)) { + cliShowParseError(); + } + } else { + cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); + } + } +} + +static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripConfig, const ledStripConfig_t *defaultLedStripConfig) +{ + const char *format = "mode_color %u %u %u"; + for (uint32_t i = 0; i < LED_MODE_COUNT; i++) { + for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = ledStripConfig->modeColors[i].color[j]; + bool equalsDefault = false; + if (defaultLedStripConfig) { + int colorIndexDefault = defaultLedStripConfig->modeColors[i].color[j]; + equalsDefault = colorIndex == colorIndexDefault; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, j, colorIndexDefault); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, j, colorIndex); + } + } + + for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + const int colorIndex = ledStripConfig->specialColors.color[j]; + bool equalsDefault = false; + if (defaultLedStripConfig) { + const int colorIndexDefault = defaultLedStripConfig->specialColors.color[j]; + equalsDefault = colorIndex == colorIndexDefault; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, LED_SPECIAL, j, colorIndexDefault); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, LED_SPECIAL, j, colorIndex); + } + + const int ledStripAuxChannel = ledStripConfig->ledstrip_aux_channel; + bool equalsDefault = false; + if (defaultLedStripConfig) { + const int ledStripAuxChannelDefault = defaultLedStripConfig->ledstrip_aux_channel; + equalsDefault = ledStripAuxChannel == ledStripAuxChannelDefault; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannelDefault); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannel); +} + +static void cliModeColor(char *cmdline) +{ + if (isEmpty(cmdline)) { + printModeColor(DUMP_MASTER, ledStripConfig(), NULL); + } else { + enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; + int args[ARGS_COUNT]; + int argNo = 0; + char *saveptr; + const char* ptr = strtok_r(cmdline, " ", &saveptr); + while (ptr && argNo < ARGS_COUNT) { + args[argNo++] = atoi(ptr); + ptr = strtok_r(NULL, " ", &saveptr); + } + + if (ptr != NULL || argNo != ARGS_COUNT) { + cliShowParseError(); + return; + } + + int modeIdx = args[MODE]; + int funIdx = args[FUNCTION]; + int color = args[COLOR]; + if(!setModeColor(modeIdx, funIdx, color)) { + cliShowParseError(); + return; + } + // values are validated + cliPrintLinef("mode_color %u %u %u", modeIdx, funIdx, color); + } +} +#endif + +#ifdef USE_SERVOS +static void printServo(uint8_t dumpMask, const servoParam_t *servoParams, const servoParam_t *defaultServoParams) +{ + // print out servo settings + const char *format = "servo %u %d %d %d %d %d"; + for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + const servoParam_t *servoConf = &servoParams[i]; + bool equalsDefault = false; + if (defaultServoParams) { + const servoParam_t *defaultServoConf = &defaultServoParams[i]; + equalsDefault = servoConf->min == defaultServoConf->min + && servoConf->max == defaultServoConf->max + && servoConf->middle == defaultServoConf->middle + && servoConf->rate == defaultServoConf->rate + && servoConf->forwardFromChannel == defaultServoConf->forwardFromChannel; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + defaultServoConf->min, + defaultServoConf->max, + defaultServoConf->middle, + defaultServoConf->rate, + defaultServoConf->forwardFromChannel + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + servoConf->min, + servoConf->max, + servoConf->middle, + servoConf->rate, + servoConf->forwardFromChannel + ); + } + // print servo directions + for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + const char *format = "smix reverse %d %d r"; + const servoParam_t *servoConf = &servoParams[i]; + const servoParam_t *servoConfDefault = &defaultServoParams[i]; + if (defaultServoParams) { + bool equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; + for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); + if (servoConfDefault->reversedSources & (1 << channel)) { + cliDefaultPrintLinef(dumpMask, equalsDefault, format, i , channel); + } + if (servoConf->reversedSources & (1 << channel)) { + cliDumpPrintLinef(dumpMask, equalsDefault, format, i , channel); + } + } + } else { + for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + if (servoConf->reversedSources & (1 << channel)) { + cliDumpPrintLinef(dumpMask, true, format, i , channel); + } + } + } + } +} + +static void cliServo(char *cmdline) +{ + enum { SERVO_ARGUMENT_COUNT = 6 }; + int16_t arguments[SERVO_ARGUMENT_COUNT]; + + servoParam_t *servo; + + int i; + char *ptr; + + if (isEmpty(cmdline)) { + printServo(DUMP_MASTER, servoParams(0), NULL); + } else { + int validArgumentCount = 0; + + ptr = cmdline; + + // Command line is integers (possibly negative) separated by spaces, no other characters allowed. + + // If command line doesn't fit the format, don't modify the config + while (*ptr) { + if (*ptr == '-' || (*ptr >= '0' && *ptr <= '9')) { + if (validArgumentCount >= SERVO_ARGUMENT_COUNT) { + cliShowParseError(); + return; + } + + arguments[validArgumentCount++] = atoi(ptr); + + do { + ptr++; + } while (*ptr >= '0' && *ptr <= '9'); + } else if (*ptr == ' ') { + ptr++; + } else { + cliShowParseError(); + return; + } + } + + enum {INDEX = 0, MIN, MAX, MIDDLE, RATE, FORWARD}; + + i = arguments[INDEX]; + + // Check we got the right number of args and the servo index is correct (don't validate the other values) + if (validArgumentCount != SERVO_ARGUMENT_COUNT || i < 0 || i >= MAX_SUPPORTED_SERVOS) { + cliShowParseError(); + return; + } + + servo = servoParamsMutable(i); + + if ( + arguments[MIN] < PWM_PULSE_MIN || arguments[MIN] > PWM_PULSE_MAX || + arguments[MAX] < PWM_PULSE_MIN || arguments[MAX] > PWM_PULSE_MAX || + arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] || + arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] || + arguments[RATE] < -100 || arguments[RATE] > 100 || + arguments[FORWARD] >= MAX_SUPPORTED_RC_CHANNEL_COUNT + ) { + cliShowParseError(); + return; + } + + servo->min = arguments[MIN]; + servo->max = arguments[MAX]; + servo->middle = arguments[MIDDLE]; + servo->rate = arguments[RATE]; + servo->forwardFromChannel = arguments[FORWARD]; + } +} +#endif + +#ifdef USE_SERVOS +static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixers, const servoMixer_t *defaultCustomServoMixers) +{ + const char *format = "smix %d %d %d %d %d %d %d %d"; + for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { + const servoMixer_t customServoMixer = customServoMixers[i]; + if (customServoMixer.rate == 0) { + break; + } + + bool equalsDefault = false; + if (defaultCustomServoMixers) { + servoMixer_t customServoMixerDefault = defaultCustomServoMixers[i]; + equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel + && customServoMixer.inputSource == customServoMixerDefault.inputSource + && customServoMixer.rate == customServoMixerDefault.rate + && customServoMixer.speed == customServoMixerDefault.speed + && customServoMixer.min == customServoMixerDefault.min + && customServoMixer.max == customServoMixerDefault.max + && customServoMixer.box == customServoMixerDefault.box; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + customServoMixerDefault.targetChannel, + customServoMixerDefault.inputSource, + customServoMixerDefault.rate, + customServoMixerDefault.speed, + customServoMixerDefault.min, + customServoMixerDefault.max, + customServoMixerDefault.box + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + customServoMixer.targetChannel, + customServoMixer.inputSource, + customServoMixer.rate, + customServoMixer.speed, + customServoMixer.min, + customServoMixer.max, + customServoMixer.box + ); + } + + cliPrintLinefeed(); +} + +static void cliServoMix(char *cmdline) +{ + int args[8], check = 0; + int len = strlen(cmdline); + + if (len == 0) { + printServoMix(DUMP_MASTER, customServoMixers(0), NULL); + } else if (strncasecmp(cmdline, "reset", 5) == 0) { + // erase custom mixer + memset(customServoMixers_array(), 0, sizeof(*customServoMixers_array())); + for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoParamsMutable(i)->reversedSources = 0; + } + } else if (strncasecmp(cmdline, "load", 4) == 0) { + const char *ptr = nextArg(cmdline); + if (ptr) { + len = strlen(ptr); + for (uint32_t i = 0; ; i++) { + if (mixerNames[i] == NULL) { + cliPrintLine("Invalid name"); + break; + } + if (strncasecmp(ptr, mixerNames[i], len) == 0) { + servoMixerLoadMix(i); + cliPrintLinef("Loaded %s", mixerNames[i]); + cliServoMix(""); + break; + } + } + } + } else if (strncasecmp(cmdline, "reverse", 7) == 0) { + enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; + char *ptr = strchr(cmdline, ' '); + + len = strlen(ptr); + if (len == 0) { + cliPrintf("s"); + for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) + cliPrintf("\ti%d", inputSource); + cliPrintLinefeed(); + + for (uint32_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { + cliPrintf("%d", servoIndex); + for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) + cliPrintf("\t%s ", (servoParams(servoIndex)->reversedSources & (1 << inputSource)) ? "r" : "n"); + cliPrintLinefeed(); + } + return; + } + + char *saveptr; + ptr = strtok_r(ptr, " ", &saveptr); + while (ptr != NULL && check < ARGS_COUNT - 1) { + args[check++] = atoi(ptr); + ptr = strtok_r(NULL, " ", &saveptr); + } + + if (ptr == NULL || check != ARGS_COUNT - 1) { + cliShowParseError(); + return; + } + + if (args[SERVO] >= 0 && args[SERVO] < MAX_SUPPORTED_SERVOS + && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT + && (*ptr == 'r' || *ptr == 'n')) { + if (*ptr == 'r') + servoParamsMutable(args[SERVO])->reversedSources |= 1 << args[INPUT]; + else + servoParamsMutable(args[SERVO])->reversedSources &= ~(1 << args[INPUT]); + } else + cliShowParseError(); + + cliServoMix("reverse"); + } else { + enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; + char *saveptr; + char *ptr = strtok_r(cmdline, " ", &saveptr); + while (ptr != NULL && check < ARGS_COUNT) { + args[check++] = atoi(ptr); + ptr = strtok_r(NULL, " ", &saveptr); + } + + if (ptr != NULL || check != ARGS_COUNT) { + cliShowParseError(); + return; + } + + int32_t i = args[RULE]; + if (i >= 0 && i < MAX_SERVO_RULES && + args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS && + args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && + args[RATE] >= -100 && args[RATE] <= 100 && + args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED && + args[MIN] >= 0 && args[MIN] <= 100 && + args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && + args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { + 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(); + } + } +} +#endif + +#ifdef USE_SDCARD + +static void cliWriteBytes(const uint8_t *buffer, int count) +{ + while (count > 0) { + cliWrite(*buffer); + buffer++; + count--; + } +} + +static void cliSdInfo(char *cmdline) +{ + UNUSED(cmdline); + + cliPrint("SD card: "); + + if (!sdcard_isInserted()) { + cliPrintLine("None inserted"); + return; + } + + if (!sdcard_isInitialized()) { + cliPrintLine("Startup failed"); + return; + } + + const sdcardMetadata_t *metadata = sdcard_getMetadata(); + + cliPrintf("Manufacturer 0x%x, %ukB, %02d/%04d, v%d.%d, '", + metadata->manufacturerID, + metadata->numBlocks / 2, /* One block is half a kB */ + metadata->productionMonth, + metadata->productionYear, + metadata->productRevisionMajor, + metadata->productRevisionMinor + ); + + cliWriteBytes((uint8_t*)metadata->productName, sizeof(metadata->productName)); + + cliPrint("'\r\n" "Filesystem: "); + + switch (afatfs_getFilesystemState()) { + case AFATFS_FILESYSTEM_STATE_READY: + cliPrint("Ready"); + break; + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + cliPrint("Initializing"); + break; + case AFATFS_FILESYSTEM_STATE_UNKNOWN: + case AFATFS_FILESYSTEM_STATE_FATAL: + cliPrint("Fatal"); + + switch (afatfs_getLastError()) { + case AFATFS_ERROR_BAD_MBR: + cliPrint(" - no FAT MBR partitions"); + break; + case AFATFS_ERROR_BAD_FILESYSTEM_HEADER: + cliPrint(" - bad FAT header"); + break; + case AFATFS_ERROR_GENERIC: + case AFATFS_ERROR_NONE: + ; // Nothing more detailed to print + break; + } + break; + } + cliPrintLinefeed(); +} + +#endif + +#ifdef USE_FLASHFS + +static void cliFlashInfo(char *cmdline) +{ + const flashGeometry_t *layout = flashfsGetGeometry(); + + UNUSED(cmdline); + + cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u, usedSize=%u", + layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset()); +} + + +static void cliFlashErase(char *cmdline) +{ + UNUSED(cmdline); + +#ifndef MINIMAL_CLI + uint32_t i = 0; + cliPrintLine("Erasing, please wait ... "); +#else + cliPrintLine("Erasing,"); +#endif + + bufWriterFlush(cliWriter); + flashfsEraseCompletely(); + + while (!flashfsIsReady()) { +#ifndef MINIMAL_CLI + cliPrintf("."); + if (i++ > 120) { + i=0; + cliPrintLinefeed(); + } + + bufWriterFlush(cliWriter); +#endif + delay(100); + } + beeper(BEEPER_BLACKBOX_ERASE); + cliPrintLinefeed(); + cliPrintLine("Done."); +} + +#ifdef USE_FLASH_TOOLS + +static void cliFlashWrite(char *cmdline) +{ + const uint32_t address = atoi(cmdline); + const char *text = strchr(cmdline, ' '); + + if (!text) { + cliShowParseError(); + } else { + flashfsSeekAbs(address); + flashfsWrite((uint8_t*)text, strlen(text), true); + flashfsFlushSync(); + + cliPrintLinef("Wrote %u bytes at %u.", strlen(text), address); + } +} + +static void cliFlashRead(char *cmdline) +{ + uint32_t address = atoi(cmdline); + + const char *nextArg = strchr(cmdline, ' '); + + if (!nextArg) { + cliShowParseError(); + } else { + uint32_t length = atoi(nextArg); + + cliPrintLinef("Reading %u bytes at %u:", length, address); + + uint8_t buffer[32]; + while (length > 0) { + int bytesRead = flashfsReadAbs(address, buffer, length < sizeof(buffer) ? length : sizeof(buffer)); + + for (int i = 0; i < bytesRead; i++) { + cliWrite(buffer[i]); + } + + length -= bytesRead; + address += bytesRead; + + if (bytesRead == 0) { + //Assume we reached the end of the volume or something fatal happened + break; + } + } + cliPrintLinefeed(); + } +} + +#endif +#endif + +#ifdef VTX_CONTROL +static void printVtx(uint8_t dumpMask, const vtxConfig_t *vtxConfig, const vtxConfig_t *vtxConfigDefault) +{ + // print out vtx channel settings + const char *format = "vtx %u %u %u %u %u %u"; + bool equalsDefault = false; + for (uint32_t i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { + const vtxChannelActivationCondition_t *cac = &vtxConfig->vtxChannelActivationConditions[i]; + if (vtxConfigDefault) { + const vtxChannelActivationCondition_t *cacDefault = &vtxConfigDefault->vtxChannelActivationConditions[i]; + equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex + && cac->band == cacDefault->band + && cac->channel == cacDefault->channel + && cac->range.startStep == cacDefault->range.startStep + && cac->range.endStep == cacDefault->range.endStep; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + cacDefault->auxChannelIndex, + cacDefault->band, + cacDefault->channel, + MODE_STEP_TO_CHANNEL_VALUE(cacDefault->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(cacDefault->range.endStep) + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + cac->auxChannelIndex, + cac->band, + cac->channel, + MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) + ); + } +} + +// FIXME remove these and use the VTX API +#define VTX_BAND_MIN 1 +#define VTX_BAND_MAX 5 +#define VTX_CHANNEL_MIN 1 +#define VTX_CHANNEL_MAX 8 + +static void cliVtx(char *cmdline) +{ + int i, val = 0; + const char *ptr; + + if (isEmpty(cmdline)) { + printVtx(DUMP_MASTER, vtxConfig(), NULL); + } else { + ptr = cmdline; + i = atoi(ptr++); + if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) { + vtxChannelActivationCondition_t *cac = &vtxConfigMutable()->vtxChannelActivationConditions[i]; + uint8_t validArgumentCount = 0; + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { + cac->auxChannelIndex = val; + validArgumentCount++; + } + } + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + // FIXME Use VTX API to get min/max + if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) { + cac->band = val; + validArgumentCount++; + } + } + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + // FIXME Use VTX API to get min/max + if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) { + cac->channel = val; + validArgumentCount++; + } + } + ptr = processChannelRangeArgs(ptr, &cac->range, &validArgumentCount); + + if (validArgumentCount != 5) { + memset(cac, 0, sizeof(vtxChannelActivationCondition_t)); + } + } else { + cliShowArgumentRangeError("index", 0, MAX_CHANNEL_ACTIVATION_CONDITION_COUNT - 1); + } + } +} + +#endif // VTX_CONTROL + +static void printName(uint8_t dumpMask, const systemConfig_t *systemConfig) +{ + const bool equalsDefault = strlen(systemConfig->name) == 0; + cliDumpPrintLinef(dumpMask, equalsDefault, "name %s", equalsDefault ? emptyName : systemConfig->name); +} + +static void cliName(char *cmdline) +{ + const uint32_t len = strlen(cmdline); + if (len > 0) { + memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfig()->name)); + if (strncmp(cmdline, emptyName, len)) { + strncpy(systemConfigMutable()->name, cmdline, MIN(len, MAX_NAME_LENGTH)); + } + } + printName(DUMP_MASTER, systemConfig()); +} + +static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) +{ + const uint32_t mask = featureConfig->enabledFeatures; + const uint32_t defaultMask = featureConfigDefault->enabledFeatures; + for (uint32_t i = 0; featureNames[i]; i++) { // disable all feature first + const char *format = "feature -%s"; + cliDefaultPrintLinef(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]); + cliDumpPrintLinef(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); + } + for (uint32_t i = 0; featureNames[i]; i++) { // reenable what we want. + const char *format = "feature %s"; + if (defaultMask & (1 << i)) { + cliDefaultPrintLinef(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); + } + if (mask & (1 << i)) { + cliDumpPrintLinef(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]); + } + } +} + +static void cliFeature(char *cmdline) +{ + uint32_t len = strlen(cmdline); + uint32_t mask = featureMask(); + + if (len == 0) { + cliPrint("Enabled: "); + for (uint32_t i = 0; ; i++) { + if (featureNames[i] == NULL) + break; + if (mask & (1 << i)) + cliPrintf("%s ", featureNames[i]); + } + cliPrintLinefeed(); + } else if (strncasecmp(cmdline, "list", len) == 0) { + cliPrint("Available:"); + for (uint32_t i = 0; ; i++) { + if (featureNames[i] == NULL) + break; + cliPrintf(" %s", featureNames[i]); + } + cliPrintLinefeed(); + return; + } else { + bool remove = false; + if (cmdline[0] == '-') { + // remove feature + remove = true; + cmdline++; // skip over - + len--; + } + + for (uint32_t i = 0; ; i++) { + if (featureNames[i] == NULL) { + cliPrintLine("Invalid name"); + break; + } + + if (strncasecmp(cmdline, featureNames[i], len) == 0) { + + mask = 1 << i; +#ifndef GPS + if (mask & FEATURE_GPS) { + cliPrintLine("unavailable"); + break; + } +#endif +#ifndef SONAR + if (mask & FEATURE_SONAR) { + cliPrintLine("unavailable"); + break; + } +#endif + if (remove) { + featureClear(mask); + cliPrint("Disabled"); + } else { + featureSet(mask); + cliPrint("Enabled"); + } + cliPrintLinef(" %s", featureNames[i]); + break; + } + } + } +} + +#ifdef BEEPER +static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault) +{ + const uint8_t beeperCount = beeperTableEntryCount(); + const uint32_t mask = beeperConfig->beeper_off_flags; + const uint32_t defaultMask = beeperConfigDefault->beeper_off_flags; + for (int32_t i = 0; i < beeperCount - 2; i++) { + const char *formatOff = "beeper -%s"; + const char *formatOn = "beeper %s"; + cliDefaultPrintLinef(dumpMask, ~(mask ^ defaultMask) & (1 << i), mask & (1 << i) ? formatOn : formatOff, beeperNameForTableIndex(i)); + cliDumpPrintLinef(dumpMask, ~(mask ^ defaultMask) & (1 << i), mask & (1 << i) ? formatOff : formatOn, beeperNameForTableIndex(i)); + } +} + +static void cliBeeper(char *cmdline) +{ + uint32_t len = strlen(cmdline); + uint8_t beeperCount = beeperTableEntryCount(); + uint32_t mask = getBeeperOffMask(); + + if (len == 0) { + cliPrintf("Disabled:"); + for (int32_t i = 0; ; i++) { + if (i == beeperCount - 2){ + if (mask == 0) + cliPrint(" none"); + break; + } + if (mask & (1 << i)) + cliPrintf(" %s", beeperNameForTableIndex(i)); + } + cliPrintLinefeed(); + } else if (strncasecmp(cmdline, "list", len) == 0) { + cliPrint("Available:"); + for (uint32_t i = 0; i < beeperCount; i++) + cliPrintf(" %s", beeperNameForTableIndex(i)); + cliPrintLinefeed(); + return; + } else { + bool remove = false; + if (cmdline[0] == '-') { + remove = true; // this is for beeper OFF condition + cmdline++; + len--; + } + + for (uint32_t i = 0; ; i++) { + if (i == beeperCount) { + cliPrintLine("Invalid name"); + break; + } + if (strncasecmp(cmdline, beeperNameForTableIndex(i), len) == 0) { + if (remove) { // beeper off + if (i == BEEPER_ALL-1) + beeperOffSetAll(beeperCount-2); + else + if (i == BEEPER_PREFERENCE-1) + setBeeperOffMask(getPreferredBeeperOffMask()); + else { + mask = 1 << i; + beeperOffSet(mask); + } + cliPrint("Disabled"); + } + else { // beeper on + if (i == BEEPER_ALL-1) + beeperOffClearAll(); + else + if (i == BEEPER_PREFERENCE-1) + setPreferredBeeperOffMask(getBeeperOffMask()); + else { + mask = 1 << i; + beeperOffClear(mask); + } + cliPrint("Enabled"); + } + cliPrintLinef(" %s", beeperNameForTableIndex(i)); + break; + } + } + } +} +#endif + +static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) +{ + bool equalsDefault = true; + char buf[16]; + char bufDefault[16]; + uint32_t i; + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { + buf[rxConfig->rcmap[i]] = rcChannelLetters[i]; + if (defaultRxConfig) { + bufDefault[defaultRxConfig->rcmap[i]] = rcChannelLetters[i]; + equalsDefault = equalsDefault && (rxConfig->rcmap[i] == defaultRxConfig->rcmap[i]); + } + } + buf[i] = '\0'; + + const char *formatMap = "map %s"; + cliDefaultPrintLinef(dumpMask, equalsDefault, formatMap, bufDefault); + cliDumpPrintLinef(dumpMask, equalsDefault, formatMap, buf); +} + +static void cliMap(char *cmdline) +{ + uint32_t len; + char out[9]; + + len = strlen(cmdline); + + if (len == 8) { + // uppercase it + for (uint32_t i = 0; i < 8; i++) + cmdline[i] = toupper((unsigned char)cmdline[i]); + for (uint32_t i = 0; i < 8; i++) { + if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i])) + continue; + cliShowParseError(); + return; + } + parseRcChannels(cmdline, rxConfigMutable()); + } + cliPrint("Map: "); + uint32_t i; + for (i = 0; i < 8; i++) + out[rxConfig()->rcmap[i]] = rcChannelLetters[i]; + out[i] = '\0'; + cliPrintLine(out); +} + +static char *checkCommand(char *cmdLine, const char *command) +{ + if(!strncasecmp(cmdLine, command, strlen(command)) // command names match + && (isspace((unsigned)cmdLine[strlen(command)]) || cmdLine[strlen(command)] == 0)) { + return cmdLine + strlen(command) + 1; + } else { + return 0; + } +} + +static void cliRebootEx(bool bootLoader) +{ + cliPrint("\r\nRebooting"); + bufWriterFlush(cliWriter); + waitForSerialPortToFinishTransmitting(cliPort); + stopPwmAllMotors(); + if (bootLoader) { + systemResetToBootloader(); + return; + } + systemReset(); +} + +static void cliReboot(void) +{ + cliRebootEx(false); +} + +static void cliBootloader(char *cmdLine) +{ + UNUSED(cmdLine); + + cliPrintHashLine("restarting in bootloader mode"); + cliRebootEx(true); +} + +static void cliExit(char *cmdline) +{ + UNUSED(cmdline); + + cliPrintHashLine("leaving CLI mode, unsaved changes lost"); + bufWriterFlush(cliWriter); + + *cliBuffer = '\0'; + bufferIndex = 0; + cliMode = 0; + // incase a motor was left running during motortest, clear it here + mixerResetDisarmedMotors(); + cliReboot(); + + cliWriter = NULL; +} + +#ifdef GPS +static void cliGpsPassthrough(char *cmdline) +{ + UNUSED(cmdline); + + gpsEnablePassthrough(cliPort); +} +#endif + +#if defined(USE_ESCSERIAL) || defined(USE_DSHOT) + +#ifndef ALL_ESCS +#define ALL_ESCS 255 +#endif + +static int parseEscNumber(char *pch, bool allowAllEscs) { + int escNumber = atoi(pch); + if ((escNumber >= 0) && (escNumber < getMotorCount())) { + tfp_printf("Programming on ESC %d.\r\n", escNumber); + } else if (allowAllEscs && escNumber == ALL_ESCS) { + tfp_printf("Programming on all ESCs.\r\n"); + } else { + tfp_printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1); + + return -1; + } + + return escNumber; +} +#endif + +#ifdef USE_DSHOT +static void cliDshotProg(char *cmdline) +{ + if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) { + cliShowParseError(); + + return; + } + + char *saveptr; + char *pch = strtok_r(cmdline, " ", &saveptr); + int pos = 0; + int escNumber = 0; + while (pch != NULL) { + switch (pos) { + case 0: + escNumber = parseEscNumber(pch, true); + if (escNumber == -1) { + return; + } + + break; + default: + motorControlEnable = false; + + int command = atoi(pch); + if (command >= 0 && command < DSHOT_MIN_THROTTLE) { + if (escNumber == ALL_ESCS) { + for (unsigned i = 0; i < getMotorCount(); i++) { + pwmWriteDshotCommand(i, command); + } + } else { + pwmWriteDshotCommand(escNumber, command); + } + + if (command <= 5) { + delay(10); // wait for sound output to finish + } + + tfp_printf("Command %d written.\r\n", command); + } else { + tfp_printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1); + } + + break; + } + + pos++; + pch = strtok_r(NULL, " ", &saveptr); + } + + motorControlEnable = true; +} +#endif + +#ifdef USE_ESCSERIAL +static void cliEscPassthrough(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliShowParseError(); + + return; + } + + char *saveptr; + char *pch = strtok_r(cmdline, " ", &saveptr); + int pos = 0; + uint8_t mode = 0; + int escNumber = 0; + while (pch != NULL) { + switch (pos) { + case 0: + if(strncasecmp(pch, "sk", strlen(pch)) == 0) { + mode = PROTOCOL_SIMONK; + } else if(strncasecmp(pch, "bl", strlen(pch)) == 0) { + mode = PROTOCOL_BLHELI; + } else if(strncasecmp(pch, "ki", strlen(pch)) == 0) { + mode = PROTOCOL_KISS; + } else if(strncasecmp(pch, "cc", strlen(pch)) == 0) { + mode = PROTOCOL_KISSALL; + } else { + cliShowParseError(); + + return; + } + break; + case 1: + escNumber = parseEscNumber(pch, mode == PROTOCOL_KISS); + if (escNumber == -1) { + return; + } + + break; + default: + cliShowParseError(); + + return; + + break; + + } + pos++; + pch = strtok_r(NULL, " ", &saveptr); + } + + escEnablePassthrough(cliPort, escNumber, mode); +} +#endif + +#ifndef USE_QUAD_MIXER_ONLY +static void cliMixer(char *cmdline) +{ + int len; + + len = strlen(cmdline); + + if (len == 0) { + cliPrintLinef("Mixer: %s", mixerNames[mixerConfig()->mixerMode - 1]); + return; + } else if (strncasecmp(cmdline, "list", len) == 0) { + cliPrint("Available:"); + for (uint32_t i = 0; ; i++) { + if (mixerNames[i] == NULL) + break; + cliPrintf(" %s", mixerNames[i]); + } + cliPrintLinefeed(); + return; + } + + for (uint32_t i = 0; ; i++) { + if (mixerNames[i] == NULL) { + cliPrintLine("Invalid name"); + return; + } + if (strncasecmp(cmdline, mixerNames[i], len) == 0) { + mixerConfigMutable()->mixerMode = i + 1; + break; + } + } + + cliMixer(""); +} +#endif + +static void cliMotor(char *cmdline) +{ + int motor_index = 0; + int motor_value = 0; + int index = 0; + char *pch = NULL; + char *saveptr; + + if (isEmpty(cmdline)) { + cliShowParseError(); + return; + } + + pch = strtok_r(cmdline, " ", &saveptr); + while (pch != NULL) { + switch (index) { + case 0: + motor_index = atoi(pch); + break; + case 1: + motor_value = atoi(pch); + break; + } + index++; + pch = strtok_r(NULL, " ", &saveptr); + } + + if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) { + cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); + return; + } + + if (index == 2) { + if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) { + cliShowArgumentRangeError("value", 1000, 2000); + } else { + motor_disarmed[motor_index] = convertExternalToMotor(motor_value); + + cliPrintLinef("motor %d: %d", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); + } + } + +} + +#ifndef MINIMAL_CLI +static void cliPlaySound(char *cmdline) +{ + int i; + const char *name; + static int lastSoundIdx = -1; + + if (isEmpty(cmdline)) { + i = lastSoundIdx + 1; //next sound index + if ((name=beeperNameForTableIndex(i)) == NULL) { + while (true) { //no name for index; try next one + if (++i >= beeperTableEntryCount()) + i = 0; //if end then wrap around to first entry + if ((name=beeperNameForTableIndex(i)) != NULL) + break; //if name OK then play sound below + if (i == lastSoundIdx + 1) { //prevent infinite loop + cliPrintLine("Error playing sound"); + return; + } + } + } + } else { //index value was given + i = atoi(cmdline); + if ((name=beeperNameForTableIndex(i)) == NULL) { + cliPrintLinef("No sound for index %d", i); + return; + } + } + lastSoundIdx = i; + beeperSilence(); + cliPrintLinef("Playing sound %d: %s", i, name); + beeper(beeperModeForTableIndex(i)); +} +#endif + +static void cliProfile(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliPrintLinef("profile %d", getCurrentPidProfileIndex()); + return; + } else { + const int i = atoi(cmdline); + if (i >= 0 && i < MAX_PROFILE_COUNT) { + systemConfigMutable()->pidProfileIndex = i; + cliProfile(""); + } + } +} + +static void cliRateProfile(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliPrintLinef("rateprofile %d", getCurrentControlRateProfileIndex()); + return; + } else { + const int i = atoi(cmdline); + if (i >= 0 && i < CONTROL_RATE_PROFILE_COUNT) { + changeControlRateProfile(i); + cliRateProfile(""); + } + } +} + +static void cliDumpPidProfile(uint8_t pidProfileIndex, uint8_t dumpMask) +{ + if (pidProfileIndex >= MAX_PROFILE_COUNT) { + // Faulty values + return; + } + changePidProfile(pidProfileIndex); + cliPrintHashLine("profile"); + cliProfile(""); + cliPrintLinefeed(); + dumpAllValues(PROFILE_VALUE, dumpMask); +} + +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask) +{ + if (rateProfileIndex >= CONTROL_RATE_PROFILE_COUNT) { + // Faulty values + return; + } + changeControlRateProfile(rateProfileIndex); + cliPrintHashLine("rateprofile"); + cliRateProfile(""); + cliPrintLinefeed(); + dumpAllValues(PROFILE_RATE_VALUE, dumpMask); +} + +static void cliSave(char *cmdline) +{ + UNUSED(cmdline); + + cliPrintHashLine("saving"); + writeEEPROM(); + cliReboot(); +} + +static void cliDefaults(char *cmdline) +{ + UNUSED(cmdline); + + cliPrintHashLine("resetting to defaults"); + resetEEPROM(); + cliReboot(); +} + +static void cliGet(char *cmdline) +{ + const clivalue_t *val; + int matchedCommands = 0; + + for (uint32_t i = 0; i < valueTableEntryCount; i++) { + if (strstr(valueTable[i].name, cmdline)) { + val = &valueTable[i]; + cliPrintf("%s = ", valueTable[i].name); + cliPrintVar(val, 0); + cliPrintLinefeed(); + cliPrintVarRange(val); + cliPrintLinefeed(); + + matchedCommands++; + } + } + + + if (matchedCommands) { + return; + } + + cliPrintLine("Invalid name"); +} + +static char *skipSpace(char *buffer) +{ + while (*(buffer) == ' ') { + buffer++; + } + + return buffer; +} + +static uint8_t getWordLength(char *bufBegin, char *bufEnd) +{ + while (*(bufEnd - 1) == ' ') { + bufEnd--; + } + + return bufEnd - bufBegin; +} + +static void cliSet(char *cmdline) +{ + const uint32_t len = strlen(cmdline); + char *eqptr; + + if (len == 0 || (len == 1 && cmdline[0] == '*')) { + cliPrintLine("Current settings: "); + + for (uint32_t i = 0; i < valueTableEntryCount; i++) { + const clivalue_t *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 + cliPrintLinefeed(); + } + } else if ((eqptr = strstr(cmdline, "=")) != NULL) { + // has equals + + uint8_t variableNameLength = getWordLength(cmdline, eqptr); + + // skip the '=' and any ' ' characters + eqptr++; + eqptr = skipSpace(eqptr); + + for (uint32_t i = 0; i < valueTableEntryCount; i++) { + const clivalue_t *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)) { + + bool valueChanged = false; + int16_t value = 0; + switch (valueTable[i].type & VALUE_MODE_MASK) { + case MODE_DIRECT: { + int16_t value = atoi(eqptr); + + if (value >= valueTable[i].config.minmax.min && value <= valueTable[i].config.minmax.max) { + cliSetVar(val, value); + valueChanged = true; + } + } + + break; + case MODE_LOOKUP: { + const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex]; + bool matched = false; + for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { + matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; + + if (matched) { + value = tableValueIndex; + + cliSetVar(val, value); + valueChanged = true; + } + } + } + + break; + case MODE_ARRAY: { + const uint8_t arrayLength = valueTable[i].config.array.length; + char *valPtr = eqptr; + uint8_t array[256]; + char curVal[4]; + for (int i = 0; i < arrayLength; i++) { + valPtr = skipSpace(valPtr); + char *valEnd = strstr(valPtr, ","); + if ((valEnd != NULL) && (i < arrayLength - 1)) { + uint8_t varLength = getWordLength(valPtr, valEnd); + if (varLength <= 3) { + strncpy(curVal, valPtr, getWordLength(valPtr, valEnd)); + curVal[varLength] = '\0'; + array[i] = (uint8_t)atoi((const char *)curVal); + valPtr = valEnd + 1; + } else { + break; + } + } else if ((valEnd == NULL) && (i == arrayLength - 1)) { + array[i] = atoi(valPtr); + + uint8_t *ptr = getValuePointer(val); + memcpy(ptr, array, arrayLength); + valueChanged = true; + } else { + break; + } + } + } + + break; + + } + + if (valueChanged) { + cliPrintf("%s set to ", valueTable[i].name); + cliPrintVar(val, 0); + } else { + cliPrintLine("Invalid value"); + cliPrintVarRange(val); + } + + return; + } + } + cliPrintLine("Invalid name"); + } else { + // no equals, check for matching variables. + cliGet(cmdline); + } +} + +static void cliStatus(char *cmdline) +{ + UNUSED(cmdline); + + cliPrintLinef("System Uptime: %d seconds", millis() / 1000); + cliPrintLinef("Voltage: %d * 0.1V (%dS battery - %s)", getBatteryVoltage(), getBatteryCellCount(), getBatteryStateString()); + + cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); + +#if defined(USE_SENSOR_NAMES) + const uint32_t detectedSensorsMask = sensorsMask(); + for (uint32_t i = 0; ; i++) { + if (sensorTypeNames[i] == NULL) { + break; + } + const uint32_t mask = (1 << i); + if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) { + const uint8_t sensorHardwareIndex = detectedSensors[i]; + const char *sensorHardware = sensorHardwareNames[i][sensorHardwareIndex]; + cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware); + if (mask == SENSOR_ACC && acc.dev.revisionCode) { + cliPrintf(".%c", acc.dev.revisionCode); + } + } + } +#endif /* USE_SENSOR_NAMES */ + cliPrintLinefeed(); + +#ifdef USE_SDCARD + cliSdInfo(NULL); +#endif + +#ifdef USE_I2C + const uint16_t i2cErrorCounter = i2cGetErrorCounter(); +#else + const uint16_t i2cErrorCounter = 0; +#endif + +#ifdef STACK_CHECK + cliPrintf("Stack used: %d, ", stackUsedSize()); +#endif + cliPrintLinef("Stack size: %d, Stack address: 0x%x", stackTotalSize(), stackHighMem()); + + cliPrintLinef("I2C Errors: %d, config size: %d, max available config: %d", i2cErrorCounter, getEEPROMConfigSize(), &__config_end - &__config_start); + + const int gyroRate = getTaskDeltaTime(TASK_GYROPID) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYROPID))); + const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX))); + const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); + cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", + constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate); + +} + +#ifndef SKIP_TASK_STATISTICS +static void cliTasks(char *cmdline) +{ + UNUSED(cmdline); + int maxLoadSum = 0; + int averageLoadSum = 0; + +#ifndef MINIMAL_CLI + if (systemConfig()->task_statistics) { + cliPrintLine("Task list rate/hz max/us avg/us maxload avgload total/ms"); + } else { + cliPrintLine("Task list"); + } +#endif + for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { + cfTaskInfo_t taskInfo; + getTaskInfo(taskId, &taskInfo); + if (taskInfo.isEnabled) { + int taskFrequency; + int subTaskFrequency = 0; + if (taskId == TASK_GYROPID) { + subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); + taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom; + if (pidConfig()->pid_process_denom > 1) { + cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName); + } else { + taskFrequency = subTaskFrequency; + cliPrintf("%02d - (%11s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); + } + } else { + taskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); + cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName); + } + const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000; + const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000; + if (taskId != TASK_SERIAL) { + maxLoadSum += maxLoad; + averageLoadSum += averageLoad; + } + if (systemConfig()->task_statistics) { + cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d", + taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, + maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000); + } else { + cliPrintLinef("%6d", taskFrequency); + } + if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) { + cliPrintLinef(" - (%15s) %6d", taskInfo.subTaskName, subTaskFrequency); + } + } + } + if (systemConfig()->task_statistics) { + cfCheckFuncInfo_t checkFuncInfo; + getCheckFuncInfo(&checkFuncInfo); + cliPrintLinef("RX Check Function %19d %7d %25d", checkFuncInfo.maxExecutionTime, checkFuncInfo.averageExecutionTime, checkFuncInfo.totalExecutionTime / 1000); + cliPrintLinef("Total (excluding SERIAL) %25d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10); + } +} +#endif + +static void cliVersion(char *cmdline) +{ + UNUSED(cmdline); + + cliPrintLinef("# %s / %s %s %s / %s (%s)", + FC_FIRMWARE_NAME, + targetName, + FC_VERSION_STRING, + buildDate, + buildTime, + shortGitRevision + ); +} + +#if defined(USE_RESOURCE_MGMT) + +#define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x)) + +typedef struct { + const uint8_t owner; + pgn_t pgn; + uint16_t offset; + const uint8_t maxIndex; +} cliResourceValue_t; + +const cliResourceValue_t resourceTable[] = { +#ifdef BEEPER + { OWNER_BEEPER, PG_BEEPER_DEV_CONFIG, offsetof(beeperDevConfig_t, ioTag), 0 }, +#endif + { OWNER_MOTOR, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.ioTags[0]), MAX_SUPPORTED_MOTORS }, +#ifdef USE_SERVOS + { OWNER_SERVO, PG_SERVO_CONFIG, offsetof(servoConfig_t, dev.ioTags[0]), MAX_SUPPORTED_SERVOS }, +#endif +#if defined(USE_PWM) || defined(USE_PPM) + { OWNER_PPMINPUT, PG_PPM_CONFIG, offsetof(ppmConfig_t, ioTag), 0 }, + { OWNER_PWMINPUT, PG_PWM_CONFIG, offsetof(pwmConfig_t, ioTags[0]), PWM_INPUT_PORT_COUNT }, +#endif +#ifdef SONAR + { OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, offsetof(sonarConfig_t, triggerTag), 0 }, + { OWNER_SONAR_ECHO, PG_SONAR_CONFIG, offsetof(sonarConfig_t, echoTag), 0 }, +#endif +#ifdef LED_STRIP + { OWNER_LED_STRIP, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ioTag), 0 }, +#endif + { OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagTx[0]), SERIAL_PORT_MAX_INDEX }, + { OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagRx[0]), SERIAL_PORT_MAX_INDEX }, +#ifdef USE_INVERTER + { OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagInverter[0]), SERIAL_PORT_MAX_INDEX }, +#endif +#ifdef USE_I2C + { OWNER_I2C_SCL, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagScl[0]), I2CDEV_COUNT }, + { OWNER_I2C_SDA, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagSda[0]), I2CDEV_COUNT }, +#endif + { OWNER_LED, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, ioTags[0]), STATUS_LED_NUMBER }, +<<<<<<< HEAD +#ifdef TRANSPONDER + { OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 }, +#endif +======= +>>>>>>> betaflight/master +}; + +static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) +{ + const pgRegistry_t* rec = pgFind(value.pgn); + return CONST_CAST(ioTag_t *, rec->address + value.offset + index); +} + +static void printResource(uint8_t dumpMask) +{ + for (unsigned int i = 0; i < ARRAYLEN(resourceTable); i++) { + const char* owner = ownerNames[resourceTable[i].owner]; + const pgRegistry_t* pg = pgFind(resourceTable[i].pgn); + const void *currentConfig; + const void *defaultConfig; + if (configIsInCopy) { + currentConfig = pg->copy; + defaultConfig = pg->address; + } else { + currentConfig = pg->address; + defaultConfig = NULL; + } + + for (int index = 0; index < MAX_RESOURCE_INDEX(resourceTable[i].maxIndex); index++) { + const ioTag_t ioTag = *((const ioTag_t *)currentConfig + resourceTable[i].offset + index); + const ioTag_t ioTagDefault = *((const ioTag_t *)defaultConfig + resourceTable[i].offset + index); + + bool equalsDefault = ioTag == ioTagDefault; + const char *format = "resource %s %d %c%02d"; + const char *formatUnassigned = "resource %s %d NONE"; + if (!ioTagDefault) { + cliDefaultPrintLinef(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } else { + cliDefaultPrintLinef(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault)); + } + if (!ioTag) { + if (!(dumpMask & HIDE_UNUSED)) { + cliDumpPrintLinef(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } + } else { + cliDumpPrintLinef(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); + } + } + } +} + +static void printResourceOwner(uint8_t owner, uint8_t index) +{ + cliPrintf("%s", ownerNames[resourceTable[owner].owner]); + + if (resourceTable[owner].maxIndex > 0) { + cliPrintf(" %d", RESOURCE_INDEX(index)); + } +} + +static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t newTag) +{ + if (!newTag) { + 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++) { + ioTag_t *tag = getIoTag(resourceTable[r], i); + if (*tag == newTag) { + bool cleared = false; + if (r == resourceIndex) { + if (i == index) { + continue; + } + *tag = IO_TAG_NONE; + cleared = true; + } + + cliPrintf(format, DEFIO_TAG_GPIOID(newTag) + 'A', DEFIO_TAG_PIN(newTag)); + + printResourceOwner(r, i); + + if (cleared) { + cliPrintf(". "); + printResourceOwner(r, i); + cliPrintf(" disabled"); + } + + cliPrintLine("."); + } + } + } +} + +static void cliResource(char *cmdline) +{ + int len = strlen(cmdline); + + if (len == 0) { + printResource(DUMP_MASTER | HIDE_UNUSED); + + return; + } else if (strncasecmp(cmdline, "list", len) == 0) { +#ifdef MINIMAL_CLI + cliPrintLine("IO"); +#else + cliPrintLine("Currently active IO resource assignments:\r\n(reboot to update)"); + cliRepeat('-', 20); +#endif + for (int i = 0; i < DEFIO_IO_USED_COUNT; i++) { + const char* owner; + owner = ownerNames[ioRecs[i].owner]; + + cliPrintf("%c%02d: %s", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner); + if (ioRecs[i].index > 0) { + cliPrintf(" %d", ioRecs[i].index); + } + cliPrintLinefeed(); + } + + cliPrintLinefeed(); + +#ifdef MINIMAL_CLI + cliPrintLine("DMA:"); +#else + cliPrintLine("Currently active DMA:"); + cliRepeat('-', 20); +#endif + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + const char* owner; + owner = ownerNames[dmaGetOwner(i)]; + + cliPrintf(DMA_OUTPUT_STRING, i / DMA_MOD_VALUE + 1, (i % DMA_MOD_VALUE) + DMA_MOD_OFFSET); + uint8_t resourceIndex = dmaGetResourceIndex(i); + if (resourceIndex > 0) { + cliPrintLinef(" %s %d", owner, resourceIndex); + } else { + cliPrintLinef(" %s", owner); + } + } + +#ifndef MINIMAL_CLI + cliPrintLine("\r\nUse: 'resource' to see how to change resources."); +#endif + + return; + } + + uint8_t resourceIndex = 0; + int index = 0; + char *pch = NULL; + char *saveptr; + + pch = strtok_r(cmdline, " ", &saveptr); + for (resourceIndex = 0; ; resourceIndex++) { + if (resourceIndex >= ARRAYLEN(resourceTable)) { + cliPrintLine("Invalid"); + return; + } + + if (strncasecmp(pch, ownerNames[resourceTable[resourceIndex].owner], len) == 0) { + break; + } + } + + pch = strtok_r(NULL, " ", &saveptr); + index = atoi(pch); + + if (resourceTable[resourceIndex].maxIndex > 0 || index > 0) { + if (index <= 0 || index > MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)) { + cliShowArgumentRangeError("index", 1, MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)); + return; + } + index -= 1; + + pch = strtok_r(NULL, " ", &saveptr); + } + + ioTag_t *tag = getIoTag(resourceTable[resourceIndex], index); + + uint8_t pin = 0; + if (strlen(pch) > 0) { + if (strcasecmp(pch, "NONE") == 0) { + *tag = IO_TAG_NONE; +#ifdef MINIMAL_CLI + cliPrintLine("Freed"); +#else + cliPrintLine("Resource is freed"); +#endif + return; + } else { + uint8_t port = (*pch) - 'A'; + if (port >= 8) { + port = (*pch) - 'a'; + } + + if (port < 8) { + pch++; + pin = atoi(pch); + if (pin < 16) { + ioRec_t *rec = IO_Rec(IOGetByTag(DEFIO_TAG_MAKE(port, pin))); + if (rec) { + resourceCheck(resourceIndex, index, DEFIO_TAG_MAKE(port, pin)); +#ifdef MINIMAL_CLI + cliPrintLinef(" %c%02d set", port + 'A', pin); +#else + cliPrintLinef("\r\nResource is set to %c%02d", port + 'A', pin); +#endif + *tag = DEFIO_TAG_MAKE(port, pin); + } else { + cliShowParseError(); + } + return; + } + } + } + } + + cliShowParseError(); +} +#endif /* USE_RESOURCE_MGMT */ + +static void backupConfigs(void) +{ + // make copies of configs to do differencing + PG_FOREACH(pg) { + memcpy(pg->copy, pg->address, pg->size); + } + + configIsInCopy = true; +} + +static void restoreConfigs(void) +{ + PG_FOREACH(pg) { + memcpy(pg->address, pg->copy, pg->size); + } + + configIsInCopy = false; +} + +static void printConfig(char *cmdline, bool doDiff) +{ + uint8_t dumpMask = DUMP_MASTER; + char *options; + if ((options = checkCommand(cmdline, "master"))) { + dumpMask = DUMP_MASTER; // only + } else if ((options = checkCommand(cmdline, "profile"))) { + dumpMask = DUMP_PROFILE; // only + } else if ((options = checkCommand(cmdline, "rates"))) { + dumpMask = DUMP_RATES; // only + } else if ((options = checkCommand(cmdline, "all"))) { + dumpMask = DUMP_ALL; // all profiles and rates + } else { + options = cmdline; + } + + if (doDiff) { + dumpMask = dumpMask | DO_DIFF; + } + + backupConfigs(); + // reset all configs to defaults to do differencing + resetConfigs(); + +#if defined(TARGET_CONFIG) + targetConfiguration(); +#endif + if (checkCommand(options, "defaults")) { + dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values + } + + if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { + cliPrintHashLine("version"); + cliVersion(NULL); + + if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { + cliPrintHashLine("reset configuration to default settings"); + cliPrint("defaults"); + cliPrintLinefeed(); + } + + cliPrintHashLine("name"); + printName(dumpMask, &systemConfig_Copy); + +#ifdef USE_RESOURCE_MGMT + cliPrintHashLine("resources"); + printResource(dumpMask); +#endif + +#ifndef USE_QUAD_MIXER_ONLY + cliPrintHashLine("mixer"); + const bool equalsDefault = mixerConfig_Copy.mixerMode == mixerConfig()->mixerMode; + const char *formatMixer = "mixer %s"; + cliDefaultPrintLinef(dumpMask, equalsDefault, formatMixer, mixerNames[mixerConfig()->mixerMode - 1]); + cliDumpPrintLinef(dumpMask, equalsDefault, formatMixer, mixerNames[mixerConfig_Copy.mixerMode - 1]); + + cliDumpPrintLinef(dumpMask, customMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n"); + + printMotorMix(dumpMask, customMotorMixer_CopyArray, customMotorMixer(0)); + +#ifdef USE_SERVOS + cliPrintHashLine("servo"); + printServo(dumpMask, servoParams_CopyArray, servoParams(0)); + + cliPrintHashLine("servo mix"); + // print custom servo mixer if exists + cliDumpPrintLinef(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n"); + printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); +#endif +#endif + + cliPrintHashLine("feature"); + printFeature(dumpMask, &featureConfig_Copy, featureConfig()); + +#ifdef BEEPER + cliPrintHashLine("beeper"); + printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig()); +#endif + + cliPrintHashLine("map"); + printMap(dumpMask, &rxConfig_Copy, rxConfig()); + + cliPrintHashLine("serial"); + printSerial(dumpMask, &serialConfig_Copy, serialConfig()); + +#ifdef LED_STRIP + cliPrintHashLine("led"); + printLed(dumpMask, ledStripConfig_Copy.ledConfigs, ledStripConfig()->ledConfigs); + + cliPrintHashLine("color"); + printColor(dumpMask, ledStripConfig_Copy.colors, ledStripConfig()->colors); + + cliPrintHashLine("mode_color"); + printModeColor(dumpMask, &ledStripConfig_Copy, ledStripConfig()); +#endif + + cliPrintHashLine("aux"); + printAux(dumpMask, modeActivationConditions_CopyArray, modeActivationConditions(0)); + + cliPrintHashLine("adjrange"); + printAdjustmentRange(dumpMask, adjustmentRanges_CopyArray, adjustmentRanges(0)); + + cliPrintHashLine("rxrange"); + printRxRange(dumpMask, rxChannelRangeConfigs_CopyArray, rxChannelRangeConfigs(0)); + +#ifdef VTX_CONTROL + cliPrintHashLine("vtx"); + printVtx(dumpMask, &vtxConfig_Copy, vtxConfig()); +#endif + + cliPrintHashLine("rxfail"); + printRxFailsafe(dumpMask, rxFailsafeChannelConfigs_CopyArray, rxFailsafeChannelConfigs(0)); + + cliPrintHashLine("master"); + dumpAllValues(MASTER_VALUE, dumpMask); + + if (dumpMask & DUMP_ALL) { + const uint8_t pidProfileIndexSave = systemConfig_Copy.pidProfileIndex; + for (uint32_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + cliDumpPidProfile(pidProfileIndex, dumpMask); + } + changePidProfile(pidProfileIndexSave); + cliPrintHashLine("restore original profile selection"); + cliProfile(""); + + const uint8_t controlRateProfileIndexSave = systemConfig_Copy.activeRateProfile; + for (uint32_t rateIndex = 0; rateIndex < CONTROL_RATE_PROFILE_COUNT; rateIndex++) { + cliDumpRateProfile(rateIndex, dumpMask); + } + changeControlRateProfile(controlRateProfileIndexSave); + cliPrintHashLine("restore original rateprofile selection"); + cliRateProfile(""); + + cliPrintHashLine("save configuration"); + cliPrint("save"); + } else { + cliDumpPidProfile(systemConfig_Copy.pidProfileIndex, dumpMask); + + cliDumpRateProfile(systemConfig_Copy.activeRateProfile, dumpMask); + } + } + + if (dumpMask & DUMP_PROFILE) { + cliDumpPidProfile(systemConfig_Copy.pidProfileIndex, dumpMask); + } + + if (dumpMask & DUMP_RATES) { + cliDumpRateProfile(systemConfig_Copy.activeRateProfile, dumpMask); + } + // restore configs from copies + restoreConfigs(); +} + +static void cliDump(char *cmdline) +{ + printConfig(cmdline, false); +} + +static void cliDiff(char *cmdline) +{ + printConfig(cmdline, true); +} + +typedef struct { + const char *name; +#ifndef MINIMAL_CLI + const char *description; + const char *args; +#endif + void (*func)(char *cmdline); +} clicmd_t; + +#ifndef MINIMAL_CLI +#define CLI_COMMAND_DEF(name, description, args, method) \ +{ \ + name , \ + description , \ + args , \ + method \ +} +#else +#define CLI_COMMAND_DEF(name, description, args, method) \ +{ \ + name, \ + method \ +} +#endif + +static void cliHelp(char *cmdline); + +// should be sorted a..z for bsearch() +const clicmd_t cmdTable[] = { + CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange), + CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), +#ifdef BEEPER + CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" + "\t<+|->[name]", cliBeeper), +#endif +#ifdef LED_STRIP + CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), +#endif + CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), + CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), + CLI_COMMAND_DEF("diff", "list configuration changes from default", + "[master|profile|rates|all] {showdefaults}", cliDiff), +#ifdef USE_DSHOT + CLI_COMMAND_DEF("dshotprog", "program DShot ESC(s)", " +", cliDshotProg), +#endif + CLI_COMMAND_DEF("dump", "dump configuration", + "[master|profile|rates|all] {showdefaults}", cliDump), +#ifdef USE_ESCSERIAL + CLI_COMMAND_DEF("escprog", "passthrough esc to serial", " ", cliEscPassthrough), +#endif + CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), + CLI_COMMAND_DEF("feature", "configure features", + "list\r\n" + "\t<+|->[name]", cliFeature), +#ifdef USE_FLASHFS + CLI_COMMAND_DEF("flash_erase", "erase flash chip", NULL, cliFlashErase), + CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo), +#ifdef USE_FLASH_TOOLS + CLI_COMMAND_DEF("flash_read", NULL, "
", cliFlashRead), + CLI_COMMAND_DEF("flash_write", NULL, "
", cliFlashWrite), +#endif +#endif + CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), +#ifdef GPS + CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), +#endif + CLI_COMMAND_DEF("help", NULL, NULL, cliHelp), +#ifdef LED_STRIP + CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed), +#endif + CLI_COMMAND_DEF("map", "configure rc channel order", "[]", cliMap), +#ifndef USE_QUAD_MIXER_ONLY + CLI_COMMAND_DEF("mixer", "configure mixer", "list\r\n\t", cliMixer), +#endif + CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix), +#ifdef LED_STRIP + CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), +#endif + CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), + CLI_COMMAND_DEF("name", "name of craft", NULL, cliName), +#ifndef MINIMAL_CLI + CLI_COMMAND_DEF("play_sound", NULL, "[]", cliPlaySound), +#endif + CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), + CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), +#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, cliRxFailsafe), + CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), + CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), +#ifdef USE_SDCARD + CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), +#endif + CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial), +#ifndef SKIP_SERIAL_PASSTHROUGH + CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] : passthrough to serial", cliSerialPassthrough), +#endif +#ifdef USE_SERVOS + CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo), +#endif + CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), +#ifdef USE_SERVOS + CLI_COMMAND_DEF("smix", "servo mixer", " \r\n" + "\treset\r\n" + "\tload \r\n" + "\treverse r|n", cliServoMix), +#endif + CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), +#ifndef SKIP_TASK_STATISTICS + CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks), +#endif + CLI_COMMAND_DEF("version", "show version", NULL, cliVersion), +#ifdef VTX_CONTROL + CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), +#endif +}; +static void cliHelp(char *cmdline) +{ + UNUSED(cmdline); + + for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) { + cliPrint(cmdTable[i].name); +#ifndef MINIMAL_CLI + if (cmdTable[i].description) { + cliPrintf(" - %s", cmdTable[i].description); + } + if (cmdTable[i].args) { + cliPrintf("\r\n\t%s", cmdTable[i].args); + } +#endif + cliPrintLinefeed(); + } +} + +void cliProcess(void) +{ + if (!cliWriter) { + return; + } + + // Be a little bit tricky. Flush the last inputs buffer, if any. + bufWriterFlush(cliWriter); + + while (serialRxBytesWaiting(cliPort)) { + uint8_t c = serialRead(cliPort); + if (c == '\t' || c == '?') { + // do tab completion + const clicmd_t *cmd, *pstart = NULL, *pend = NULL; + uint32_t i = bufferIndex; + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { + if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0)) + continue; + if (!pstart) + pstart = cmd; + pend = cmd; + } + if (pstart) { /* Buffer matches one or more commands */ + for (; ; bufferIndex++) { + if (pstart->name[bufferIndex] != pend->name[bufferIndex]) + break; + if (!pstart->name[bufferIndex] && bufferIndex < sizeof(cliBuffer) - 2) { + /* Unambiguous -- append a space */ + cliBuffer[bufferIndex++] = ' '; + cliBuffer[bufferIndex] = '\0'; + break; + } + cliBuffer[bufferIndex] = pstart->name[bufferIndex]; + } + } + if (!bufferIndex || pstart != pend) { + /* Print list of ambiguous matches */ + cliPrint("\r\033[K"); + for (cmd = pstart; cmd <= pend; cmd++) { + cliPrint(cmd->name); + cliWrite('\t'); + } + cliPrompt(); + i = 0; /* Redraw prompt */ + } + for (; i < bufferIndex; i++) + cliWrite(cliBuffer[i]); + } else if (!bufferIndex && c == 4) { // CTRL-D + cliExit(cliBuffer); + return; + } else if (c == 12) { // NewPage / CTRL-L + // clear screen + cliPrint("\033[2J\033[1;1H"); + cliPrompt(); + } else if (bufferIndex && (c == '\n' || c == '\r')) { + // enter pressed + cliPrintLinefeed(); + + // Strip comment starting with # from line + char *p = cliBuffer; + p = strchr(p, '#'); + if (NULL != p) { + bufferIndex = (uint32_t)(p - cliBuffer); + } + + // Strip trailing whitespace + while (bufferIndex > 0 && cliBuffer[bufferIndex - 1] == ' ') { + bufferIndex--; + } + + // Process non-empty lines + if (bufferIndex > 0) { + cliBuffer[bufferIndex] = 0; // null terminate + + const clicmd_t *cmd; + char *options; + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { + if ((options = checkCommand(cliBuffer, cmd->name))) { + break; + } + } + if(cmd < cmdTable + ARRAYLEN(cmdTable)) + cmd->func(options); + else + cliPrint("Unknown command, try 'help'"); + bufferIndex = 0; + } + + memset(cliBuffer, 0, sizeof(cliBuffer)); + + // 'exit' will reset this flag, so we don't need to print prompt again + if (!cliMode) + return; + + cliPrompt(); + } else if (c == 127) { + // backspace + if (bufferIndex) { + cliBuffer[--bufferIndex] = 0; + cliPrint("\010 \010"); + } + } else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) { + if (!bufferIndex && c == ' ') + continue; // Ignore leading spaces + cliBuffer[bufferIndex++] = c; + cliWrite(c); + } + } +} + +void cliEnter(serialPort_t *serialPort) +{ + cliMode = 1; + cliPort = serialPort; + setPrintfSerialPort(cliPort); + cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort); + + schedulerSetCalulateTaskStatistics(systemConfig()->task_statistics); + +#ifndef MINIMAL_CLI + cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'"); +#else + cliPrintLine("\r\nCLI"); +#endif + cliPrompt(); + + ENABLE_ARMING_FLAG(PREVENT_ARMING); +} + +void cliInit(const serialConfig_t *serialConfig) +{ + UNUSED(serialConfig); +} +#endif // USE_CLI diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d1cfe8c963..b4d3d6822d 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -148,7 +148,6 @@ PG_REGISTER_WITH_RESET_FN(pwmConfig_t, pwmConfig, PG_PWM_CONFIG, 0); #ifdef USE_PPM PG_REGISTER_WITH_RESET_FN(ppmConfig_t, ppmConfig, PG_PPM_CONFIG, 0); #endif -PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); #ifdef USE_FLASHFS PG_REGISTER_WITH_RESET_TEMPLATE(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0); @@ -255,35 +254,6 @@ void pgResetFn_pwmConfig(pwmConfig_t *pwmConfig) #define SECOND_PORT_INDEX 1 #endif -void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) -{ - for (int i = 0; i < LED_NUMBER; i++) { - statusLedConfig->ledTags[i] = IO_TAG_NONE; - } - -#ifdef LED0 - statusLedConfig->ledTags[0] = IO_TAG(LED0); -#endif -#ifdef LED1 - statusLedConfig->ledTags[1] = IO_TAG(LED1); -#endif -#ifdef LED2 - statusLedConfig->ledTags[2] = IO_TAG(LED2); -#endif - - statusLedConfig->polarity = 0 -#ifdef LED0_INVERTED - | BIT(0) -#endif -#ifdef LED1_INVERTED - | BIT(1) -#endif -#ifdef LED2_INVERTED - | BIT(2) -#endif - ; -} - #ifndef USE_OSD_SLAVE uint8_t getCurrentPidProfileIndex(void) { @@ -312,14 +282,12 @@ uint16_t getCurrentMinthrottle(void) void resetConfigs(void) { - pgResetAll(MAX_PROFILE_COUNT); + pgResetAll(); #if defined(TARGET_CONFIG) targetConfiguration(); #endif - pgActivateProfile(0); - #ifndef USE_OSD_SLAVE setPidProfile(0); setControlRateProfile(0); @@ -337,7 +305,7 @@ void activateConfig(void) resetAdjustmentStates(); - useRcControlsConfig(modeActivationConditions(0), currentPidProfile); + useRcControlsConfig(currentPidProfile); useAdjustmentConfig(currentPidProfile); #ifdef GPS diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 4f2ee925b2..4ced00ee29 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -27,6 +27,7 @@ #include "drivers/rx_pwm.h" #include "drivers/sdcard.h" #include "drivers/serial.h" +#include "drivers/bus_i2c.h" #include "drivers/sound_beeper.h" #include "drivers/vcd.h" diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f2ba387e92..4c7b2a6120 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -107,7 +107,7 @@ int16_t magHold; int16_t headFreeModeHold; uint8_t motorControlEnable = false; - +static bool reverseMotors; static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; @@ -206,6 +206,21 @@ void mwArm(void) return; } if (!ARMING_FLAG(PREVENT_ARMING)) { + #ifdef USE_DSHOT + //TODO: Use BOXDSHOTREVERSE here + if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = false; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); + } + } + if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = true; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); + } + } + #endif ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); @@ -645,3 +660,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs) runTaskMainSubprocesses = true; } } + +bool isMotorsReversed() +{ + return reverseMotors; +} diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index e242886a1a..baba693d5c 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -48,3 +48,4 @@ void updateLEDs(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); +bool isMotorsReversed(void); \ No newline at end of file diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index d546deb37d..b3794a5b79 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -54,6 +54,7 @@ #include "drivers/rx_pwm.h" #include "drivers/pwm_output.h" #include "drivers/adc.h" +#include "drivers/bus.h" #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "drivers/buttons.h" @@ -135,10 +136,6 @@ void targetPreInit(void); #endif -#ifdef TARGET_BUS_INIT -void targetBusInit(void); -#endif - extern uint8_t motorControlEnable; #ifdef SOFTSERIAL_LOOPBACK @@ -226,7 +223,9 @@ void init(void) targetPreInit(); #endif +#if !defined(UNIT_TEST) && !defined(USE_FAKE_LED) ledInit(statusLedConfig()); +#endif LED2_ON; #ifdef USE_EXTI @@ -367,6 +366,11 @@ void init(void) #endif /* USE_SPI */ #ifdef USE_I2C + i2cHardwareConfigure(); + + // Note: Unlike UARTs which are configured when client is present, + // I2C buses are initialized unconditionally if they are configured. + #ifdef USE_I2C_DEVICE_1 i2cInit(I2CDEV_1); #endif diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 10ebaaee0a..28f6ea4258 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -29,6 +29,7 @@ #include "build/version.h" #include "common/axis.h" +#include "common/bitarray.h" #include "common/color.h" #include "common/maths.h" #include "common/streambuf.h" @@ -59,6 +60,8 @@ #include "fc/fc_msp.h" #include "fc/fc_rc.h" #include "fc/rc_adjustments.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/altitude.h" @@ -110,12 +113,13 @@ #endif #define STATIC_ASSERT(condition, name) \ - typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] + typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] __attribute__((unused)) 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; #ifndef USE_OSD_SLAVE +// permanent IDs must uniquely identify BOX meaning, DO NOT REUSE THEM! static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXARM, "ARM", 0 }, { BOXANGLE, "ANGLE", 1 }, @@ -152,9 +156,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index -static uint32_t activeBoxIds; -// check that all boxId_e values fit -STATIC_ASSERT(sizeof(activeBoxIds) * 8 >= CHECKBOX_ITEM_COUNT, CHECKBOX_ITEMS_wont_fit_in_activeBoxIds); + +static boxBitmask_t activeBoxIds; static const char pidnames[] = "ROLL;" @@ -276,23 +279,41 @@ const box_t *findBoxByPermanentId(uint8_t permanentId) return NULL; } -static void serializeBoxNamesReply(sbuf_t *dst) +static bool activeBoxIdGet(boxId_e boxId) { - for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) { - if(activeBoxIds & (1 << id)) { - const box_t *box = findBoxByBoxId(id); - sbufWriteString(dst, box->boxName); - sbufWriteU8(dst, ';'); - } - } + if(boxId > sizeof(activeBoxIds) * 8) + return false; + return bitArrayGet(&activeBoxIds, boxId); } -static void serializeBoxIdsReply(sbuf_t *dst) + +// callcack for box serialization +typedef void serializeBoxFn(sbuf_t *dst, const box_t *box); + +static void serializeBoxNameFn(sbuf_t *dst, const box_t *box) { + sbufWriteString(dst, box->boxName); + sbufWriteU8(dst, ';'); +} + +static void serializeBoxPermanentIdFn(sbuf_t *dst, const box_t *box) +{ + sbufWriteU8(dst, box->permanentId); +} + +// serialize 'page' of boxNames. +// Each page contains at most 32 boxes +static void serializeBoxReply(sbuf_t *dst, int page, serializeBoxFn *serializeBox) +{ + unsigned boxIdx = 0; + unsigned pageStart = page * 32; + unsigned pageEnd = pageStart + 32; for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) { - if(activeBoxIds & (1 << id)) { - const box_t *box = findBoxByBoxId(id); - sbufWriteU8(dst, box->permanentId); + if (activeBoxIdGet(id)) { + if (boxIdx >= pageStart && boxIdx < pageEnd) { + (*serializeBox)(dst, findBoxByBoxId(id)); + } + boxIdx++; // count active boxes } } } @@ -300,9 +321,11 @@ static void serializeBoxIdsReply(sbuf_t *dst) void initActiveBoxIds(void) { // calculate used boxes based on features and set corresponding activeBoxIds bits - uint32_t ena = 0; // temporary variable to collect result + boxBitmask_t ena; // temporary variable to collect result + memset(&ena, 0, sizeof(ena)); + // macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only -#define BME(boxId) do { ena |= (1 << (boxId)); } while(0) +#define BME(boxId) do { bitArraySet(&ena, boxId); } while(0) BME(BOXARM); if (!feature(FEATURE_AIRMODE)) { @@ -370,9 +393,8 @@ void initActiveBoxIds(void) BME(BOXFPVANGLEMIX); - if (feature(FEATURE_3D)) { - BME(BOX3DDISABLESWITCH); - } + //TODO: Split this into BOX3DDISABLESWITCH and BOXDSHOTREVERSE + BME(BOX3DDISABLESWITCH); if (feature(FEATURE_SERVO_TILT)) { BME(BOXCAMSTAB); @@ -400,63 +422,69 @@ void initActiveBoxIds(void) #undef BME // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions) - for(boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) - if((ena & (1 << boxId)) - && findBoxByBoxId(boxId) == NULL) - ena &= ~ (1 << boxId); // this should not happen, but handle it gracefully + for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) + if (bitArrayGet(&ena, boxId) + && findBoxByBoxId(boxId) == NULL) + bitArrayClr(&ena, boxId); // this should not happen, but handle it gracefully - activeBoxIds = ena; // set global variable + activeBoxIds = ena; // set global variable } -static uint32_t packFlightModeFlags(void) +// pack used flightModeFlags into supplied array +// returns number of bits used +static int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) { // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES + memset(mspFlightModeFlags, 0, sizeof(boxBitmask_t)); - uint32_t boxEnabledMask = 0; // enabled BOXes, bits indexed by boxId_e + // enabled BOXes, bits indexed by boxId_e + boxBitmask_t boxEnabledMask; + memset(&boxEnabledMask, 0, sizeof(boxEnabledMask)); // enable BOXes dependent on FLIGHT_MODE, use mapping table (from runtime_config.h) // flightMode_boxId_map[HORIZON_MODE] == BOXHORIZON static const int8_t flightMode_boxId_map[] = FLIGHT_MODE_BOXID_MAP_INITIALIZER; flightModeFlags_e flightModeCopyMask = ~0; // only modes with bit set will be copied - for(unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) { - if(flightMode_boxId_map[i] != -1 // boxId_e does exist for this FLIGHT_MODE + for (unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) { + if (flightMode_boxId_map[i] != -1 // boxId_e does exist for this FLIGHT_MODE && (flightModeCopyMask & (1 << i)) // this flightmode is copy is enabled && FLIGHT_MODE(1 << i)) { // this flightmode is active - boxEnabledMask |= (1 << flightMode_boxId_map[i]); + bitArraySet(&boxEnabledMask, flightMode_boxId_map[i]); } } // enable BOXes dependent on rcMode bits, indexes are the same. // only subset of BOXes depend on rcMode, use mask to select them -#define BM(x) (1 << (x)) - const uint32_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON) +#define BM(x) (1ULL << (x)) + // limited to 64 BOXes now to keep code simple + const uint64_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON) | BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD) | BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE) | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX); - for(unsigned i = 0; i < sizeof(rcModeCopyMask) * 8; i++) { - if((rcModeCopyMask & BM(i)) // mode copy is enabled + STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes); + for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) { + if ((rcModeCopyMask & BM(i)) // mode copy is enabled && IS_RC_MODE_ACTIVE(i)) { // mode is active - boxEnabledMask |= (1 << i); + bitArraySet(&boxEnabledMask, i); } } #undef BM // copy ARM state - if(ARMING_FLAG(ARMED)) - boxEnabledMask |= (1 << BOXARM); + if (ARMING_FLAG(ARMED)) + bitArraySet(&boxEnabledMask, BOXARM); // map boxId_e enabled bits to MSP status indexes // only active boxIds are sent in status over MSP, other bits are not counted - uint32_t mspBoxEnabledMask = 0; unsigned mspBoxIdx = 0; // index of active boxId (matches sent permanentId and boxNames) for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) { - if(activeBoxIds & (1 << boxId)) { - if (boxEnabledMask & (1 << boxId)) - mspBoxEnabledMask |= 1 << mspBoxIdx; // box is enabled - mspBoxIdx++; // box is active, count it + if (activeBoxIdGet(boxId)) { + if (bitArrayGet(&boxEnabledMask, boxId)) + bitArraySet(mspFlightModeFlags, mspBoxIdx); // box is enabled + mspBoxIdx++; // box is active, count it } } - - return mspBoxEnabledMask; + // return count of used bits + return mspBoxIdx; } static void serializeSDCardSummaryReply(sbuf_t *dst) @@ -831,20 +859,6 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro switch (cmdMSP) { case MSP_STATUS_EX: - sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL)); -#ifdef USE_I2C - sbufWriteU16(dst, i2cGetErrorCounter()); -#else - sbufWriteU16(dst, 0); -#endif - sbufWriteU16(dst, 0); // sensors - sbufWriteU32(dst, 0); // flight modes - sbufWriteU8(dst, 0); // profile - sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU8(dst, 1); // max profiles - sbufWriteU8(dst, 0); // control rate profile - break; - case MSP_STATUS: sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL)); #ifdef USE_I2C @@ -856,7 +870,12 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro sbufWriteU32(dst, 0); // flight modes sbufWriteU8(dst, 0); // profile sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU16(dst, 0); // gyro cycle time + if (cmdMSP == MSP_STATUS_EX) { + sbufWriteU8(dst, 1); // max profiles + sbufWriteU8(dst, 0); // control rate profile + } else { + sbufWriteU16(dst, 0); // gyro cycle time + } break; default: @@ -873,32 +892,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn switch (cmdMSP) { case MSP_STATUS_EX: - sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID)); -#ifdef USE_I2C - sbufWriteU16(dst, i2cGetErrorCounter()); -#else - sbufWriteU16(dst, 0); -#endif - sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - sbufWriteU32(dst, packFlightModeFlags()); - sbufWriteU8(dst, getCurrentPidProfileIndex()); - sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU8(dst, MAX_PROFILE_COUNT); - sbufWriteU8(dst, getCurrentControlRateProfileIndex()); - break; - case MSP_STATUS: - sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID)); + { + boxBitmask_t flightModeFlags; + const int flagBits = packFlightModeFlags(&flightModeFlags); + + sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID)); #ifdef USE_I2C - sbufWriteU16(dst, i2cGetErrorCounter()); + sbufWriteU16(dst, i2cGetErrorCounter()); #else - sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); #endif - sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - sbufWriteU32(dst, packFlightModeFlags()); - sbufWriteU8(dst, getCurrentPidProfileIndex()); - sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU16(dst, 0); // gyro cycle time + sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits + sbufWriteU8(dst, getCurrentPidProfileIndex()); + sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); + if (cmdMSP == MSP_STATUS_EX) { + sbufWriteU8(dst, MAX_PROFILE_COUNT); + sbufWriteU8(dst, getCurrentControlRateProfileIndex()); + } else { // MSP_STATUS + sbufWriteU16(dst, 0); // gyro cycle time + } + + // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow + // header is emited even when all bits fit into 32 bits to allow future extension + int byteCount = (flagBits - 32 + 7) / 8; // 32 already stored, round up + byteCount = constrain(byteCount, 0, 15); // limit to 16 bytes (128 bits) + sbufWriteU8(dst, byteCount); + sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount); + } break; case MSP_RAW_IMU: @@ -1060,14 +1082,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn } break; - case MSP_BOXNAMES: - serializeBoxNamesReply(dst); - break; - - case MSP_BOXIDS: - serializeBoxIdsReply(dst); - break; - case MSP_MOTOR_CONFIG: sbufWriteU16(dst, motorConfig()->minthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle); @@ -1334,13 +1348,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn uint8_t band=0, channel=0; vtxCommonGetBandAndChannel(&band,&channel); - + uint8_t powerIdx=0; // debug vtxCommonGetPowerIndex(&powerIdx); - + uint8_t pitmode=0; vtxCommonGetPitMode(&pitmode); - + sbufWriteU8(dst, deviceType); sbufWriteU8(dst, band); sbufWriteU8(dst, channel); @@ -1360,6 +1374,29 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn } return true; } + +static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) +{ + UNUSED(mspPostProcessFn); + + switch (cmdMSP) { + case MSP_BOXNAMES: + { + const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0; + serializeBoxReply(dst, page, &serializeBoxNameFn); + } + break; + case MSP_BOXIDS: + { + const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0; + serializeBoxReply(dst, page, &serializeBoxPermanentIdFn); + } + break; + default: + return MSP_RESULT_CMD_UNKNOWN; + } + return MSP_RESULT_ACK; +} #endif #ifdef GPS @@ -1497,7 +1534,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationConditions(0), currentPidProfile); + useRcControlsConfig(currentPidProfile); } else { return MSP_RESULT_ERROR; } @@ -1659,7 +1696,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) }*/ validateAndFixGyroConfig(); - if (sbufBytesRemaining(src)) { + if (sbufBytesRemaining(src)) { motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src); } break; @@ -1762,13 +1799,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (sbufBytesRemaining(src) < 2) break; - + uint8_t power = sbufReadU8(src); uint8_t current_power = 0; vtxCommonGetPowerIndex(¤t_power); if (current_power != power) vtxCommonSetPowerByIndex(power); - + uint8_t pitmode = sbufReadU8(src); uint8_t current_pitmode = 0; vtxCommonGetPitMode(¤t_pitmode); @@ -2178,6 +2215,8 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro #ifndef USE_OSD_SLAVE } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { ret = MSP_RESULT_ACK; + } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) { + /* ret */; #endif #ifdef USE_OSD_SLAVE } else if (mspOsdSlaveProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index ac3bc51958..92e0cc2647 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -18,12 +18,12 @@ #pragma once #include "msp/msp.h" -#include "rc_controls.h" +#include "fc/rc_modes.h" typedef struct box_e { const uint8_t boxId; // see boxId_e const char *boxName; // GUI-readable box name - const uint8_t permanentId; // + const uint8_t permanentId; // permanent ID used to identify BOX. This ID is unique for one function, DO NOT REUSE IT } box_t; const box_t *findBoxByBoxId(boxId_e boxId); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index bc0c47ecaf..afc1f45b7d 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -37,6 +37,7 @@ #include "fc/fc_core.h" #include "fc/fc_rc.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "rx/rx.h" @@ -46,6 +47,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/pid.h" +#include "flight/mixer.h" static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; @@ -166,7 +168,7 @@ static void scaleRcCommandToFpvCamAngle(void) { const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; if(ABS(rcCommandSpeed) > throttleVelocityThreshold) - pidSetItermAccelerator(0.001f * currentPidProfile->itermAcceleratorGain); + pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); else pidSetItermAccelerator(1.0f); } diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 2fbba702b0..3ad6b04867 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -88,7 +88,7 @@ static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustm } #endif -static uint8_t adjustmentStateMask = 0; +STATIC_UNIT_TESTED uint8_t adjustmentStateMask = 0; #define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex) #define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex) @@ -199,9 +199,9 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 -static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; +STATIC_UNIT_TESTED adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; -static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) +STATIC_UNIT_TESTED void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) { adjustmentState_t *adjustmentState = &adjustmentStates[index]; @@ -386,10 +386,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig) continue; } - const int32_t signedDiff = now - adjustmentState->timeoutAt; - const bool canResetReadyStates = signedDiff >= 0L; - - if (canResetReadyStates) { + if (cmp32(now, adjustmentState->timeoutAt) >= 0) { adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); } diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 3d44a4629a..60900403a6 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -19,7 +19,7 @@ #include #include "config/parameter_group.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" typedef enum { ADJUSTMENT_NONE = 0, diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0bcdfe7cb5..259cab256f 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -65,8 +65,6 @@ static bool isUsingSticksToArm = true; float rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e - PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, @@ -85,15 +83,13 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig, .auto_disarm_delay = 5 ); -PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); - -bool isAirmodeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); -} - -bool isAntiGravityModeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); -} +PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); +PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, + .deadband3d_low = 1406, + .deadband3d_high = 1514, + .neutral3d = 1460, + .deadband3d_throttle = 50 +); bool isUsingSticksForArming(void) { @@ -308,51 +304,13 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } -bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId) -{ - uint8_t index; - - for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; - - if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { - return true; - } - } - - return false; -} - -bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { - if (!IS_RANGE_USABLE(range)) { - return false; - } - - const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1); - return (channelValue >= 900 + (range->startStep * 25) && - channelValue < 900 + (range->endStep * 25)); -} - -void updateActivatedModes(void) -{ - rcModeActivationMask = 0; - - for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); - - if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) { - ACTIVATE_RC_MODE(modeActivationCondition->modeId); - } - } -} - int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(pidProfile_t *pidProfileToUse) { pidProfile = pidProfileToUse; - isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); + isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM); } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 9be4198d2a..a7eb5c10a0 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -21,47 +21,6 @@ #include "config/parameter_group.h" -typedef enum { - BOXARM = 0, - BOXANGLE, - BOXHORIZON, - BOXBARO, - BOXANTIGRAVITY, - BOXMAG, - BOXHEADFREE, - BOXHEADADJ, - BOXCAMSTAB, - BOXCAMTRIG, - BOXGPSHOME, - BOXGPSHOLD, - BOXPASSTHRU, - BOXBEEPERON, - BOXLEDMAX, - BOXLEDLOW, - BOXLLIGHTS, - BOXCALIB, - BOXGOV, - BOXOSD, - BOXTELEMETRY, - BOXGTUNE, - BOXSONAR, - BOXSERVO1, - BOXSERVO2, - BOXSERVO3, - BOXBLACKBOX, - BOXFAILSAFE, - BOXAIRMODE, - BOX3DDISABLESWITCH, - BOXFPVANGLEMIX, - BOXBLACKBOXERASE, - CHECKBOX_ITEM_COUNT -} boxId_e; - -extern uint32_t rcModeActivationMask; - -#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask) -#define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId)) - typedef enum rc_alias { ROLL = 0, PITCH, @@ -109,17 +68,6 @@ typedef enum { #define THR_CE (3 << (2 * THROTTLE)) #define THR_HI (2 << (2 * THROTTLE)) -#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 - -#define CHANNEL_RANGE_MIN 900 -#define CHANNEL_RANGE_MAX 2100 - -#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step) -#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25) - -#define MIN_MODE_RANGE_STEP 0 -#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25) - // Roll/pitch rates are a proportion used for mixing, so it tops out at 1.0: #define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 100 @@ -128,29 +76,6 @@ typedef enum { #define CONTROL_RATE_CONFIG_TPA_MAX 100 -// steps are 25 apart -// a value of 0 corresponds to a channel value of 900 or less -// a value of 48 corresponds to a channel value of 2100 or more -// 48 steps between 900 and 1200 -typedef struct channelRange_s { - uint8_t startStep; - uint8_t endStep; -} channelRange_t; - -typedef struct modeActivationCondition_s { - boxId_e modeId; - uint8_t auxChannelIndex; - 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; - -#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) - extern float rcCommand[4]; typedef struct rcControlsConfig_s { @@ -186,15 +111,9 @@ bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(void); void processRcStickPositions(throttleStatus_e throttleStatus); -bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); -void updateActivatedModes(void); - -bool isAirmodeActive(void); -bool isAntiGravityModeActive(void); - bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); -bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse); +struct modeActivationCondition_s; +void useRcControlsConfig(struct pidProfile_s *pidProfileToUse); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c new file mode 100644 index 0000000000..16d5369c25 --- /dev/null +++ b/src/main/fc/rc_modes.c @@ -0,0 +1,101 @@ +/* + * 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 "rc_modes.h" + +#include "common/bitarray.h" +#include "common/maths.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 "rx/rx.h" + +boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e + +PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, + PG_MODE_ACTIVATION_PROFILE, 0); + + +bool IS_RC_MODE_ACTIVE(boxId_e boxId) +{ + return bitArrayGet(&rcModeActivationMask, boxId); +} + +void rcModeUpdate(boxBitmask_t *newState) +{ + rcModeActivationMask = *newState; +} + +bool isAirmodeActive(void) { + return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); +} + +bool isAntiGravityModeActive(void) { + return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); +} + +bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { + if (!IS_RANGE_USABLE(range)) { + return false; + } + + const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1); + return (channelValue >= 900 + (range->startStep * 25) && + channelValue < 900 + (range->endStep * 25)); +} + + +void updateActivatedModes(void) +{ + boxBitmask_t newMask; + memset(&newMask, 0, sizeof(newMask)); + + for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { + const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); + + if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) { + boxId_e mode = modeActivationCondition->modeId; + if (mode < CHECKBOX_ITEM_COUNT) + bitArraySet(&newMask, mode); + } + } + rcModeUpdate(&newMask); +} + +bool isModeActivationConditionPresent(boxId_e modeId) +{ + uint8_t index; + + for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { + const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); + + if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { + return true; + } + } + + return false; +} diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h new file mode 100644 index 0000000000..2b0d223786 --- /dev/null +++ b/src/main/fc/rc_modes.h @@ -0,0 +1,105 @@ +/* + * 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" + +typedef enum { + BOXARM = 0, + BOXANGLE, + BOXHORIZON, + BOXBARO, + BOXANTIGRAVITY, + BOXMAG, + BOXHEADFREE, + BOXHEADADJ, + BOXCAMSTAB, + BOXCAMTRIG, + BOXGPSHOME, + BOXGPSHOLD, + BOXPASSTHRU, + BOXBEEPERON, + BOXLEDMAX, + BOXLEDLOW, + BOXLLIGHTS, + BOXCALIB, + BOXGOV, + BOXOSD, + BOXTELEMETRY, + BOXGTUNE, + BOXSONAR, + BOXSERVO1, + BOXSERVO2, + BOXSERVO3, + BOXBLACKBOX, + BOXFAILSAFE, + BOXAIRMODE, + BOX3DDISABLESWITCH, + BOXFPVANGLEMIX, + BOXBLACKBOXERASE, + CHECKBOX_ITEM_COUNT +} boxId_e; + +// type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior +typedef struct { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } boxBitmask_t; + +#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 + +#define CHANNEL_RANGE_MIN 900 +#define CHANNEL_RANGE_MAX 2100 + +#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step) +#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25) + +#define MIN_MODE_RANGE_STEP 0 +#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25) + +// steps are 25 apart +// a value of 0 corresponds to a channel value of 900 or less +// a value of 48 corresponds to a channel value of 2100 or more +// 48 steps between 900 and 1200 +typedef struct channelRange_s { + uint8_t startStep; + uint8_t endStep; +} channelRange_t; + +typedef struct modeActivationCondition_s { + boxId_e modeId; + uint8_t auxChannelIndex; + 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; + +#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) + +bool IS_RC_MODE_ACTIVE(boxId_e boxId); +void rcModeUpdate(boxBitmask_t *newState); + +bool isAirmodeActive(void); +bool isAntiGravityModeActive(void); + +bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); +void updateActivatedModes(void); +bool isModeActivationConditionPresent(boxId_e modeId); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 6907ab8058..f316e5c92b 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -36,10 +36,13 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/light_led.h" + #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/fc_core.h" #include "fc/rc_adjustments.h" +#include "fc/rc_controls.h" #include "fc/settings.h" #include "flight/altitude.h" @@ -225,7 +228,7 @@ static const char * const lookupTableSuperExpoYaw[] = { static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", #ifdef USE_DSHOT - "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200" + "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "PROSHOT1000" #endif }; @@ -593,7 +596,7 @@ const clivalue_t valueTable[] = { // PG_TELEMETRY_CONFIG #ifdef TELEMETRY { "tlm_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_switch) }, - { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, + { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, { "tlm_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, halfDuplex) }, { "frsky_default_lat", VAR_INT16 | MASTER_VALUE, .config.minmax = { -9000, 9000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) }, { "frsky_default_long", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLongitude) }, @@ -603,7 +606,7 @@ const clivalue_t valueTable[] = { { "hott_alarm_int", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 120 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, hottAlarmSoundInterval) }, { "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pidValuesAsTelemetry) }, #if defined(TELEMETRY_IBUS) - { "report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) }, + { "report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) }, #endif #endif @@ -652,7 +655,7 @@ const clivalue_t valueTable[] = { { "osd_debug_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DEBUG]) }, { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_POWER]) }, { "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PIDRATE_PROFILE]) }, - { "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAIN_BATT_WARNING]) }, + { "osd_warnings_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_WARNINGS]) }, { "osd_avg_cell_voltage_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_AVG_CELL_VOLTAGE]) }, { "osd_pit_ang_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PITCH_ANGLE]) }, { "osd_rol_ang_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_ANGLE]) }, @@ -661,6 +664,8 @@ const clivalue_t valueTable[] = { { "osd_disarmed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DISARMED]) }, { "osd_nheading_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_HEADING]) }, { "osd_nvario_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_VARIO]) }, + { "osd_esc_tmp_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_TMP]) }, + { "osd_esc_rpm_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_RPM]) }, { "osd_stat_max_spd", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_MAX_SPEED])}, { "osd_stat_max_dist", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_MAX_DISTANCE])}, @@ -713,6 +718,7 @@ const clivalue_t valueTable[] = { #ifdef USE_ESC_SENSOR { "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) }, #endif + { "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) }, }; const uint16_t valueTableEntryCount = ARRAYLEN(valueTable); diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 62ceb04775..52ddb0a3f5 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -69,34 +69,31 @@ typedef struct lookupTableEntry_s { #define VALUE_TYPE_OFFSET 0 -#define VALUE_SECTION_OFFSET 4 -#define VALUE_MODE_OFFSET 6 +#define VALUE_SECTION_OFFSET 2 +#define VALUE_MODE_OFFSET 4 typedef enum { - // value type, bits 0-3 + // value type, bits 0-1 VAR_UINT8 = (0 << VALUE_TYPE_OFFSET), VAR_INT8 = (1 << VALUE_TYPE_OFFSET), VAR_UINT16 = (2 << VALUE_TYPE_OFFSET), VAR_INT16 = (3 << VALUE_TYPE_OFFSET), - // value section, bits 4-5 + // value section, bits 2-3 MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), - PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20 - // value mode - MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40 - MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80 + PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), + + // value mode, bits 4-5 + MODE_DIRECT = (0 << VALUE_MODE_OFFSET), + MODE_LOOKUP = (1 << VALUE_MODE_OFFSET), + MODE_ARRAY = (2 << VALUE_MODE_OFFSET) } cliValueFlag_e; -#define VALUE_TYPE_MASK (0x0F) -#define VALUE_SECTION_MASK (0x30) -#define VALUE_MODE_MASK (0xC0) -typedef union { - int8_t int8; - uint8_t uint8; - int16_t int16; -} cliVar_t; +#define VALUE_TYPE_MASK (0x03) +#define VALUE_SECTION_MASK (0x0c) +#define VALUE_MODE_MASK (0x30) typedef struct cliMinMaxConfig_s { const int16_t min; @@ -107,9 +104,14 @@ typedef struct cliLookupTableConfig_s { const lookupTableIndex_e tableIndex; } cliLookupTableConfig_t; +typedef struct cliArrayLengthConfig_s { + const uint8_t length; +} cliArrayLengthConfig_t; + typedef union { cliLookupTableConfig_t lookup; cliMinMaxConfig_t minmax; + cliArrayLengthConfig_t array; } cliValueConfig_t; typedef struct { diff --git a/src/main/flight/altitude.c b/src/main/flight/altitude.c index ada46f6d4c..7ae6f62496 100644 --- a/src/main/flight/altitude.c +++ b/src/main/flight/altitude.c @@ -33,6 +33,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/altitude.h" diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index bf21c74d87..a9c9e9c877 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -31,6 +31,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 094e16ae5b..818b2e9fb8 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -40,7 +40,9 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/fc_core.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -51,16 +53,6 @@ #include "sensors/battery.h" - -PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); - -PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, - .deadband3d_low = 1406, - .deadband3d_high = 1514, - .neutral3d = 1460, - .deadband3d_throttle = 50 -); - PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); #ifndef TARGET_DEFAULT_MIXER @@ -119,12 +111,11 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight - static uint8_t motorCount; static float motorMixRange; -int16_t motor[MAX_SUPPORTED_MOTORS]; -int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +float motor[MAX_SUPPORTED_MOTORS]; +float motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -321,8 +312,8 @@ const mixer_t mixers[] = { }; #endif // !USE_QUAD_MIXER_ONLY -static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; -uint16_t motorOutputHigh, motorOutputLow; +static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; +float motorOutputHigh, motorOutputLow; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; uint8_t getMotorCount() @@ -361,17 +352,18 @@ bool isMotorProtocolDshot(void) { #endif } -// Add here scaled ESC outputs for digital protol +// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator +// DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; if (feature(FEATURE_3D)) - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); + motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); else - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); + motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; } else #endif @@ -518,7 +510,7 @@ void mixTable(pidProfile_t *pidProfile) // Scale roll/pitch/yaw uniformly to fit within throttle range // Initial mixer concept by bdoiron74 reused and optimized for Air Mode float throttle = 0, currentThrottleInputRange = 0; - uint16_t motorOutputMin, motorOutputMax; + float motorOutputMin, motorOutputMax; static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions bool mixerInversion = false; @@ -576,11 +568,14 @@ void mixTable(pidProfile_t *pidProfile) float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed); + int motorDirection = GET_DIRECTION(isMotorsReversed()); + + for (int i = 0; i < motorCount; i++) { float mix = - scaledAxisPidRoll * currentMixer[i].roll + - scaledAxisPidPitch * currentMixer[i].pitch + - scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection); + scaledAxisPidRoll * currentMixer[i].roll * (motorDirection) + + scaledAxisPidPitch * currentMixer[i].pitch * (motorDirection) + + scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection) * (motorDirection); if (vbatCompensationFactor > 1.0f) { mix *= vbatCompensationFactor; // Add voltage compensation @@ -614,7 +609,7 @@ void mixTable(pidProfile_t *pidProfile) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (uint32_t i = 0; i < motorCount; i++) { - float motorOutput = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))); + float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)); // Dshot works exactly opposite in lower 3D section. if (mixerInversion) { @@ -648,7 +643,7 @@ void mixTable(pidProfile_t *pidProfile) } } -uint16_t convertExternalToMotor(uint16_t externalValue) +float convertExternalToMotor(uint16_t externalValue) { uint16_t motorValue = externalValue; #ifdef USE_DSHOT @@ -667,12 +662,12 @@ uint16_t convertExternalToMotor(uint16_t externalValue) } #endif - return motorValue; + return (float)motorValue; } -uint16_t convertMotorToExternal(uint16_t motorValue) +uint16_t convertMotorToExternal(float motorValue) { - uint16_t externalValue = motorValue; + uint16_t externalValue = lrintf(motorValue); #ifdef USE_DSHOT if (isMotorProtocolDshot()) { if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 51733d31e7..665b39fd25 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -118,8 +118,8 @@ PG_DECLARE(motorConfig_t, motorConfig); #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF extern const mixer_t mixers[]; -extern int16_t motor[MAX_SUPPORTED_MOTORS]; -extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +extern float motor[MAX_SUPPORTED_MOTORS]; +extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; struct rxConfig_s; uint8_t getMotorCount(); @@ -141,5 +141,5 @@ void stopMotors(void); void stopPwmAllMotors(void); bool isMotorProtocolDshot(void); -uint16_t convertExternalToMotor(uint16_t externalValue); -uint16_t convertMotorToExternal(uint16_t motorValue); +float convertExternalToMotor(uint16_t externalValue); +uint16_t convertMotorToExternal(float motorValue); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index d5ce24a515..1ee364b87b 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -37,7 +37,7 @@ #include "fc/config.h" #include "fc/fc_core.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/imu.h" diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index d137127416..72b08821e6 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -38,6 +38,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/imu.h" diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 9b610eadf5..3b35e4be8f 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -28,6 +28,9 @@ #include "drivers/sound_beeper.h" #include "drivers/time.h" +#include "drivers/pwm_output.h" + +#include "flight/mixer.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -153,7 +156,6 @@ static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2]; #define BEEPER_CONFIRMATION_BEEP_DURATION 2 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20 - // Beeper off = 0 Beeper on = 1 static uint8_t beeperIsOn = 0; @@ -338,6 +340,14 @@ void beeperUpdate(timeUs_t currentTimeUs) return; } + #ifdef USE_DSHOT + if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) { + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_BEEP3); + } + } + #endif + if (!beeperIsOn) { beeperIsOn = 1; if (currentBeeperEntry->sequence[beeperPos] != 0) { diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 9b0154d1a0..0be72d3a94 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -44,6 +44,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 88d12a18d7..675ec10630 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -58,6 +58,7 @@ #include "drivers/vtx_common.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/beeper.h" #include "io/flashfs.h" #include "io/gps.h" #include "io/osd.h" @@ -79,6 +80,7 @@ #include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/sensors.h" +#include "sensors/esc_sensor.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -95,7 +97,8 @@ // Blink control -bool blinkState = true; +static bool blinkState = true; +static bool showVisualBeeper = false; static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32]; #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32))) @@ -154,6 +157,10 @@ static const char compassBar[] = { PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0); +#ifdef USE_ESC_SENSOR +static escSensorData_t *escData; +#endif + /** * Gets the correct altitude symbol for the current unit system */ @@ -518,7 +525,7 @@ static void osdDrawSingleElement(uint8_t item) break; } - case OSD_MAIN_BATT_WARNING: + case OSD_WARNINGS: switch(getBatteryState()) { case BATTERY_WARNING: tfp_sprintf(buff, "LOW BATTERY"); @@ -529,7 +536,13 @@ static void osdDrawSingleElement(uint8_t item) break; default: - return; + if (showVisualBeeper) { + tfp_sprintf(buff, " * * * *"); + } else { + return; + } + break; + } break; @@ -605,6 +618,16 @@ static void osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10)); break; } +#ifdef USE_ESC_SENSOR + case OSD_ESC_TMP: + buff[0] = SYM_TEMP_C; + tfp_sprintf(buff + 1, "%d", escData == NULL ? 0 : escData->temperature); + break; + + case OSD_ESC_RPM: + tfp_sprintf(buff, "%d", escData == NULL ? 0 : escData->rpm); + break; +#endif default: return; @@ -613,7 +636,7 @@ static void osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX + elemOffsetX, elemPosY, buff); } -void osdDrawElements(void) +static void osdDrawElements(void) { displayClearScreen(osdDisplayPort); @@ -621,13 +644,6 @@ void osdDrawElements(void) if (IS_RC_MODE_ACTIVE(BOXOSD)) return; -#if 0 - if (currentElement) - osdDrawElementPositioningHelp(); -#else - if (false) - ; -#endif #ifdef CMS else if (sensors(SENSOR_ACC) || displayIsGrabbed(osdDisplayPort)) #else @@ -654,7 +670,7 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_YAW_PIDS); osdDrawSingleElement(OSD_POWER); osdDrawSingleElement(OSD_PIDRATE_PROFILE); - osdDrawSingleElement(OSD_MAIN_BATT_WARNING); + osdDrawSingleElement(OSD_WARNINGS); osdDrawSingleElement(OSD_AVG_CELL_VOLTAGE); osdDrawSingleElement(OSD_DEBUG); osdDrawSingleElement(OSD_PITCH_ANGLE); @@ -681,6 +697,13 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_HOME_DIR); } #endif // GPS + +#ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + osdDrawSingleElement(OSD_ESC_TMP); + osdDrawSingleElement(OSD_ESC_RPM); + } +#endif } void pgResetFn_osdConfig(osdConfig_t *osdConfig) @@ -706,7 +729,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[OSD_YAW_PIDS] = OSD_POS(7, 15) | VISIBLE_FLAG; osdConfig->item_pos[OSD_POWER] = OSD_POS(1, 10) | VISIBLE_FLAG; osdConfig->item_pos[OSD_PIDRATE_PROFILE] = OSD_POS(25, 10) | VISIBLE_FLAG; - osdConfig->item_pos[OSD_MAIN_BATT_WARNING] = OSD_POS(9, 10) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | VISIBLE_FLAG; osdConfig->item_pos[OSD_AVG_CELL_VOLTAGE] = OSD_POS(12, 2) | VISIBLE_FLAG; osdConfig->item_pos[OSD_DEBUG] = OSD_POS(1, 0) | VISIBLE_FLAG; osdConfig->item_pos[OSD_PITCH_ANGLE] = OSD_POS(1, 8) | VISIBLE_FLAG; @@ -721,6 +744,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[OSD_DISARMED] = OSD_POS(10, 4) | VISIBLE_FLAG; osdConfig->item_pos[OSD_NUMERICAL_HEADING] = OSD_POS(23, 9) | VISIBLE_FLAG; osdConfig->item_pos[OSD_NUMERICAL_VARIO] = OSD_POS(23, 8) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_ESC_TMP] = OSD_POS(18, 2) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_ESC_RPM] = OSD_POS(19, 2) | VISIBLE_FLAG; osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true; osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true; @@ -802,12 +827,12 @@ void osdUpdateAlarms(void) CLR_BLINK(OSD_RSSI_VALUE); if (getBatteryState() == BATTERY_OK) { + CLR_BLINK(OSD_WARNINGS); CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); - CLR_BLINK(OSD_MAIN_BATT_WARNING); CLR_BLINK(OSD_AVG_CELL_VOLTAGE); } else { + SET_BLINK(OSD_WARNINGS); SET_BLINK(OSD_MAIN_BATT_VOLTAGE); - SET_BLINK(OSD_MAIN_BATT_WARNING); SET_BLINK(OSD_AVG_CELL_VOLTAGE); } @@ -839,7 +864,7 @@ void osdResetAlarms(void) { CLR_BLINK(OSD_RSSI_VALUE); CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); - CLR_BLINK(OSD_MAIN_BATT_WARNING); + CLR_BLINK(OSD_WARNINGS); CLR_BLINK(OSD_GPS_SATS); CLR_BLINK(OSD_FLYTIME); CLR_BLINK(OSD_MAH_DRAWN); @@ -1064,6 +1089,12 @@ static void osdRefresh(timeUs_t currentTimeUs) blinkState = (currentTimeUs / 200000) % 2; +#ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + escData = getEscSensorData(ESC_SENSOR_COMBINED); + } +#endif + #ifdef CMS if (!displayIsGrabbed(osdDisplayPort)) { osdUpdateAlarms(); @@ -1083,6 +1114,11 @@ static void osdRefresh(timeUs_t currentTimeUs) void osdUpdate(timeUs_t currentTimeUs) { static uint32_t counter = 0; + + if (isBeeperOn()) { + showVisualBeeper = true; + } + #ifdef MAX7456_DMA_CHANNEL_TX // don't touch buffers if DMA transaction is in progress if (displayIsTransferInProgress(osdDisplayPort)) { @@ -1108,6 +1144,8 @@ void osdUpdate(timeUs_t currentTimeUs) if (counter++ % DRAW_FREQ_DENOM == 0) { osdRefresh(currentTimeUs); + + showVisualBeeper = false; } else { // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle displayDrawScreen(osdDisplayPort); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 90ef5eb0bd..133517e7a1 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -48,7 +48,7 @@ typedef enum { OSD_YAW_PIDS, OSD_POWER, OSD_PIDRATE_PROFILE, - OSD_MAIN_BATT_WARNING, + OSD_WARNINGS, OSD_AVG_CELL_VOLTAGE, OSD_GPS_LON, OSD_GPS_LAT, @@ -63,6 +63,8 @@ typedef enum { OSD_NUMERICAL_HEADING, OSD_NUMERICAL_VARIO, OSD_COMPASS_BAR, + OSD_ESC_TMP, + OSD_ESC_RPM, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 76007d6172..3715b60e00 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -17,7 +17,7 @@ #pragma once -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10 diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 9a9ddb0f4b..702898bb9b 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -23,7 +23,8 @@ typedef enum { MSP_RESULT_ACK = 1, MSP_RESULT_ERROR = -1, - MSP_RESULT_NO_REPLY = 0 + MSP_RESULT_NO_REPLY = 0, + MSP_RESULT_CMD_UNKNOWN = -2, // don't know how to process command, try next handler } mspResult_e; typedef enum { diff --git a/src/main/osd_slave/osd_slave_init.c b/src/main/osd_slave/osd_slave_init.c index df95914da9..b63566538c 100644 --- a/src/main/osd_slave/osd_slave_init.c +++ b/src/main/osd_slave/osd_slave_init.c @@ -34,6 +34,7 @@ #include "config/parameter_group_ids.h" #include "drivers/adc.h" +#include "drivers/bus.h" #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "drivers/dma.h" @@ -92,10 +93,6 @@ void targetPreInit(void); #endif -#ifdef TARGET_BUS_INIT -void targetBusInit(void); -#endif - uint8_t systemState = SYSTEM_STATE_INITIALISING; void processLoopback(void) @@ -199,6 +196,11 @@ void init(void) #endif /* USE_SPI */ #ifdef USE_I2C + i2cHardwareConfigure(); + + // Note: Unlike UARTs which are configured when client is present, + // I2C buses are initialized unconditionally if they are configured. + #ifdef USE_I2C_DEVICE_1 i2cInit(I2CDEV_1); #endif diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index c8deb8010c..46c5f95208 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -41,6 +41,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "flight/failsafe.h" @@ -287,6 +288,7 @@ void rxInit(void) rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; // Initialize ARM switch to OFF position when arming via switch is defined + // TODO - move to rc_mode.c for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(i); if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) { diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index f6e6647851..fd2078a855 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -211,7 +211,6 @@ void currentMeterESCReadCombined(currentMeter_t *meter) meter->amperageLatest = currentMeterESCState.amperage; meter->amperage = currentMeterESCState.amperage; meter->mAhDrawn = currentMeterESCState.mAhDrawn; - currentMeterReset(meter); } void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter) diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 42b292f0b2..33fb04fb8c 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB5 // Blue LED - PB5 +#define LED0_PIN PB5 // Blue LED - PB5 #define BEEPER PA0 diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index b0a6b87197..f91f87b3ac 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -22,8 +22,8 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define CONFIG_PREFER_ACC_ON -#define LED0 PB3 -#define LED1 PB4 +#define LED0_PIN PB3 +#define LED1_PIN PB4 #define BEEPER PA12 #define BEEPER_INVERTED diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 55172ae6bc..596b4f47a7 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -22,8 +22,8 @@ #define BRUSHED_ESC_AUTODETECT -#define LED0 PB3 -#define LED1 PB4 +#define LED0_PIN PB3 +#define LED1_PIN PB4 #define BEEPER PA12 diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index dee3884cb2..200915262f 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -57,7 +57,7 @@ void targetConfiguration(void) { /* depending on revision ... depends on the LEDs to be utilised. */ if (hardwareRevision == AFF3_REV_2) { - statusLedConfigMutable()->polarity = 0 + statusLedConfigMutable()->inversion = 0 #ifdef LED0_A_INVERTED | BIT(0) #endif @@ -69,17 +69,17 @@ void targetConfiguration(void) #endif ; - for (int i = 0; i < LED_NUMBER; i++) { - statusLedConfigMutable()->ledTags[i] = IO_TAG_NONE; + for (int i = 0; i < STATUS_LED_NUMBER; i++) { + statusLedConfigMutable()->ioTags[i] = IO_TAG_NONE; } #ifdef LED0_A - statusLedConfigMutable()->ledTags[0] = IO_TAG(LED0_A); + statusLedConfigMutable()->ioTags[0] = IO_TAG(LED0_A); #endif #ifdef LED1_A - statusLedConfigMutable()->ledTags[1] = IO_TAG(LED1_A); + statusLedConfigMutable()->ioTags[1] = IO_TAG(LED1_A); #endif #ifdef LED2_A - statusLedConfigMutable()->ledTags[2] = IO_TAG(LED2_A); + statusLedConfigMutable()->ioTags[2] = IO_TAG(LED2_A); #endif } else { gyroConfigMutable()->gyro_sync_denom = 2; diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 8e1ecf53eb..f0ce648bc9 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -19,7 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "AFF3" // AlienFlight F3. #define TARGET_CONFIG -#define TARGET_BUS_INIT #define REMAP_TIM17_DMA #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT @@ -29,8 +28,8 @@ #define BRUSHED_ESC_AUTODETECT // LED's V1 -#define LED0 PB4 -#define LED1 PB5 +#define LED0_PIN PB4 +#define LED1_PIN PB5 // LED's V2 #define LED0_A PB8 diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index cf7b5f17cf..bb7b3329d0 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -25,8 +25,8 @@ #define USBD_PRODUCT_STRING "AlienFlight F4" -#define LED0 PC12 -#define LED1 PD2 +#define LED0_PIN PC12 +#define LED1_PIN PD2 #define BEEPER PC13 #define BEEPER_INVERTED diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index ca4cfcd439..7e07c38670 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -25,8 +25,8 @@ #define USBD_PRODUCT_STRING "AlienFlightNG F7" -#define LED0 PC12 -#define LED1 PD2 +#define LED0_PIN PC12 +#define LED1_PIN PD2 #define BEEPER PC13 #define BEEPER_INVERTED diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index e6939b3d8a..01bcfc995c 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -20,7 +20,7 @@ #define USBD_PRODUCT_STRING "BeeRotorF4" -#define LED0 PB4 +#define LED0_PIN PB4 #define BEEPER PB3 #define BEEPER_INVERTED @@ -126,6 +126,7 @@ //#define SPI_RX_CS_PIN PD2 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) #define I2C1_SCL PB6 #define I2C1_SDA PB7 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index cd5516755a..a9ec2522bd 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -28,9 +28,9 @@ #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define LED0 PB6 -#define LED1 PB5 -#define LED2 PB4 +#define LED0_PIN PB6 +#define LED1_PIN PB5 +#define LED2_PIN PB4 #define BEEPER PC1 #define BEEPER_OPT PB7 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 7fdd2af6b7..d03e0aa676 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -17,7 +17,7 @@ #define TARGET_BOARD_IDENTIFIER "CC3D" // CopterControl 3D -#define LED0 PB3 +#define LED0_PIN PB3 #define INVERTER_PIN_UART1 PB2 // PB2 (BOOT1) used as inverter select GPIO diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index c44f461412..848cfd3c66 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -24,9 +24,9 @@ #define STM32F3DISCOVERY #endif -#define LED0 PE8 // Blue LEDs - PE8/PE12 +#define LED0_PIN PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1_PIN PE10 // Orange LEDs - PE10/PE14 #define LED1_INVERTED #define BEEPER PE9 // Red LEDs - PE9/PE13 diff --git a/src/main/target/CJMCU/initialisation.c b/src/main/target/CJMCU/initialisation.c index 884ea03350..01986ce33e 100644 --- a/src/main/target/CJMCU/initialisation.c +++ b/src/main/target/CJMCU/initialisation.c @@ -19,18 +19,20 @@ #include #include "platform.h" +#include "drivers/bus.h" #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "io/serial.h" void targetBusInit(void) { - #if defined(USE_SPI) && defined(USE_SPI_DEVICE_1) - spiInit(SPIDEV_1); - #endif +#if defined(USE_SPI) && defined(USE_SPI_DEVICE_1) + spiInit(SPIDEV_1); +#endif if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { serialRemovePort(SERIAL_PORT_USART3); + i2cHardwareConfigure(); i2cInit(I2C_DEVICE); } -} \ No newline at end of file +} diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index cfb52ab8b0..84d573b88c 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -21,9 +21,9 @@ #define USE_HARDWARE_REVISION_DETECTION #define TARGET_BUS_INIT -#define LED0 PC14 -#define LED1 PC13 -#define LED2 PC15 +#define LED0_PIN PC14 +#define LED1_PIN PC13 +#define LED2_PIN PC15 #undef BEEPER diff --git a/src/main/target/CLRACINGF4/target.h b/src/main/target/CLRACINGF4/target.h index 0b0638d659..8ddfe2e402 100644 --- a/src/main/target/CLRACINGF4/target.h +++ b/src/main/target/CLRACINGF4/target.h @@ -24,7 +24,7 @@ #endif -#define LED0 PB5 +#define LED0_PIN PB5 #define BEEPER PB4 #define BEEPER_INVERTED #define BEEPER_PWM_HZ 3800 // Beeper PWM frequency in Hz diff --git a/src/main/target/CLRACINGF7/target.h b/src/main/target/CLRACINGF7/target.h index 7d025fd3ea..62ab3fe003 100644 --- a/src/main/target/CLRACINGF7/target.h +++ b/src/main/target/CLRACINGF7/target.h @@ -20,7 +20,7 @@ #define USBD_PRODUCT_STRING "CLRACINGF7" -#define LED0 PB0 +#define LED0_PIN PB0 #define BEEPER PB4 #define BEEPER_INVERTED @@ -87,7 +87,8 @@ #define SERIAL_PORT_COUNT 5 -#define USE_I2C +// XXX To target maintainer: Bus device to configure must be specified. +//#define USE_I2C #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 47f1a60ceb..14df4b4594 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -26,8 +26,8 @@ #define TARGET_XTAL_MHZ 16 -#define LED0 PC14 -#define LED1 PC13 +#define LED0_PIN PC14 +#define LED1_PIN PC13 #define BEEPER PC5 diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 970fab00f8..9da520de11 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -139,7 +139,7 @@ static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT]; // this is the number of filled indexes in above array static uint8_t activeBoxIdCount = 0; // from mixer.c -extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; // cause reboot after BST processing complete static bool isRebootScheduled = false; @@ -483,7 +483,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationConditions(0), currentPidProfile); + useRcControlsConfig(currentPidProfile); } else { ret = BST_FAILED; } diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 2e17b07b8b..12d4cdeebc 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -16,9 +16,11 @@ */ #include +#include #include +#include "drivers/bus_i2c.h" #include "drivers/io.h" #include "drivers/dma.h" #include "drivers/timer.h" @@ -46,10 +48,12 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { }; - +// XXX Requires some additional work here. +// XXX Can't do this now without proper semantics about I2C on this target. #ifdef USE_BST void targetBusInit(void) { + i2cHardwareConfigure(); bstInit(BST_DEVICE); } #endif diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 52bd44f43a..73843b2e52 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -26,9 +26,9 @@ #undef USE_RX_MSP // never used. -#define LED0 PC15 -#define LED1 PC14 -#define LED2 PC13 +#define LED0_PIN PC15 +#define LED1_PIN PC14 +#define LED2_PIN PC13 #define BEEPER PB13 #define BEEPER_INVERTED diff --git a/src/main/target/CRAZYFLIE2/target.h b/src/main/target/CRAZYFLIE2/target.h index e61ed7c0fd..2a45adf722 100644 --- a/src/main/target/CRAZYFLIE2/target.h +++ b/src/main/target/CRAZYFLIE2/target.h @@ -42,9 +42,9 @@ #define USED_TIMERS ( TIM_N(2) | TIM_N(4) ) #endif -#define LED0 PD2 -#define LED1 PC0 -#define LED2 PC3 +#define LED0_PIN PD2 +#define LED1_PIN PC0 +#define LED2_PIN PC3 // Using STM32F405RG, 64 pin package (LQFP64) // 16 pins per port, ports A, B, C, and also PD2 diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index cd6206c225..a59e827abc 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -22,11 +22,11 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT // tqfp48 pin 34 -#define LED0 PA13 +#define LED0_PIN PA13 // tqfp48 pin 37 -#define LED1 PA14 +#define LED1_PIN PA14 // tqfp48 pin 38 -#define LED2 PA15 +#define LED2_PIN PA15 #define BEEPER PB2 #define BEEPER_INVERTED diff --git a/src/main/target/ELLE0/target.h b/src/main/target/ELLE0/target.h index ec5cd8aaae..9ba906cc4f 100644 --- a/src/main/target/ELLE0/target.h +++ b/src/main/target/ELLE0/target.h @@ -22,9 +22,9 @@ #define USBD_PRODUCT_STRING "Elle0" -#define LED0 PA8 -#define LED1 PB4 -#define LED2 PC2 +#define LED0_PIN PA8 +#define LED1_PIN PB4 +#define LED2_PIN PC2 // MPU9250 interrupt #define USE_EXTI diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index df1f436278..91280a0fa5 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -20,9 +20,9 @@ #define USBD_PRODUCT_STRING "Swift-Flyer F4BY" -#define LED0 PE3 // Blue LED -#define LED1 PE2 // Red LED -#define LED2 PE1 // Blue LED +#define LED0_PIN PE3 // Blue LED +#define LED1_PIN PE2 // Red LED +#define LED2_PIN PE1 // Blue LED #define BEEPER PE5 diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 1f66ab2438..5e5069d0c7 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "FORT" #define USBD_PRODUCT_STRING "FortiniF4" /*--------------LED----------------*/ -#define LED0 PB5 -#define LED1 PB6 +#define LED0_PIN PB5 +#define LED1_PIN PB6 #define LED_STRIP /*---------------------------------*/ diff --git a/src/main/target/FF_PIKOBLX/target.h b/src/main/target/FF_PIKOBLX/target.h index f340f76369..2b7d9ed722 100644 --- a/src/main/target/FF_PIKOBLX/target.h +++ b/src/main/target/FF_PIKOBLX/target.h @@ -32,8 +32,8 @@ #define TARGET_CONFIG #define BRUSHED_ESC_AUTODETECT -#define LED0 PB9 -#define LED1 PB5 +#define LED0_PIN PB9 +#define LED1_PIN PB5 #define BEEPER PA0 #define BEEPER_INVERTED diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 10ee4f1588..ab0cb499cc 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "PIK4" #define USBD_PRODUCT_STRING "PikoF4" /*--------------LED----------------*/ -#define LED0 PA15 -#define LED1 PB6 +#define LED0_PIN PA15 +#define LED1_PIN PB6 #define LED_STRIP /*---------------------------------*/ diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 8855c40c0c..027919a90d 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -21,8 +21,8 @@ #define USBD_PRODUCT_STRING "FishDroneF4" -#define LED0 PC13 -#define LED1 PC14 +#define LED0_PIN PC13 +#define LED1_PIN PC14 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index e3d4484e01..550666518b 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -21,7 +21,7 @@ #define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index f715d723d2..26b83a916e 100644 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -19,7 +19,7 @@ #define USBD_PRODUCT_STRING "FRSKYF4" #define TARGET_CONFIG -#define LED0 PB5 +#define LED0_PIN PB5 #define BEEPER PB4 #define BEEPER_INVERTED diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index a58c27a134..5a9d893841 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -28,7 +28,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define CONFIG_PREFER_ACC_ON -#define LED0 PC14 +#define LED0_PIN PC14 #define BEEPER PC15 #define BEEPER_INVERTED @@ -127,6 +127,7 @@ #endif #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) #define I2C1_SCL PB8 #define I2C1_SDA PB9 diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 45d9c65113..182b662e30 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -25,8 +25,8 @@ #define USBD_PRODUCT_STRING "FuryF4" #endif -#define LED0 PB5 -#define LED1 PB4 +#define LED0_PIN PB5 +#define LED1_PIN PB4 #define BEEPER PA8 #define BEEPER_INVERTED diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index e7c09271b1..335d1513a1 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -21,8 +21,8 @@ #define USBD_PRODUCT_STRING "FuryF7" -#define LED0 PB5 -#define LED1 PB4 +#define LED0_PIN PB5 +#define LED1_PIN PB4 #define BEEPER PD10 #define BEEPER_INVERTED diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 6a4e3a140e..f63687d568 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB7 +#define LED0_PIN PB7 #define BEEPER PC15 diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index bec37f4b37..41a478ffa5 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #define USE_EXTI #define MPU_INT_EXTI PC13 diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h index e9eef103fd..2367ef2151 100644 --- a/src/main/target/ISHAPEDF3/target.h +++ b/src/main/target/ISHAPEDF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 8e2874df0c..8edd69b2fc 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -20,9 +20,9 @@ #define USBD_PRODUCT_STRING "KakuteF4-V1" -#define LED0 PB5 -#define LED1 PB4 -#define LED2 PB6 +#define LED0_PIN PB5 +#define LED1_PIN PB4 +#define LED2_PIN PB6 #define BEEPER PC9 #define BEEPER_INVERTED diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index a552c0089c..46e7b3c445 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -29,7 +29,7 @@ #define ESCSERIAL_TIMER_TX_HARDWARE 6 #define REMAP_TIM17_DMA -#define LED0 PB1 +#define LED0_PIN PB1 #define BEEPER PB13 #define BEEPER_INVERTED diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index bcc469df0d..0f72485724 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -32,11 +32,11 @@ #endif #if defined(PLUMF4) || defined(KIWIF4V2) -#define LED0 PB4 +#define LED0_PIN PB4 #else -#define LED0 PB5 -#define LED1 PB4 +#define LED0_PIN PB5 +#define LED1_PIN PB4 #endif #define BEEPER PA8 diff --git a/src/main/target/KROOZX/initialisation.c b/src/main/target/KROOZX/initialisation.c index 624615f978..d6b8c009cd 100755 --- a/src/main/target/KROOZX/initialisation.c +++ b/src/main/target/KROOZX/initialisation.c @@ -21,33 +21,6 @@ #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" -void targetBusInit(void) -{ -#ifdef USE_SPI - #ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); - #endif - #ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); - #endif - #ifdef USE_SPI_DEVICE_3 - spiInit(SPIDEV_3); - #endif - #ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4); - #endif -#endif - -#ifdef USE_I2C - #ifdef USE_I2C_DEVICE_1 - i2cInit(I2CDEV_1); - #endif - #ifdef USE_I2C_DEVICE_3 - i2cInit(I2CDEV_3); - #endif -#endif -} - void targetPreInit(void) { IO_t osdChSwitch = IOGetByTag(IO_TAG(OSD_CH_SWITCH)); diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index a493c79b28..d8fa00132b 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -25,11 +25,10 @@ #define USBD_PRODUCT_STRING "KroozX" #define TARGET_CONFIG -#define TARGET_BUS_INIT #define TARGET_PREINIT -#define LED0 PA14 // Red LED -#define LED1 PA13 // Green LED +#define LED0_PIN PA14 // Red LED +#define LED1_PIN PA13 // Green LED #define BEEPER PC1 diff --git a/src/main/target/LUMBAF3/target.h b/src/main/target/LUMBAF3/target.h index cdcf2ea743..04c6650772 100644 --- a/src/main/target/LUMBAF3/target.h +++ b/src/main/target/LUMBAF3/target.h @@ -17,7 +17,7 @@ #define TARGET_BOARD_IDENTIFIER "MCF3" // LumbaF3 Flight Controller by mC -#define LED0 PB3 +#define LED0_PIN PB3 #define BEEPER PC15 // MPU6000 interrupts @@ -88,8 +88,3 @@ #define USABLE_TIMER_CHANNEL_COUNT 8 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) ) - -#define USE_I2C -#define I2C_DEVICE (I2CDEV_2) -#define I2C2_SCL PB2 -#define I2C2_SDA PB1 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 88e86f5ad8..fea3621eea 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -26,10 +26,10 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PC15 -#define LED1 PC14 +#define LED0_PIN PC15 +#define LED1_PIN PC14 #ifndef LUXV2_RACE -#define LED2 PC13 +#define LED2_PIN PC13 #endif #ifdef LUXV2_RACE diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 430fb68a55..bff65e156e 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -22,8 +22,8 @@ #define USBD_PRODUCT_STRING "MatekF4" -#define LED0 PB9 -#define LED1 PA14 +#define LED0_PIN PB9 +#define LED1_PIN PA14 #define BEEPER PC13 #define BEEPER_INVERTED diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index a741a3f23c..7c2431aa44 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "MSKY" // Micro sciSKY -#define LED0 PB3 -#define LED1 PB4 +#define LED0_PIN PB3 +#define LED1_PIN PB4 #define BEEPER PA12 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index c9d6db3056..f9e0d681ff 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -22,8 +22,8 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define TARGET_CONFIG -#define LED0 PB5 // Blue LEDs - PB5 -//#define LED1 PB9 // Green LEDs - PB9 +#define LED0_PIN PB5 // Blue LEDs - PB5 +//#define LED1_PIN PB9 // Green LEDs - PB9 #define BEEPER PA0 #define BEEPER_INVERTED diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 4ba3a85876..aff23afdea 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -27,6 +27,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" +#include "fc/rc_modes.h" #include "fc/rc_controls.h" #include "flight/failsafe.h" diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index d646884666..0e249af074 100755 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/src/main/target/MULTIFLITEPICO/target.h @@ -22,7 +22,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/NAZE/initialisation.c b/src/main/target/NAZE/initialisation.c index 0d75c2c698..6f30d32602 100644 --- a/src/main/target/NAZE/initialisation.c +++ b/src/main/target/NAZE/initialisation.c @@ -19,6 +19,7 @@ #include #include "platform.h" +#include "drivers/bus.h" #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "io/serial.h" @@ -26,20 +27,22 @@ void targetBusInit(void) { - #ifdef USE_SPI - #ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); - #endif - #endif +#ifdef USE_SPI +#ifdef USE_SPI_DEVICE_2 + spiInit(SPIDEV_2); +#endif +#endif if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { serialRemovePort(SERIAL_PORT_USART3); + i2cHardwareConfigure(); i2cInit(I2C_DEVICE); } } else { + i2cHardwareConfigure(); i2cInit(I2C_DEVICE); } } diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index fd267ae29d..1de6785bf4 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -26,8 +26,8 @@ #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define LED0 PB3 -#define LED1 PB4 +#define LED0_PIN PB3 +#define LED1_PIN PB4 #define BEEPER PA12 diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index 3756f95f10..3106be009f 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -24,9 +24,9 @@ #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define LED0 PB6 -#define LED1 PB5 -#define LED2 PB4 +#define LED0_PIN PB6 +#define LED1_PIN PB5 +#define LED2_PIN PB4 #define BEEPER PC1 #define BEEPER_INVERTED diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 4ee2033cb3..5e47a26e11 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -26,7 +26,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #define BEEPER PC15 #define BEEPER_INVERTED @@ -77,13 +77,14 @@ #define UART2_TX_PIN PA14 // PA14 / SWCLK #define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (PWM5) +#define UART3_RX_PIN PB11 // PB11 (PWM6) -#undef USE_I2C -//#define USE_I2C -//#define USE_I2C_DEVICE_1 -//#define I2C_DEVICE (I2CDEV_1) +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL NONE // PB6 (PWM8) +#define I2C1_SDA NONE // PB7 (PWM7) +#define I2C_DEVICE (I2CDEV_1) #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 90fb45c374..3775074832 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -33,8 +33,8 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" // Remove this at the next major release (?) #endif -#define LED0 PB5 -//#define LED1 PB4 // Remove this at the next major release +#define LED0_PIN PB5 +//#define LED1_PIN PB4 // Remove this at the next major release #define BEEPER PB4 #define BEEPER_INVERTED @@ -93,9 +93,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD #define USE_SDCARD_SPI2 -#if defined(OMNIBUSF4SD) #define SDCARD_DETECT_INVERTED -#endif #define SDCARD_DETECT_PIN PB7 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN @@ -167,6 +165,18 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL NONE // PB10, shared with UART3TX +#define I2C2_SDA NONE // PB11, shared with UART3RX +#if defined(OMNIBUSF4) || defined(OMNIBUSF4SD) +#define USE_I2C_DEVICE_3 +#define I2C3_SCL NONE // PA8, PWM6 +#define I2C3_SDA NONE // PC9, CH6 +#endif +#define I2C_DEVICE (I2CDEV_2) +#define OLED_I2C_INSTANCE (I2CDEV_3) + #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 #define VBAT_ADC_PIN PC2 diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index d62348e721..2461db0b35 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -19,7 +19,7 @@ #define USBD_PRODUCT_STRING "OmnibusF7" -#define LED0 PE0 +#define LED0_PIN PE0 #define BEEPER PD15 #define BEEPER_INVERTED @@ -134,7 +134,9 @@ #define USE_I2C #define USE_I2C_DEVICE_2 -#define I2C_DEVICE (I2CDEV_2) +#define I2C_DEVICE (I2CDEV_2) +#define I2C2_SCL NONE // PB10 (UART3_TX) +#define I2C2_SDA NONE // PB11 (UART3_RX) #define BARO #define USE_BARO_BMP280 diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 1fe543c435..139d760d08 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -21,10 +21,10 @@ #define USE_HARDWARE_REVISION_DETECTION #define TARGET_CONFIG -#define LED0 PB3 +#define LED0_PIN PB3 #define LED0_INVERTED -#define LED1 PB4 +#define LED1_PIN PB4 #define LED1_INVERTED #define BEEPER PA12 diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 5dfa3dca79..93f1e37309 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -21,8 +21,8 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB4 -#define LED1 PB5 +#define LED0_PIN PB4 +#define LED1_PIN PB5 #define BEEPER PA0 #define BEEPER_INVERTED diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index a2b178f1e1..0c3926c7e5 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -48,10 +48,10 @@ #endif -#define LED0 PB5 +#define LED0_PIN PB5 #if defined(PODIUMF4) -#define LED1 PB4 -#define LED2 PB6 +#define LED1_PIN PB4 +#define LED2_PIN PB6 #endif // Disable LED1, conflicts with AirbotF4/Flip32F4/Revolt beeper @@ -64,7 +64,7 @@ #define BEEPER PB6 #define BEEPER_INVERTED #else -#define LED1 PB4 +#define LED1_PIN PB4 // Leave beeper here but with none as io - so disabled unless mapped. #define BEEPER NONE #endif @@ -136,7 +136,7 @@ #define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL -#if !defined(AIRBOTF4) && !defined(REVOLT) && !defined(SOULF4) && !defined(PODIUMF4) +#if defined(AIRBOTF4) || defined(AIRBOTF4SD) #define MAG #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW90_DEG @@ -224,9 +224,17 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#if defined(AIRBOTF4) || defined(AIRBOTF4SD) +// On AIRBOTF4 and AIRBOTF4SD, I2C2 and I2C3 are configurable #define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C_DEVICE (I2CDEV_1) +#define USE_I2C_DEVICE_2 +#define I2C2_SCL NONE // PB10, shared with UART3TX +#define I2C2_SDA NONE // PB11, shared with UART3RX +#define USE_I2C_DEVICE_3 +#define I2C3_SCL NONE // PA8, PWM6 +#define I2C3_SDA NONE // PC9, CH6 +#define I2C_DEVICE (I2CDEV_2) +#endif #define USE_ADC #if !defined(PODIUMF4) diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 87a9b4008d..67c178b5b4 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -23,8 +23,8 @@ #define USBD_SERIALNUMBER_STRING "0x8010000" #endif -#define LED0 PC14 -#define LED1 PC13 +#define LED0_PIN PC14 +#define LED1_PIN PC13 #define BEEPER PC13 diff --git a/src/main/target/RG_SSD_F3/target.h b/src/main/target/RG_SSD_F3/target.h index 73ceb71ace..b83c875a10 100644 --- a/src/main/target/RG_SSD_F3/target.h +++ b/src/main/target/RG_SSD_F3/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "RGF3" // rgSSD_F3 -#define LED0 PC1 -#define LED1 PC0 +#define LED0_PIN PC1 +#define LED1_PIN PC0 #define BEEPER PA8 #define BEEPER_INVERTED diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 19f51f4d91..34fbef33fa 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #define BEEPER PC15 diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 4f8c84e623..52b68f9cbc 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -19,7 +19,7 @@ #define TARGET_BOARD_IDENTIFIER "SIRF" -#define LED0 PB2 +#define LED0_PIN PB2 #define BEEPER PA1 diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index c233dafa7c..bb0d756b00 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -392,7 +392,7 @@ pwmOutputPort_t *pwmGetMotors(void) { bool pwmAreMotorsEnabled(void) { return pwmMotorsEnabled; } -void pwmWriteMotor(uint8_t index, uint16_t value) { +void pwmWriteMotor(uint8_t index, float value) { motorsPwm[index] = value - idlePulse; } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { @@ -419,7 +419,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) { udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); // printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); } -void pwmWriteServo(uint8_t index, uint16_t value) { +void pwmWriteServo(uint8_t index, float value) { servosPwm[index] = value; } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index dc811d9246..d7d39d97b7 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -45,6 +45,8 @@ #undef SCHEDULER_DELAY_LIMIT #define SCHEDULER_DELAY_LIMIT 1 +#define USE_FAKE_LED + #define ACC #define USE_FAKE_ACC diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 0d7e768e2d..ef2fb481f0 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -21,8 +21,8 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB4 // Blue (Rev 1 & 2) - PB4 -#define LED1 PB5 // Green (Rev 1) / Red (Rev 2) - PB5 +#define LED0_PIN PB4 // Blue (Rev 1 & 2) - PB4 +#define LED1_PIN PB5 // Green (Rev 1) / Red (Rev 2) - PB5 #define BEEPER PA1 #define BEEPER_INVERTED diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index c7275f34d5..626b10dd2b 100755 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -23,9 +23,9 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -#define LED0 PB5 -#define LED1 PB4 -#define LED2 PB6 +#define LED0_PIN PB5 +#define LED1_PIN PB4 +#define LED2_PIN PB6 #define BEEPER PC9 #define BEEPER_INVERTED diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 99a0069123..baf48775ea 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -30,11 +30,11 @@ #if defined(ZCOREF3) #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0_PIN PB8 #else #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #endif #define BEEPER PC15 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index a1e84fe49a..80da46617b 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -46,7 +46,7 @@ #define BRUSHED_ESC_AUTODETECT -#define LED0 PB8 +#define LED0_PIN PB8 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 30536dc657..eefad26999 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -22,7 +22,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0_PIN PB8 #else #define TARGET_BOARD_IDENTIFIER "SRFM" @@ -32,7 +32,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0_PIN PB3 #endif #define BEEPER PC15 diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 4fa325d14c..de917c8595 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -22,8 +22,8 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB9 -#define LED1 PB2 +#define LED0_PIN PB9 +#define LED1_PIN PB2 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/SPRACINGF3OSD/target.h b/src/main/target/SPRACINGF3OSD/target.h index bd2d30a798..c532c98cf2 100644 --- a/src/main/target/SPRACINGF3OSD/target.h +++ b/src/main/target/SPRACINGF3OSD/target.h @@ -22,7 +22,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PA15 +#define LED0_PIN PA15 #define USE_EXTI diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index b404e149b1..256e1a31c9 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -26,7 +26,7 @@ #define USBD_PRODUCT_STRING "SP Racing F4 NEO" -#define LED0 PA0 +#define LED0_PIN PA0 #define BEEPER PC15 #define BEEPER_INVERTED @@ -89,6 +89,7 @@ // #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) #if (SPRACINGF4EVO_REV >= 2) #define I2C1_SCL PB8 diff --git a/src/main/target/SPRACINGF4NEO/target.h b/src/main/target/SPRACINGF4NEO/target.h index fc10fc3544..04b18fde14 100644 --- a/src/main/target/SPRACINGF4NEO/target.h +++ b/src/main/target/SPRACINGF4NEO/target.h @@ -27,16 +27,16 @@ #define USBD_PRODUCT_STRING "SP Racing F4 NEO" #if (SPRACINGF4NEO_REV >= 3) - #define LED0 PA0 - #define LED1 PB1 + #define LED0_PIN PA0 + #define LED1_PIN PB1 #endif #if (SPRACINGF4NEO_REV == 2) - #define LED0 PB9 - #define LED1 PB2 + #define LED0_PIN PB9 + #define LED1_PIN PB2 #endif #if (SPRACINGF4NEO_REV == 1) - #define LED0 PB9 - #define LED1 PB2 + #define LED0_PIN PB9 + #define LED1_PIN PB2 #endif #define BEEPER PC15 @@ -103,6 +103,7 @@ #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #if (SPRACINGF4NEO_REV >= 3) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 1bf54b7eaa..460e8c1d94 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -31,9 +31,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PE8 // Blue LEDs - PE8/PE12 +#define LED0_PIN PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1_PIN PE10 // Orange LEDs - PE10/PE14 #define LED1_INVERTED #define BEEPER PD12 diff --git a/src/main/target/TINYFISH/target.h b/src/main/target/TINYFISH/target.h index 423c459f69..dd2706bffa 100644 --- a/src/main/target/TINYFISH/target.h +++ b/src/main/target/TINYFISH/target.h @@ -22,8 +22,8 @@ #define TARGET_BOARD_IDENTIFIER "TFSH" // http://fishpepper.de/projects/tinyFISH -#define LED0 PC14 -#define LED1 PA13 +#define LED0_PIN PC14 +#define LED1_PIN PA13 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index ff34683003..5984c6accf 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -22,7 +22,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PC14 +#define LED0_PIN PC14 #define BEEPER PC15 #define BEEPER_INVERTED diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 5c101ced58..bcb65b557c 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -21,9 +21,9 @@ #define USBD_PRODUCT_STRING "YupiF4" -#define LED0 PB6 -#define LED1 PB4 -#define LED2 PB5 +#define LED0_PIN PB6 +#define LED1_PIN PB4 +#define LED2_PIN PB5 #define BEEPER PC9 #define BEEPER_PWM_HZ 2200 // Beeper PWM frequency in Hz diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index 8479caa243..84001a41dd 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -28,3 +28,14 @@ #if defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS) #undef USE_SERVOS #endif + +// XXX Followup implicit dependencies among DASHBOARD, display_xxx and USE_I2C. +// XXX This should eventually be cleaned up. +#ifndef USE_I2C +#undef USE_I2C_OLED_DISPLAY +#undef USE_DASHBOARD +#else +#ifdef USE_DASHBOARD +#define USE_I2C_OLED_DISPLAY +#endif +#endif diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 34e8878ad7..5f32767a66 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -38,7 +38,7 @@ #include "io/gps.h" #include "io/serial.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "io/gps.h" diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 39b8470c03..304271d8c9 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -35,7 +35,7 @@ #include "io/serial.h" #include "fc/config.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "msp/msp_serial.h" diff --git a/src/test/Makefile b/src/test/Makefile index 7528b6f90d..6a02179fdd 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -68,13 +68,18 @@ encoding_unittest_SRC := \ flight_failsafe_unittest_SRC := \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/flight/failsafe.c flight_imu_unittest_SRC := \ - $(USER_DIR)/flight/imu.c \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/config/feature.c \ + $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/flight/altitude.c \ - $(USER_DIR)/common/maths.c + $(USER_DIR)/flight/imu.c flight_mixer_unittest := \ @@ -93,6 +98,8 @@ io_serial_unittest_SRC := \ ledstrip_unittest_SRC := \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/io/ledstrip.c @@ -106,7 +113,11 @@ parameter_groups_unittest_SRC := \ rc_controls_unittest_SRC := \ $(USER_DIR)/fc/rc_controls.c \ - $(USER_DIR)/common/maths.c + $(USER_DIR)/config/parameter_group.c \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/fc/rc_adjustments.c \ + $(USER_DIR)/fc/rc_modes.c \ rx_crsf_unittest_SRC := \ @@ -119,12 +130,16 @@ rx_ibus_unittest_SRC := \ rx_ranges_unittest_SRC := \ - $(USER_DIR)/rx/rx.c \ - $(USER_DIR)/common/maths.c + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/rx/rx.c rx_rx_unittest_SRC := \ $(USER_DIR)/rx/rx.c \ + $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/common/bitarray.c \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/config/feature.c diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 5976c96b61..afc75d113a 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -29,14 +29,19 @@ extern "C" { #include "common/axis.h" #include "common/maths.h" + #include "common/bitarray.h" #include "fc/runtime_config.h" - - #include "io/beeper.h" + #include "fc/rc_modes.h" #include "fc/rc_controls.h" - #include "rx/rx.h" #include "flight/failsafe.h" + + #include "io/beeper.h" + + #include "rx/rx.h" + + extern boxBitmask_t rcModeActivationMask; } #include "unittest_macros.h" @@ -304,7 +309,11 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) // and throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure failsafeConfigMutable()->failsafe_kill_switch = true; // configure AUX switch as kill switch - ACTIVATE_RC_MODE(BOXFAILSAFE); // and activate it + + boxBitmask_t newMask; + bitArraySet(&newMask, BOXFAILSAFE); + rcModeUpdate(&newMask); // activate BOXFAILSAFE mode + sysTickUptime = 0; // restart time from 0 failsafeOnValidDataReceived(); // set last valid sample at current time sysTickUptime = PERIOD_RXDATA_FAILURE + 1; // adjust time to point just past the failure time to @@ -324,7 +333,8 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to failsafeOnValidDataReceived(); // cause a recovered link - rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be off (kill switch) + memset(&newMask, 0, sizeof(newMask)); + rcModeUpdate(&newMask); // BOXFAILSAFE must be off (kill switch) // when failsafeUpdateState(); @@ -404,7 +414,6 @@ extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t armingFlags; float rcCommand[4]; -uint32_t rcModeActivationMask; int16_t debug[DEBUG16_VALUE_COUNT]; bool isUsingSticksToArm = true; diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index fc626b5e0e..b9fc11a320 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -28,6 +28,7 @@ extern "C" { #include "common/axis.h" #include "common/maths.h" + #include "config/feature.h" #include "config/parameter_group_ids.h" #include "drivers/accgyro/accgyro.h" @@ -42,6 +43,7 @@ extern "C" { #include "fc/runtime_config.h" #include "fc/rc_controls.h" + #include "fc/rc_modes.h" #include "rx/rx.h" @@ -57,6 +59,12 @@ extern "C" { PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); + + PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); + + PG_RESET_TEMPLATE(featureConfig_t, featureConfig, + .enabledFeatures = 0 + ); } #include "unittest_macros.h" diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 62ffa90f43..d4d47cb1d9 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -35,6 +35,7 @@ extern "C" { #include "fc/config.h" #include "fc/rc_controls.h" + #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "drivers/light_ws2811strip.h" diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc index 4a0543bd32..4ef93af25a 100644 --- a/src/test/unit/parameter_groups_unittest.cc +++ b/src/test/unit/parameter_groups_unittest.cc @@ -48,7 +48,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, TEST(ParameterGroupsfTest, Test_pgResetAll) { memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); - pgResetAll(0); + pgResetAll(); EXPECT_EQ(1150, motorConfig()->minthrottle); EXPECT_EQ(1850, motorConfig()->maxthrottle); EXPECT_EQ(1000, motorConfig()->mincommand); @@ -59,7 +59,7 @@ TEST(ParameterGroupsfTest, Test_pgFind) { memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); const pgRegistry_t *pgRegistry = pgFind(PG_MOTOR_CONFIG); - pgResetCurrent(pgRegistry); + pgReset(pgRegistry); EXPECT_EQ(1150, motorConfig()->minthrottle); EXPECT_EQ(1850, motorConfig()->maxthrottle); EXPECT_EQ(1000, motorConfig()->mincommand); @@ -68,7 +68,7 @@ TEST(ParameterGroupsfTest, Test_pgFind) motorConfig_t motorConfig2; memset(&motorConfig2, 0, sizeof(motorConfig_t)); motorConfigMutable()->dev.motorPwmRate = 500; - pgStore(pgRegistry, &motorConfig2, sizeof(motorConfig_t), 0); + pgStore(pgRegistry, &motorConfig2, sizeof(motorConfig_t)); EXPECT_EQ(1150, motorConfig2.minthrottle); EXPECT_EQ(1850, motorConfig2.maxthrottle); EXPECT_EQ(1000, motorConfig2.mincommand); diff --git a/src/test/unit/rc_controls_unittest.cc.txt b/src/test/unit/rc_controls_unittest.cc similarity index 65% rename from src/test/unit/rc_controls_unittest.cc.txt rename to src/test/unit/rc_controls_unittest.cc index d21defb795..415f9deb1c 100644 --- a/src/test/unit/rc_controls_unittest.cc.txt +++ b/src/test/unit/rc_controls_unittest.cc @@ -26,121 +26,124 @@ extern "C" { #include "common/maths.h" #include "common/axis.h" + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "blackbox/blackbox.h" + #include "drivers/sensor.h" - #include "drivers/accgyro.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "io/beeper.h" - #include "io/escservo.h" - #include "io/rc_controls.h" #include "rx/rx.h" #include "flight/pid.h" + + #include "fc/controlrate_profile.h" + #include "fc/rc_modes.h" + #include "fc/rc_adjustments.h" + + #include "fc/rc_controls.h" } #include "unittest_macros.h" #include "gtest/gtest.h" -extern "C" { -extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); -} - class RcControlsModesTest : public ::testing::Test { protected: - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - virtual void SetUp() { - memset(&modeActivationConditions, 0, sizeof(modeActivationConditions)); } }; TEST_F(RcControlsModesTest, updateActivatedModesWithAllInputsAtMidde) { // given - rcModeActivationMask = 0; + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + rcModeUpdate(&mask); // and memset(&rxRuntimeConfig, 0, sizeof(rxRuntimeConfig_t)); - rxRuntimeConfig.auxChannelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; + rxRuntimeConfig.channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } // when - updateActivatedModes(modeActivationConditions); + updateActivatedModes(); // then - for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { + for (int index = 0; index < CHECKBOX_ITEM_COUNT; index++) { #ifdef DEBUG_RC_CONTROLS printf("iteration: %d\n", index); #endif - EXPECT_EQ(false, IS_RC_MODE_ACTIVE(index)); + EXPECT_EQ(false, IS_RC_MODE_ACTIVE((boxId_e)index)); } } TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues) { // given - modeActivationConditions[0].modeId = (boxId_e)0; - modeActivationConditions[0].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1700); - modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(2100); + modeActivationConditionsMutable(0)->modeId = (boxId_e)0; + modeActivationConditionsMutable(0)->auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1700); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); - modeActivationConditions[1].modeId = (boxId_e)1; - modeActivationConditions[1].auxChannelIndex = AUX2 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[1].range.startStep = CHANNEL_VALUE_TO_STEP(1300); - modeActivationConditions[1].range.endStep = CHANNEL_VALUE_TO_STEP(1700); + modeActivationConditionsMutable(1)->modeId = (boxId_e)1; + modeActivationConditionsMutable(1)->auxChannelIndex = AUX2 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1300); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(1700); - modeActivationConditions[2].modeId = (boxId_e)2; - modeActivationConditions[2].auxChannelIndex = AUX3 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[2].range.startStep = CHANNEL_VALUE_TO_STEP(900); - modeActivationConditions[2].range.endStep = CHANNEL_VALUE_TO_STEP(1200); + modeActivationConditionsMutable(2)->modeId = (boxId_e)2; + modeActivationConditionsMutable(2)->auxChannelIndex = AUX3 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(900); + modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1200); - modeActivationConditions[3].modeId = (boxId_e)3; - modeActivationConditions[3].auxChannelIndex = AUX4 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[3].range.startStep = CHANNEL_VALUE_TO_STEP(900); - modeActivationConditions[3].range.endStep = CHANNEL_VALUE_TO_STEP(2100); + modeActivationConditionsMutable(3)->modeId = (boxId_e)3; + modeActivationConditionsMutable(3)->auxChannelIndex = AUX4 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(3)->range.startStep = CHANNEL_VALUE_TO_STEP(900); + modeActivationConditionsMutable(3)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); - modeActivationConditions[4].modeId = (boxId_e)4; - modeActivationConditions[4].auxChannelIndex = AUX5 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[4].range.startStep = CHANNEL_VALUE_TO_STEP(900); - modeActivationConditions[4].range.endStep = CHANNEL_VALUE_TO_STEP(925); + modeActivationConditionsMutable(4)->modeId = (boxId_e)4; + modeActivationConditionsMutable(4)->auxChannelIndex = AUX5 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(4)->range.startStep = CHANNEL_VALUE_TO_STEP(900); + modeActivationConditionsMutable(4)->range.endStep = CHANNEL_VALUE_TO_STEP(925); - EXPECT_EQ(0, modeActivationConditions[4].range.startStep); - EXPECT_EQ(1, modeActivationConditions[4].range.endStep); + EXPECT_EQ(0, modeActivationConditions(4)->range.startStep); + EXPECT_EQ(1, modeActivationConditions(4)->range.endStep); - modeActivationConditions[5].modeId = (boxId_e)5; - modeActivationConditions[5].auxChannelIndex = AUX6 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[5].range.startStep = CHANNEL_VALUE_TO_STEP(2075); - modeActivationConditions[5].range.endStep = CHANNEL_VALUE_TO_STEP(2100); + modeActivationConditionsMutable(5)->modeId = (boxId_e)5; + modeActivationConditionsMutable(5)->auxChannelIndex = AUX6 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(5)->range.startStep = CHANNEL_VALUE_TO_STEP(2075); + modeActivationConditionsMutable(5)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); - EXPECT_EQ(47, modeActivationConditions[5].range.startStep); - EXPECT_EQ(48, modeActivationConditions[5].range.endStep); + EXPECT_EQ(47, modeActivationConditions(5)->range.startStep); + EXPECT_EQ(48, modeActivationConditions(5)->range.endStep); - modeActivationConditions[6].modeId = (boxId_e)6; - modeActivationConditions[6].auxChannelIndex = AUX7 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[6].range.startStep = CHANNEL_VALUE_TO_STEP(925); - modeActivationConditions[6].range.endStep = CHANNEL_VALUE_TO_STEP(950); + modeActivationConditionsMutable(6)->modeId = (boxId_e)6; + modeActivationConditionsMutable(6)->auxChannelIndex = AUX7 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(6)->range.startStep = CHANNEL_VALUE_TO_STEP(925); + modeActivationConditionsMutable(6)->range.endStep = CHANNEL_VALUE_TO_STEP(950); - EXPECT_EQ(1, modeActivationConditions[6].range.startStep); - EXPECT_EQ(2, modeActivationConditions[6].range.endStep); + EXPECT_EQ(1, modeActivationConditions(6)->range.startStep); + EXPECT_EQ(2, modeActivationConditions(6)->range.endStep); // and - rcModeActivationMask = 0; + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + rcModeUpdate(&mask); // and memset(&rxRuntimeConfig, 0, sizeof(rxRuntimeConfig_t)); - rxRuntimeConfig.auxChannelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; + rxRuntimeConfig.channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -163,33 +166,28 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV expectedMask |= (0 << 6); // when - updateActivatedModes(modeActivationConditions); + updateActivatedModes(); // then - for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { + for (int index = 0; index < CHECKBOX_ITEM_COUNT; index++) { #ifdef DEBUG_RC_CONTROLS printf("iteration: %d\n", index); #endif - EXPECT_EQ(expectedMask & (1 << index), rcModeActivationMask & (1 << index)); + EXPECT_EQ((bool)(expectedMask & (1 << index)), IS_RC_MODE_ACTIVE((boxId_e)index)); } } enum { - COUNTER_GENERATE_PITCH_ROLL_CURVE = 0, COUNTER_QUEUE_CONFIRMATION_BEEP, COUNTER_CHANGE_CONTROL_RATE_PROFILE }; -#define CALL_COUNT_ITEM_COUNT 3 +#define CALL_COUNT_ITEM_COUNT 2 static int callCounts[CALL_COUNT_ITEM_COUNT]; #define CALL_COUNTER(item) (callCounts[item]) extern "C" { -void generatePitchRollCurve(controlRateConfig_t *) { - callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++; -} - void beeperConfirmationBeeps(uint8_t) { callCounts[COUNTER_QUEUE_CONFIRMATION_BEEP]++; } @@ -223,17 +221,19 @@ void resetMillis(void) { #define DEFAULT_MIN_CHECK 1100 #define DEFAULT_MAX_CHECK 1900 -rxConfig_t rxConfig; +extern "C" { + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); + void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig); -extern uint8_t adjustmentStateMask; -extern adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; - -static const adjustmentConfig_t rateAdjustmentConfig = { - .adjustmentFunction = ADJUSTMENT_RC_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } -}; + extern uint8_t adjustmentStateMask; + extern adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; + static const adjustmentConfig_t rateAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_RC_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { 1 } + }; +} class RcControlsAdjustmentsTest : public ::testing::Test { protected: controlRateConfig_t controlRateConfig = { @@ -251,10 +251,10 @@ protected: adjustmentStateMask = 0; memset(&adjustmentStates, 0, sizeof(adjustmentStates)); - memset(&rxConfig, 0, sizeof (rxConfig)); - rxConfig.mincheck = DEFAULT_MIN_CHECK; - rxConfig.maxcheck = DEFAULT_MAX_CHECK; - rxConfig.midrc = 1500; + PG_RESET(rxConfig); + rxConfigMutable()->mincheck = DEFAULT_MIN_CHECK; + rxConfigMutable()->maxcheck = DEFAULT_MAX_CHECK; + rxConfigMutable()->midrc = 1500; controlRateConfig.rcRate8 = 90; controlRateConfig.rcExpo8 = 0; @@ -276,8 +276,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle) configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -286,11 +285,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle) resetMillis(); // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 90); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 0); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 0); EXPECT_EQ(adjustmentStateMask, 0); } @@ -310,10 +308,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp }; // and - memset(&rxConfig, 0, sizeof (rxConfig)); - rxConfig.mincheck = DEFAULT_MIN_CHECK; - rxConfig.maxcheck = DEFAULT_MAX_CHECK; - rxConfig.midrc = 1500; + PG_RESET(rxConfig); + rxConfigMutable()->mincheck = DEFAULT_MIN_CHECK; + rxConfigMutable()->maxcheck = DEFAULT_MAX_CHECK; + rxConfigMutable()->midrc = 1500; // and adjustmentStateMask = 0; @@ -321,8 +319,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -341,15 +338,13 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 496; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 91); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 1); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); - // // now pretend a short amount of time has passed, but not enough time to allow the value to have been increased // @@ -358,7 +353,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 497; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); EXPECT_EQ(controlRateConfig.rcRate8, 91); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -380,7 +375,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp ~(1 << 0); // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); EXPECT_EQ(controlRateConfig.rcRate8, 91); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -400,11 +395,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 499; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 92); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 2); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 2); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -417,7 +411,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 500; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 92); @@ -431,7 +425,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 997; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 92); @@ -447,11 +441,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 998; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 93); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 3); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); } @@ -459,7 +452,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp static const adjustmentConfig_t rateProfileAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, .mode = ADJUSTMENT_MODE_SELECT, - .data = { { 3 } } + .data = { 3 } }; TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) @@ -469,8 +462,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) configureAdjustment(adjustmentIndex, AUX4 - NON_AUX_CHANNEL_COUNT, &rateProfileAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -486,7 +478,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) (1 << adjustmentIndex); // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1); @@ -497,61 +489,54 @@ TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidYawPAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_YAW_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidYawIAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_YAW_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidYawDAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_YAW_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0) { // given - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); - - escAndServoConfig_t escAndServoConfig; - memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); - pidProfile_t pidProfile; memset(&pidProfile, 0, sizeof (pidProfile)); - pidProfile.pidController = 0; - pidProfile.P8[PIDPITCH] = 0; - pidProfile.P8[PIDROLL] = 5; - pidProfile.P8[YAW] = 7; - pidProfile.I8[PIDPITCH] = 10; - pidProfile.I8[PIDROLL] = 15; - pidProfile.I8[YAW] = 17; - pidProfile.D8[PIDPITCH] = 20; - pidProfile.D8[PIDROLL] = 25; - pidProfile.D8[YAW] = 27; - + pidProfile.pid[PID_PITCH].P = 0; + pidProfile.pid[PID_PITCH].I = 10; + pidProfile.pid[PID_PITCH].D = 20; + pidProfile.pid[PID_ROLL].P = 5; + pidProfile.pid[PID_ROLL].I = 15; + pidProfile.pid[PID_ROLL].D = 25; + pidProfile.pid[PID_YAW].P = 7; + pidProfile.pid[PID_YAW].I = 17; + pidProfile.pid[PID_YAW].D = 27; + useAdjustmentConfig(&pidProfile); // and controlRateConfig_t controlRateConfig; memset(&controlRateConfig, 0, sizeof (controlRateConfig)); @@ -564,8 +549,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0) configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -588,34 +572,30 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0) (1 << 5); // when - useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); - processRcAdjustments(&controlRateConfig, &rxConfig); + useRcControlsConfig(&pidProfile); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(1, pidProfile.P8[PIDPITCH]); - EXPECT_EQ(6, pidProfile.P8[PIDROLL]); - EXPECT_EQ(8, pidProfile.P8[YAW]); - EXPECT_EQ(11, pidProfile.I8[PIDPITCH]); - EXPECT_EQ(16, pidProfile.I8[PIDROLL]); - EXPECT_EQ(18, pidProfile.I8[YAW]); - EXPECT_EQ(21, pidProfile.D8[PIDPITCH]); - EXPECT_EQ(26, pidProfile.D8[PIDROLL]); - EXPECT_EQ(28, pidProfile.D8[YAW]); + EXPECT_EQ(1, pidProfile.pid[PID_PITCH].P); + EXPECT_EQ(11, pidProfile.pid[PID_PITCH].I); + EXPECT_EQ(21, pidProfile.pid[PID_PITCH].D); + EXPECT_EQ(6, pidProfile.pid[PID_ROLL].P); + EXPECT_EQ(16, pidProfile.pid[PID_ROLL].I); + EXPECT_EQ(26, pidProfile.pid[PID_ROLL].D); + EXPECT_EQ(8, pidProfile.pid[PID_YAW].P); + EXPECT_EQ(18, pidProfile.pid[PID_YAW].I); + EXPECT_EQ(28, pidProfile.pid[PID_YAW].D); } +#if 0 // only one PID controller + TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) { // given - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); - - escAndServoConfig_t escAndServoConfig; - memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); - pidProfile_t pidProfile; memset(&pidProfile, 0, sizeof (pidProfile)); pidProfile.pidController = 2; @@ -628,6 +608,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) pidProfile.D_f[PIDPITCH] = 20.0f; pidProfile.D_f[PIDROLL] = 25.0f; pidProfile.D_f[PIDYAW] = 27.0f; + useAdjustmentConfig(&pidProfile); // and controlRateConfig_t controlRateConfig; @@ -641,8 +622,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -665,7 +645,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) (1 << 5); // when - useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + useRcControlsConfig(&escAndServoConfig, &pidProfile); processRcAdjustments(&controlRateConfig, &rxConfig); // then @@ -685,33 +665,39 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) } +#endif + extern "C" { void saveConfigAndNotify(void) {} -void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {} -void changeProfile(uint8_t) {} +void generateThrottleCurve(void) {} +void changePidProfile(uint8_t) {} +void pidInitConfig(const pidProfile_t *) {} void accSetCalibrationCycles(uint16_t) {} -void gyroSetCalibrationCycles(uint16_t) {} +void gyroStartCalibration(void) {} void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} void handleInflightCalibrationStickPosition(void) {} bool feature(uint32_t) { return false;} bool sensors(uint32_t) { return false;} void mwArm(void) {} void mwDisarm(void) {} -void displayDisablePageCycling() {} -void displayEnablePageCycling() {} +void dashboardDisablePageCycling() {} +void dashboardEnablePageCycling() {} bool failsafeIsActive() { return false; } bool rxIsReceivingSignal() { return true; } -uint8_t getCurrentControlRateProfile(void) { +uint8_t getCurrentControlRateProfileIndex(void) { return 0; } void GPS_reset_home_position(void) {} void baroSetCalibrationCycles(uint16_t) {} +void blackboxLogEvent(FlightLogEvent, flightLogEventData_t *) {} + uint8_t armingFlags = 0; int16_t heading; uint8_t stateFlags = 0; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; +PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); } diff --git a/src/test/unit/rx_ibus_unittest.cc b/src/test/unit/rx_ibus_unittest.cc index 7f675ff95f..eb2453dc52 100644 --- a/src/test/unit/rx_ibus_unittest.cc +++ b/src/test/unit/rx_ibus_unittest.cc @@ -28,6 +28,7 @@ extern "C" { #include "telemetry/ibus_shared.h" #include "telemetry/telemetry.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "sensors/barometer.h" #include "sensors/battery.h" } diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 59a4ecf8d4..c75d26ec84 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -26,6 +26,7 @@ extern "C" { #include "common/maths.h" #include "config/parameter_group_ids.h" #include "fc/rc_controls.h" + #include "fc/rc_modes.h" #include "rx/rx.h" } @@ -38,8 +39,6 @@ extern "C" { uint32_t rcModeActivationMask; extern uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range); - -PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); } #define RANGE_CONFIGURATION(min, max) new (rxChannelRangeConfig_t) {min, max} diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index cbaa95b2d9..db4b9c6515 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -24,11 +24,13 @@ extern "C" { #include "platform.h" #include "rx/rx.h" - #include "fc/rc_controls.h" + #include "fc/rc_modes.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 "io/beeper.h" uint32_t rcModeActivationMask; @@ -42,8 +44,6 @@ extern "C" { PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = 0 ); - PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); - } #include "unittest_macros.h" diff --git a/src/test/unit/target.h b/src/test/unit/target.h index 21a6a2fce2..b9cc9faf7d 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -63,6 +63,7 @@ #define SERIAL_PORT_COUNT 8 #define DEFAULT_AUX_CHANNEL_COUNT MAX_AUX_CHANNEL_COUNT +#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 // needed for unittest #define TARGET_BOARD_IDENTIFIER "TEST"