diff --git a/Makefile b/Makefile
index b8f4be0e29..c8deb61ba9 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 \
@@ -712,6 +713,7 @@ COMMON_SRC = \
io/serial.c \
io/statusindicator.c \
io/transponder_ir.c \
+ io/rcsplit.c \
msp/msp_serial.c \
scheduler/scheduler.c \
sensors/battery.c \
@@ -738,6 +740,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 \
@@ -1020,7 +1023,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
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/common/bitarray.c b/src/main/common/bitarray.c
new file mode 100644
index 0000000000..53ca99127c
--- /dev/null
+++ b/src/main/common/bitarray.c
@@ -0,0 +1,39 @@
+/*
+ * 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 "bitarray.h"
+
+#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)
+{
+ 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, &=~);
+}
+
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/drivers/bus_spi.h b/src/main/drivers/bus_spi.h
index 7f23f7aed1..c9a1597a6c 100644
--- a/src/main/drivers/bus_spi.h
+++ b/src/main/drivers/bus_spi.h
@@ -77,6 +77,7 @@ typedef enum SPIDevice {
#define SPIDEV_COUNT 4
#else
#define SPIDEV_COUNT 4
+
#endif
void spiPreInitCs(ioTag_t iotag);
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 34467696c1..2325890609 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -31,11 +31,13 @@
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
-#define DSHOT_MAX_COMMAND 47
-
-static pwmWriteFuncPtr pwmWritePtr;
+static pwmWriteFunc *pwmWrite;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
-static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
+static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
+
+#ifdef USE_DSHOT
+loadDmaBufferFunc *loadDmaBuffer;
+#endif
#ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
@@ -47,6 +49,7 @@ static uint16_t freqBeep=0;
#endif
bool pwmMotorsEnabled = false;
+bool isDigital = false;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@@ -125,40 +128,69 @@ 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)
+#ifdef USE_DSHOT
+static void pwmWriteDigital(uint8_t index, float value)
{
- pwmWritePtr(index, value);
+ pwmWriteDigitalInt(index, lrintf(value));
+}
+
+static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet)
+{
+ for (int i = 0; i < 16; i++) {
+ motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
+ packet <<= 1;
+ }
+
+ return DSHOT_DMA_BUFFER_SIZE;
+}
+
+static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t packet)
+{
+ 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
+ }
+
+ return PROSHOT_DMA_BUFFER_SIZE;
+}
+#endif
+
+void pwmWriteMotor(uint8_t index, float value)
+{
+ if (pwmMotorsEnabled) {
+ pwmWrite(index, value);
+ }
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
@@ -180,7 +212,7 @@ void pwmDisableMotors(void)
void pwmEnableMotors(void)
{
/* check motors can be enabled */
- pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused);
+ pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
}
bool pwmAreMotorsEnabled(void)
@@ -207,7 +239,7 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
void pwmCompleteMotorUpdate(uint8_t motorCount)
{
- pwmCompleteWritePtr(motorCount);
+ pwmCompleteWrite(motorCount);
}
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
@@ -216,48 +248,54 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
uint32_t timerMhzCounter = 0;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
- bool isDigital = false;
switch (motorConfig->motorPwmProtocol) {
default:
case PWM_TYPE_ONESHOT125:
timerMhzCounter = ONESHOT125_TIMER_MHZ;
- pwmWritePtr = pwmWriteOneShot125;
+ pwmWrite = &pwmWriteOneShot125;
break;
case PWM_TYPE_ONESHOT42:
timerMhzCounter = ONESHOT42_TIMER_MHZ;
- pwmWritePtr = pwmWriteOneShot42;
+ pwmWrite = &pwmWriteOneShot42;
break;
case PWM_TYPE_MULTISHOT:
timerMhzCounter = MULTISHOT_TIMER_MHZ;
- pwmWritePtr = pwmWriteMultiShot;
+ pwmWrite = &pwmWriteMultiShot;
break;
case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
- pwmWritePtr = pwmWriteBrushed;
+ pwmWrite = &pwmWriteBrushed;
useUnsyncedPwm = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
timerMhzCounter = PWM_TIMER_MHZ;
- pwmWritePtr = pwmWriteStandard;
+ pwmWrite = &pwmWriteStandard;
useUnsyncedPwm = true;
idlePulse = 0;
break;
#ifdef USE_DSHOT
+ case PWM_TYPE_PROSHOT1000:
+ pwmWrite = &pwmWriteDigital;
+ loadDmaBuffer = &loadDmaBufferProshot;
+ pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
+ isDigital = true;
+ break;
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
- pwmWritePtr = pwmWriteDigital;
- pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
+ pwmWrite = &pwmWriteDigital;
+ loadDmaBuffer = &loadDmaBufferDshot;
+ pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
#endif
}
if (!isDigital) {
- pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate;
+ pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
@@ -266,8 +304,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
- pwmWritePtr = pwmWriteUnused;
- pwmCompleteWritePtr = pwmCompleteWriteUnused;
+ pwmWrite = &pwmWriteUnused;
+ pwmCompleteWrite = &pwmCompleteWriteUnused;
/* TODO: block arming and add reason system cannot arm */
return;
}
@@ -320,45 +358,76 @@ 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;
}
}
void pwmWriteDshotCommand(uint8_t index, uint8_t command)
{
- if (command <= DSHOT_MAX_COMMAND) {
+ if (isDigital && (command <= DSHOT_MAX_COMMAND)) {
motorDmaOutput_t *const motor = getMotorDmaOutput(index);
unsigned repeats;
- if ((DSHOT_CMD_SPIN_ONE_WAY >= 7 && DSHOT_CMD_3D_MODE_ON <= 10) || DSHOT_CMD_SAVE_SETTINGS == 12 || (DSHOT_CMD_ROTATE_NORMAL >= 20 && DSHOT_CMD_ROTATE_REVERSE <= 21) ) {
+ 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);
+ pwmWriteDigitalInt(index, command);
pwmCompleteMotorUpdate(0);
delay(1);
}
}
}
+
+uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)
+{
+ uint16_t packet = (value << 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;
+
+ return packet;
+}
#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 1a23e493ff..16d7f37c36 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -28,6 +28,8 @@
#define MAX_SUPPORTED_SERVOS 8
#endif
+#define DSHOT_MAX_COMMAND 47
+
typedef enum {
DSHOT_CMD_MOTOR_STOP = 0,
DSHOT_CMD_BEEP1,
@@ -36,14 +38,14 @@ typedef enum {
DSHOT_CMD_BEEP4,
DSHOT_CMD_BEEP5,
DSHOT_CMD_ESC_INFO,
- DSHOT_CMD_SPIN_ONE_WAY,
- DSHOT_CMD_SPIN_OTHER_WAY,
+ 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_ROTATE_NORMAL = 20, //Blheli_S only command
- DSHOT_CMD_ROTATE_REVERSE = 21, //Blheli_S only command
+ 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;
@@ -59,6 +61,7 @@ typedef enum {
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT600,
PWM_TYPE_DSHOT1200,
+ PWM_TYPE_PROSHOT1000,
#endif
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
@@ -76,6 +79,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
@@ -95,7 +103,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;
@@ -109,9 +118,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;
@@ -124,8 +133,8 @@ 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(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointer used after motors are written
+typedef void pwmWriteFunc(uint8_t index, float value); // function pointer used to write motors
+typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer used after motors are written
typedef struct {
volatile timCCR_t *ccr;
@@ -158,9 +167,15 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
#ifdef USE_DSHOT
+typedef uint8_t loadDmaBufferFunc(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
+
+uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value);
+
+extern loadDmaBufferFunc *loadDmaBuffer;
+
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDshotCommand(uint8_t index, uint8_t command);
-void pwmWriteDigital(uint8_t index, uint16_t value);
+void pwmWriteDigitalInt(uint8_t index, uint16_t value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
#endif
@@ -171,11 +186,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..2c97bc0ef4 100644
--- a/src/main/drivers/pwm_output_dshot.c
+++ b/src/main/drivers/pwm_output_dshot.c
@@ -54,39 +54,19 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount-1;
}
-void pwmWriteDigital(uint8_t index, uint16_t value)
+void pwmWriteDigitalInt(uint8_t index, uint16_t value)
{
-
- if (!pwmMotorsEnabled) {
- return;
- }
-
- motorDmaOutput_t * const motor = &dmaMotors[index];
+ motorDmaOutput_t *const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
- uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
- motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
+ uint16_t packet = prepareDshotPacket(motor, value);
- // 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 whole packet
- for (int i = 0; i < 16; i++) {
- motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
- packet <<= 1;
- }
+ uint8_t bufferSize = loadDmaBuffer(motor, packet);
- DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, MOTOR_DMA_BUFFER_SIZE);
+ DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize);
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
}
@@ -139,7 +119,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 +185,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..de270044bc 100644
--- a/src/main/drivers/pwm_output_dshot_hal.c
+++ b/src/main/drivers/pwm_output_dshot_hal.c
@@ -49,44 +49,25 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount - 1;
}
-void pwmWriteDigital(uint8_t index, uint16_t value)
+void pwmWriteDigitalInt(uint8_t index, uint16_t value)
{
- if (!pwmMotorsEnabled) {
- return;
- }
-
- motorDmaOutput_t * const motor = &dmaMotors[index];
+ motorDmaOutput_t *const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
- uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
- motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
+ uint16_t packet = prepareDshotPacket(motor, value);
- // 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 whole packet
- for (int i = 0; i < 16; i++) {
- motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
- packet <<= 1;
- }
+ uint8_t bufferSize = loadDmaBuffer(motor, packet);
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, bufferSize) != 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, bufferSize) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
@@ -122,8 +103,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/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 48a5897174..3336b2afae 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"
@@ -125,10 +126,19 @@ 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
+#else
+// Space required to set array parameters
+#define CLI_IN_BUFFER_SIZE 256
+#endif
+#define CLI_OUT_BUFFER_SIZE 64
+
+static bufWriter_t *cliWriter;
+static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_OUT_BUFFER_SIZE];
+
+static char cliBuffer[CLI_IN_BUFFER_SIZE];
static uint32_t bufferIndex = 0;
static bool configIsInCopy = false;
@@ -294,33 +304,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;
}
}
@@ -378,14 +399,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();
}
}
@@ -425,26 +447,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;
}
}
@@ -2508,18 +2535,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();
@@ -2527,52 +2570,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 {
@@ -2756,6 +2828,7 @@ const cliResourceValue_t resourceTable[] = {
{ 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 },
#ifdef USE_SPI
{ OWNER_SPI_SCK, PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagSck[0]), SPIDEV_COUNT },
{ OWNER_SPI_MISO, PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagMiso[0]), SPIDEV_COUNT },
@@ -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/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/fc_core.c b/src/main/fc/fc_core.c
index 5685bf7bb7..9f75443241 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 bool reverseMotors = false;
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;
@@ -207,16 +207,17 @@ void mwArm(void)
}
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_ROTATE_NORMAL);
+ 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_ROTATE_REVERSE);
+ pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
}
}
#endif
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 35becb829e..94194bf046 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -124,6 +124,7 @@
#include "flight/pid.h"
#include "flight/servos.h"
+#include "io/rcsplit.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
@@ -278,7 +279,9 @@ void init(void)
targetPreInit();
#endif
+#if !defined(UNIT_TEST) && !defined(USE_FAKE_LED)
ledInit(statusLedConfig());
+#endif
LED2_ON;
#ifdef USE_EXTI
@@ -694,5 +697,10 @@ void init(void)
#else
fcTasksInit();
#endif
+
+#ifdef USE_RCSPLIT
+ rcSplitInit();
+#endif // USE_RCSPLIT
+
systemState |= SYSTEM_STATE_READY;
}
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 9998228362..ce8a74520d 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 },
@@ -149,12 +153,14 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
+ { BOXCAMERA1, "CAMERA CONTROL 1", 32},
+ { BOXCAMERA2, "CAMERA CONTROL 2", 33},
+ { BOXCAMERA3, "CAMERA CONTROL 3", 34 },
};
// 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 +282,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 +324,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,6 +396,7 @@ void initActiveBoxIds(void)
BME(BOXFPVANGLEMIX);
+ //TODO: Split this into BOX3DDISABLESWITCH and BOXDSHOTREVERSE
BME(BOX3DDISABLESWITCH);
if (feature(FEATURE_SERVO_TILT)) {
@@ -396,65 +423,77 @@ void initActiveBoxIds(void)
}
#endif
+#ifdef USE_RCSPLIT
+ BME(BOXCAMERA1);
+ BME(BOXCAMERA2);
+ BME(BOXCAMERA3);
+#endif
+
#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)
@@ -829,20 +868,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
@@ -854,7 +879,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:
@@ -871,32 +901,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:
@@ -1058,14 +1091,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);
@@ -1332,13 +1357,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);
@@ -1358,6 +1383,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
@@ -1495,7 +1543,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;
}
@@ -1657,7 +1705,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
}*/
validateAndFixGyroConfig();
- if (sbufBytesRemaining(src)) {
+ if (sbufBytesRemaining(src)) {
motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
}
break;
@@ -1760,13 +1808,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);
@@ -2176,6 +2224,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 f472787727..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"
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 4bfd4ec84e..0131892928 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -84,6 +84,7 @@
#include "telemetry/telemetry.h"
#include "io/osd_slave.h"
+#include "io/rcsplit.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
@@ -594,5 +595,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
+
+#ifdef USE_RCSPLIT
+ [TASK_RCSPLIT] = {
+ .taskName = "RCSPLIT",
+ .taskFunc = rcSplitProcess,
+ .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms
+ .staticPriority = TASK_PRIORITY_MEDIUM,
+ },
+#endif
#endif
};
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..17c35f7e22
--- /dev/null
+++ b/src/main/fc/rc_modes.h
@@ -0,0 +1,108 @@
+/*
+ * 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,
+ BOXCAMERA1,
+ BOXCAMERA2,
+ BOXCAMERA3,
+ 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 de7c331f70..fa3382475a 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -40,6 +40,7 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/fc_core.h"
@@ -52,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
@@ -123,8 +114,8 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR
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()
@@ -348,6 +339,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
switch(motorConfig()->dev.motorPwmProtocol) {
+ case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
@@ -361,17 +353,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 +511,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;
@@ -617,7 +610,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) {
@@ -651,7 +644,7 @@ void mixTable(pidProfile_t *pidProfile)
}
}
-uint16_t convertExternalToMotor(uint16_t externalValue)
+float convertExternalToMotor(uint16_t externalValue)
{
uint16_t motorValue = externalValue;
#ifdef USE_DSHOT
@@ -670,12 +663,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/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..c06a5202a3 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,19 +644,7 @@ 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
- else if (sensors(SENSOR_ACC))
-#endif
- {
+ if (sensors(SENSOR_ACC)) {
osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
}
@@ -654,7 +665,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);
@@ -667,12 +678,7 @@ void osdDrawElements(void)
osdDrawSingleElement(OSD_COMPASS_BAR);
#ifdef GPS
-#ifdef CMS
- if (sensors(SENSOR_GPS) || displayIsGrabbed(osdDisplayPort))
-#else
- if (sensors(SENSOR_GPS))
-#endif
- {
+ if (sensors(SENSOR_GPS)) {
osdDrawSingleElement(OSD_GPS_SATS);
osdDrawSingleElement(OSD_GPS_SPEED);
osdDrawSingleElement(OSD_GPS_LAT);
@@ -681,6 +687,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 +719,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 +734,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 +817,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 +854,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 +1079,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 +1104,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 +1134,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/rcsplit.c b/src/main/io/rcsplit.c
new file mode 100644
index 0000000000..e9533bed6d
--- /dev/null
+++ b/src/main/io/rcsplit.c
@@ -0,0 +1,163 @@
+/*
+ * 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 "common/utils.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/rc_controls.h"
+
+#include "io/beeper.h"
+#include "io/serial.h"
+
+#include "scheduler/scheduler.h"
+
+#include "drivers/serial.h"
+
+#include "io/rcsplit.h"
+
+// communicate with camera device variables
+serialPort_t *rcSplitSerialPort = NULL;
+rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
+rcsplit_state_e cameraState = RCSPLIT_STATE_UNKNOWN;
+
+static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
+{
+ uint8_t i;
+ uint8_t crc=0x00;
+ while (len--) {
+ crc ^= *ptr++;
+ for (i=8; i>0; --i) {
+ if (crc & 0x80)
+ crc = (crc << 1) ^ 0x31;
+ else
+ crc = (crc << 1);
+ }
+ }
+ return (crc);
+}
+
+static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
+{
+ if (!rcSplitSerialPort)
+ return ;
+
+ uint8_t uart_buffer[5] = {0};
+ uint8_t crc = 0;
+
+ uart_buffer[0] = RCSPLIT_PACKET_HEADER;
+ uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL;
+ uart_buffer[2] = argument;
+ uart_buffer[3] = RCSPLIT_PACKET_TAIL;
+ crc = crc_high_first(uart_buffer, 4);
+
+ // build up a full request [header]+[command]+[argument]+[crc]+[tail]
+ uart_buffer[3] = crc;
+ uart_buffer[4] = RCSPLIT_PACKET_TAIL;
+
+ // write to device
+ serialWriteBuf(rcSplitSerialPort, uart_buffer, 5);
+}
+
+static void rcSplitProcessMode()
+{
+ // if the device not ready, do not handle any mode change event
+ if (RCSPLIT_STATE_IS_READY != cameraState)
+ return ;
+
+ for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
+ uint8_t switchIndex = i - BOXCAMERA1;
+ if (IS_RC_MODE_ACTIVE(i)) {
+ // check last state of this mode, if it's true, then ignore it.
+ // Here is a logic to make a toggle control for this mode
+ if (switchStates[switchIndex].isActivated) {
+ continue;
+ }
+
+ uint8_t argument = RCSPLIT_CTRL_ARGU_INVALID;
+ switch (i) {
+ case BOXCAMERA1:
+ argument = RCSPLIT_CTRL_ARGU_WIFI_BTN;
+ break;
+ case BOXCAMERA2:
+ argument = RCSPLIT_CTRL_ARGU_POWER_BTN;
+ break;
+ case BOXCAMERA3:
+ argument = RCSPLIT_CTRL_ARGU_CHANGE_MODE;
+ break;
+ default:
+ argument = RCSPLIT_CTRL_ARGU_INVALID;
+ break;
+ }
+
+ if (argument != RCSPLIT_CTRL_ARGU_INVALID) {
+ sendCtrlCommand(argument);
+ switchStates[switchIndex].isActivated = true;
+ }
+ } else {
+ switchStates[switchIndex].isActivated = false;
+ }
+ }
+}
+
+bool rcSplitInit(void)
+{
+ // found the port config with FUNCTION_RUNCAM_SPLIT_CONTROL
+ // User must set some UART inteface with RunCam Split at peripherals column in Ports tab
+ serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RCSPLIT);
+ if (portConfig) {
+ rcSplitSerialPort = openSerialPort(portConfig->identifier, FUNCTION_RCSPLIT, NULL, 115200, MODE_RXTX, 0);
+ }
+
+ if (!rcSplitSerialPort) {
+ return false;
+ }
+
+ // set init value to true, to avoid the action auto run when the flight board start and the switch is on.
+ for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
+ uint8_t switchIndex = i - BOXCAMERA1;
+ switchStates[switchIndex].boxId = 1 << i;
+ switchStates[switchIndex].isActivated = true;
+ }
+
+ cameraState = RCSPLIT_STATE_IS_READY;
+
+#ifdef USE_RCSPLIT
+ setTaskEnabled(TASK_RCSPLIT, true);
+#endif
+
+ return true;
+}
+
+void rcSplitProcess(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+ if (rcSplitSerialPort == NULL)
+ return ;
+
+ // process rcsplit custom mode if has any changed
+ rcSplitProcessMode();
+}
diff --git a/src/main/io/rcsplit.h b/src/main/io/rcsplit.h
new file mode 100644
index 0000000000..6900700eee
--- /dev/null
+++ b/src/main/io/rcsplit.h
@@ -0,0 +1,56 @@
+/*
+ * 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 "common/time.h"
+#include "fc/fc_msp.h"
+
+typedef struct {
+ uint8_t boxId;
+ bool isActivated;
+} rcsplit_switch_state_t;
+
+typedef enum {
+ RCSPLIT_STATE_UNKNOWN = 0,
+ RCSPLIT_STATE_INITIALIZING,
+ RCSPLIT_STATE_IS_READY,
+} rcsplit_state_e;
+
+// packet header and tail
+#define RCSPLIT_PACKET_HEADER 0x55
+#define RCSPLIT_PACKET_CMD_CTRL 0x01
+#define RCSPLIT_PACKET_TAIL 0xaa
+
+
+// the commands of RunCam Split serial protocol
+typedef enum {
+ RCSPLIT_CTRL_ARGU_INVALID = 0x0,
+ RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1,
+ RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2,
+ RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3,
+ RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF,
+} rcsplit_ctrl_argument_e;
+
+bool rcSplitInit(void);
+void rcSplitProcess(timeUs_t currentTimeUs);
+
+// only for unit test
+extern rcsplit_state_e cameraState;
+extern serialPort_t *rcSplitSerialPort;
+extern rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index e7fb4d4c63..5d0009d1b3 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -44,6 +44,7 @@ typedef enum {
FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048
FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096
FUNCTION_VTX_TRAMP = (1 << 13), // 8192
+ FUNCTION_RCSPLIT = (1 << 14), // 16384
} serialPortFunction_e;
typedef enum {
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/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/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 7f819b6df6..f943d950cb 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -111,6 +111,10 @@ typedef enum {
TASK_VTXCTRL,
#endif
+#ifdef USE_RCSPLIT
+ TASK_RCSPLIT,
+#endif
+
/* Count of real tasks */
TASK_COUNT,
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 ef8d085ee5..f0ce648bc9 100644
--- a/src/main/target/ALIENFLIGHTF3/target.h
+++ b/src/main/target/ALIENFLIGHTF3/target.h
@@ -28,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 116a7e930d..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
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/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.c b/src/main/target/CLRACINGF7/target.c
index 1ff2591a00..710a85c900 100644
--- a/src/main/target/CLRACINGF7/target.c
+++ b/src/main/target/CLRACINGF7/target.c
@@ -30,14 +30,14 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0), // PPM -DMA1_ST7
- DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM1 - DMA1_ST6
- DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM2 - DMA2_ST2
- DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM3 - DMA1_ST1
- DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM4 - DMA1_ST2
- DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 2), // PWM5 - DMA2_ST3
- DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 - DMA1_ST0
+ DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM1 - DMA1_ST6 D(1, 7, 3),D(1, 6, 3)
+ DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM2 - DMA2_ST2 D(2, 4, 7),D(2, 2, 0)
+ DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM3 - DMA1_ST1 D(1, 1, 3)
+ DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM4 - DMA1_ST2 D(1, 2, 5)
+ DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 2), // PWM5 - DMA2_ST3 D(2, 6, 0),D(2, 1, 6),D(2, 3, 6)
+ DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 - DMA1_ST0 D(1, 0, 2)
- DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, 0), // S5_OUT - DMA2_ST6
+ DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, 0), // S5_OUT - DMA2_ST6 D(2, 6, 0),D(2, 6, 6)
};
diff --git a/src/main/target/CLRACINGF7/target.h b/src/main/target/CLRACINGF7/target.h
index b9b781f84a..09f6c9ec7b 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
@@ -42,11 +42,28 @@
#define USE_ACC_SPI_MPU6000
#define GYRO
#define USE_GYRO_SPI_MPU6000
+
#define GYRO_MPU6000_ALIGN CW0_DEG
#define ACC_MPU6000_ALIGN CW0_DEG
#define MPU6000_CS_PIN PA4
#define MPU6000_SPI_INSTANCE SPI1
+// ICM-20602
+#define USE_ACC_MPU6500
+#define USE_ACC_SPI_MPU6500
+#define USE_GYRO_MPU6500
+#define USE_GYRO_SPI_MPU6500
+
+#define ACC_MPU6500_ALIGN CW0_DEG
+#define GYRO_MPU6500_ALIGN CW0_DEG
+#define MPU6500_CS_PIN SPI1_NSS_PIN
+#define MPU6500_SPI_INSTANCE SPI1
+
+// MPU interrupts
+#define USE_EXTI
+#define MPU_INT_EXTI PC4
+#define USE_MPU_DATA_READY_SIGNAL
+
#define OSD
#define USE_MAX7456
#define MAX7456_SPI_INSTANCE SPI3
@@ -64,8 +81,8 @@
#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz
#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4
-#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5
-#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2
+#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_4
+#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1
#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0
#define USE_VCP
@@ -116,7 +133,7 @@
#define CURRENT_METER_ADC_PIN PC1
#define VBAT_ADC_PIN PC2
#define RSSI_ADC_PIN PC3
-#define CURRENT_METER_SCALE_DEFAULT 250 // 3/120A = 25mv/A
+#define CURRENT_METER_SCALE_DEFAULT 250 // 3.3/120A = 25mv/A
// LED strip configuration.
#define LED_STRIP
diff --git a/src/main/target/CLRACINGF7/target.mk b/src/main/target/CLRACINGF7/target.mk
index 719339dc72..ab85a4ba98 100644
--- a/src/main/target/CLRACINGF7/target.mk
+++ b/src/main/target/CLRACINGF7/target.mk
@@ -1,8 +1,12 @@
F7X2RE_TARGETS += $(TARGET)
FEATURES += SDCARD VCP
TARGET_SRC = \
+ drivers/accgyro/accgyro_mpu.c \
drivers/accgyro/accgyro_spi_icm20689.c\
+ drivers/accgyro/accgyro_mpu6500.c \
+ drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_hal.c \
drivers/max7456.c
+
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.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..7369bf4f62 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
@@ -41,13 +41,7 @@
#define MPU_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
-//#define MAG
-//#define USE_MAG_HMC5883
-//#define MAG_HMC5883_ALIGN CW90_DEG
-
#define BARO
-//#define USE_BARO_MS5611
-
#define USE_BARO_BMP280
#define USE_BARO_SPI_BMP280
#define BMP280_SPI_INSTANCE SPI3
diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h
index 464d7e2cc5..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
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/target.h b/src/main/target/KROOZX/target.h
index 8d8e4e291a..d8fa00132b 100755
--- a/src/main/target/KROOZX/target.h
+++ b/src/main/target/KROOZX/target.h
@@ -27,8 +27,8 @@
#define TARGET_CONFIG
#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 2fcc9369f8..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
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/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 cb712f652b..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
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index f7e66c3805..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
diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h
index 12fabe9529..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
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 283912fefc..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
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 33061f23b8..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
diff --git a/src/main/target/SPRACINGF4NEO/target.h b/src/main/target/SPRACINGF4NEO/target.h
index 5007562183..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
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.c b/src/main/target/YUPIF4/target.c
index 8f1ab8b62e..8f7f49277d 100644
--- a/src/main/target/YUPIF4/target.c
+++ b/src/main/target/YUPIF4/target.c
@@ -25,13 +25,13 @@
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
- DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN
+ DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S1_OUT - DMA1_ST2
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S2_OUT - DMA1_ST4
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S3_OUT - DMA1_ST1
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1 ), // S4_OUT - DMA1_ST6
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0 ), // S5_OUT - DMA1_ST7
- DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST2
- DEF_TIM(TIM8, CH4, PC9, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM
+ DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST2
+ DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM
};
diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h
index 5c101ced58..9ab590d7c4 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
@@ -149,4 +149,4 @@
#define TARGET_IO_PORTD (BIT(2))
#define USABLE_TIMER_CHANNEL_COUNT 8
-#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8))
+#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8))
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 9db23d8599..534e788507 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -130,3 +130,5 @@
#define USE_UNCOMMON_MIXERS
#endif
+
+#define USE_RCSPLIT
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..c97ed18723 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
@@ -170,6 +185,15 @@ type_conversion_unittest_SRC := \
ws2811_unittest_SRC := \
$(USER_DIR)/drivers/light_ws2811strip.c
+rcsplit_unittest_SRC := \
+ $(USER_DIR)/common/bitarray.c \
+ $(USER_DIR)/fc/rc_modes.c \
+ $(USER_DIR)/io/rcsplit.c
+
+rcsplit_unitest_DEFINES := \
+ USE_UART3 \
+ USE_RCSPLIT \
+
# Please tweak the following variable definitions as needed by your
# project, except GTEST_HEADERS, which you can use in your own targets
# but shouldn't modify.
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 63%
rename from src/test/unit/rc_controls_unittest.cc.txt
rename to src/test/unit/rc_controls_unittest.cc
index d21defb795..5cf1c57e6b 100644
--- a/src/test/unit/rc_controls_unittest.cc.txt
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -25,122 +25,126 @@ extern "C" {
#include "common/maths.h"
#include "common/axis.h"
+ #include "common/bitarray.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;
}
@@ -153,43 +157,38 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV
rcData[AUX7] = 950; // value equal to range step upper boundary should not activate the mode
// and
- uint32_t expectedMask = 0;
- expectedMask |= (1 << 0);
- expectedMask |= (1 << 1);
- expectedMask |= (1 << 2);
- expectedMask |= (1 << 3);
- expectedMask |= (1 << 4);
- expectedMask |= (1 << 5);
- expectedMask |= (0 << 6);
+ boxBitmask_t activeBoxIds;
+ memset(&activeBoxIds, 0, sizeof(boxBitmask_t));
+ bitArraySet(&activeBoxIds, 0);
+ bitArraySet(&activeBoxIds, 1);
+ bitArraySet(&activeBoxIds, 2);
+ bitArraySet(&activeBoxIds, 3);
+ bitArraySet(&activeBoxIds, 4);
+ bitArraySet(&activeBoxIds, 5);
// 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);
+ printf("iteration: %d, %d\n", index, (bool)(bitArrayGet(&activeBoxIds, index)));
#endif
- EXPECT_EQ(expectedMask & (1 << index), rcModeActivationMask & (1 << index));
+ EXPECT_EQ((bool)(bitArrayGet(&activeBoxIds, 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 +222,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 +252,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 +277,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 +286,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 +309,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 +320,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 +339,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 +354,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
fixedMillis = 497;
// when
- processRcAdjustments(&controlRateConfig, &rxConfig);
+ processRcAdjustments(&controlRateConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@@ -380,7 +376,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
~(1 << 0);
// when
- processRcAdjustments(&controlRateConfig, &rxConfig);
+ processRcAdjustments(&controlRateConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@@ -400,11 +396,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 +412,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
fixedMillis = 500;
// when
- processRcAdjustments(&controlRateConfig, &rxConfig);
+ processRcAdjustments(&controlRateConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
@@ -431,7 +426,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
fixedMillis = 997;
// when
- processRcAdjustments(&controlRateConfig, &rxConfig);
+ processRcAdjustments(&controlRateConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
@@ -447,11 +442,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 +453,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 +463,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 +479,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 +490,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 +550,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 +573,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 +609,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 +623,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 +646,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2)
(1 << 5);
// when
- useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
+ useRcControlsConfig(&escAndServoConfig, &pidProfile);
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
@@ -685,33 +666,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/rcsplit_unittest.cc b/src/test/unit/rcsplit_unittest.cc
new file mode 100644
index 0000000000..67be33ae78
--- /dev/null
+++ b/src/test/unit/rcsplit_unittest.cc
@@ -0,0 +1,431 @@
+/*
+ * 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 "gtest/gtest.h"
+
+extern "C" {
+ #include
+ #include
+ #include
+
+ #include "platform.h"
+
+ #include "common/utils.h"
+ #include "common/maths.h"
+ #include "common/bitarray.h"
+
+ #include "config/parameter_group.h"
+ #include "config/parameter_group_ids.h"
+
+ #include "fc/rc_controls.h"
+ #include "fc/rc_modes.h"
+
+
+ #include "io/beeper.h"
+ #include "io/serial.h"
+
+ #include "scheduler/scheduler.h"
+ #include "drivers/serial.h"
+ #include "io/rcsplit.h"
+
+ #include "rx/rx.h"
+
+ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
+
+ rcsplit_state_e unitTestRCsplitState()
+ {
+ return cameraState;
+ }
+
+ bool unitTestIsSwitchActivited(boxId_e boxId)
+ {
+ uint8_t adjustBoxID = boxId - BOXCAMERA1;
+ rcsplit_switch_state_t switchState = switchStates[adjustBoxID];
+ return switchState.isActivated;
+ }
+
+ void unitTestResetRCSplit()
+ {
+ rcSplitSerialPort = NULL;
+ cameraState = RCSPLIT_STATE_UNKNOWN;
+ }
+}
+
+typedef struct testData_s {
+ bool isRunCamSplitPortConfigurated;
+ bool isRunCamSplitOpenPortSupported;
+ int8_t maxTimesOfRespDataAvailable;
+ bool isAllowBufferReadWrite;
+} testData_t;
+
+static testData_t testData;
+
+TEST(RCSplitTest, TestRCSplitInitWithoutPortConfigurated)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ bool result = rcSplitInit();
+ EXPECT_EQ(false, result);
+ EXPECT_EQ(RCSPLIT_STATE_UNKNOWN, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestRCSplitInitWithoutOpenPortConfigurated)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = false;
+ testData.isRunCamSplitPortConfigurated = true;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(false, result);
+ EXPECT_EQ(RCSPLIT_STATE_UNKNOWN, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestRCSplitInit)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestRecvWhoAreYouResponse)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // here will generate a number in [6-255], it's make the serialRxBytesWaiting() and serialRead() run at least 5 times,
+ // so the "who are you response" will full received, and cause the state change to RCSPLIT_STATE_IS_READY;
+ int8_t randNum = rand() % 127 + 6;
+ testData.maxTimesOfRespDataAvailable = randNum;
+ rcSplitProcess((timeUs_t)0);
+
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestWifiModeChangeWithDeviceUnready)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+ testData.maxTimesOfRespDataAvailable = 0;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // bind aux1, aux2, aux3 channel to wifi button, power button and change mode
+ for (uint8_t i = 0; i <= (BOXCAMERA3 - BOXCAMERA1); i++) {
+ memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
+ }
+
+ // bind aux1 to wifi button with range [900,1600]
+ modeActivationConditionsMutable(0)->auxChannelIndex = 0;
+ modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
+ modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
+ modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // bind aux2 to power button with range [1900, 2100]
+ modeActivationConditionsMutable(1)->auxChannelIndex = 1;
+ modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
+ modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // bind aux3 to change mode with range [1300, 1600]
+ modeActivationConditionsMutable(2)->auxChannelIndex = 2;
+ modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
+ modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1300);
+ modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // make the binded mode inactive
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1800;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900;
+
+ updateActivatedModes();
+
+ // runn process loop
+ rcSplitProcess(0);
+
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+}
+
+TEST(RCSplitTest, TestWifiModeChangeWithDeviceReady)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+ testData.maxTimesOfRespDataAvailable = 0;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // bind aux1, aux2, aux3 channel to wifi button, power button and change mode
+ for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
+ memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
+ }
+
+
+ // bind aux1 to wifi button with range [900,1600]
+ modeActivationConditionsMutable(0)->auxChannelIndex = 0;
+ modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
+ modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
+ modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // bind aux2 to power button with range [1900, 2100]
+ modeActivationConditionsMutable(1)->auxChannelIndex = 1;
+ modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
+ modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // bind aux3 to change mode with range [1300, 1600]
+ modeActivationConditionsMutable(2)->auxChannelIndex = 2;
+ modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
+ modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+
+ updateActivatedModes();
+
+ // runn process loop
+ int8_t randNum = rand() % 127 + 6;
+ testData.maxTimesOfRespDataAvailable = randNum;
+ rcSplitProcess((timeUs_t)0);
+
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+}
+
+TEST(RCSplitTest, TestWifiModeChangeCombine)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+ testData.maxTimesOfRespDataAvailable = 0;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // bind aux1, aux2, aux3 channel to wifi button, power button and change mode
+ for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
+ memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
+ }
+
+
+ // bind aux1 to wifi button with range [900,1600]
+ modeActivationConditionsMutable(0)->auxChannelIndex = 0;
+ modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
+ modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
+ modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // bind aux2 to power button with range [1900, 2100]
+ modeActivationConditionsMutable(1)->auxChannelIndex = 1;
+ modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
+ modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // bind aux3 to change mode with range [1300, 1600]
+ modeActivationConditionsMutable(2)->auxChannelIndex = 2;
+ modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
+ modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // // make the binded mode inactive
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+ updateActivatedModes();
+
+ // runn process loop
+ int8_t randNum = rand() % 127 + 6;
+ testData.maxTimesOfRespDataAvailable = randNum;
+ rcSplitProcess((timeUs_t)0);
+
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+
+
+ // // make the binded mode inactive
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1500;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1300;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1900;
+ updateActivatedModes();
+ rcSplitProcess((timeUs_t)0);
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA3));
+
+
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1899;
+ updateActivatedModes();
+ rcSplitProcess((timeUs_t)0);
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2001;
+ updateActivatedModes();
+ rcSplitProcess((timeUs_t)0);
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+}
+
+extern "C" {
+ serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options)
+ {
+ UNUSED(identifier);
+ UNUSED(functionMask);
+ UNUSED(baudRate);
+ UNUSED(callback);
+ UNUSED(mode);
+ UNUSED(options);
+
+ if (testData.isRunCamSplitOpenPortSupported) {
+ static serialPort_t s;
+ s.vTable = NULL;
+
+ // common serial initialisation code should move to serialPort::init()
+ s.rxBufferHead = s.rxBufferTail = 0;
+ s.txBufferHead = s.txBufferTail = 0;
+ s.rxBufferSize = 0;
+ s.txBufferSize = 0;
+ s.rxBuffer = s.rxBuffer;
+ s.txBuffer = s.txBuffer;
+
+ // callback works for IRQ-based RX ONLY
+ s.rxCallback = NULL;
+ s.baudRate = 0;
+
+ return (serialPort_t *)&s;
+ }
+
+ return NULL;
+ }
+
+ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
+ {
+ UNUSED(function);
+ if (testData.isRunCamSplitPortConfigurated) {
+ static serialPortConfig_t portConfig;
+
+ portConfig.identifier = SERIAL_PORT_USART3;
+ portConfig.msp_baudrateIndex = BAUD_115200;
+ portConfig.gps_baudrateIndex = BAUD_57600;
+ portConfig.telemetry_baudrateIndex = BAUD_AUTO;
+ portConfig.blackbox_baudrateIndex = BAUD_115200;
+ portConfig.functionMask = FUNCTION_MSP;
+
+ return &portConfig;
+ }
+
+ return NULL;
+ }
+
+ uint32_t serialRxBytesWaiting(const serialPort_t *instance)
+ {
+ UNUSED(instance);
+
+ testData.maxTimesOfRespDataAvailable--;
+ if (testData.maxTimesOfRespDataAvailable > 0) {
+ return 1;
+ }
+
+ return 0;
+ }
+
+ uint8_t serialRead(serialPort_t *instance)
+ {
+ UNUSED(instance);
+
+ if (testData.maxTimesOfRespDataAvailable > 0) {
+ static uint8_t i = 0;
+ static uint8_t buffer[] = { 0x55, 0x01, 0xFF, 0xad, 0xaa };
+
+ if (i >= 5) {
+ i = 0;
+ }
+
+ return buffer[i++];
+ }
+
+ return 0;
+ }
+
+ void sbufWriteString(sbuf_t *dst, const char *string)
+ {
+ UNUSED(dst); UNUSED(string);
+
+ if (testData.isAllowBufferReadWrite) {
+ sbufWriteData(dst, string, strlen(string));
+ }
+ }
+ void sbufWriteU8(sbuf_t *dst, uint8_t val)
+ {
+ UNUSED(dst); UNUSED(val);
+
+ if (testData.isAllowBufferReadWrite) {
+ *dst->ptr++ = val;
+ }
+ }
+
+ void sbufWriteData(sbuf_t *dst, const void *data, int len)
+ {
+ UNUSED(dst); UNUSED(data); UNUSED(len);
+
+ if (testData.isAllowBufferReadWrite) {
+ memcpy(dst->ptr, data, len);
+ dst->ptr += len;
+
+ }
+ }
+
+ // modifies streambuf so that written data are prepared for reading
+ void sbufSwitchToReader(sbuf_t *buf, uint8_t *base)
+ {
+ UNUSED(buf); UNUSED(base);
+
+ if (testData.isAllowBufferReadWrite) {
+ buf->end = buf->ptr;
+ buf->ptr = base;
+ }
+ }
+
+ bool feature(uint32_t) { return false;}
+ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { UNUSED(instance); UNUSED(data); UNUSED(count); }
+}
\ No newline at end of file
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"