From 685a0d6234c45b285cfc084826744bd8c1bd0f8a Mon Sep 17 00:00:00 2001 From: Joe Poser Date: Mon, 24 Aug 2015 10:55:57 +0100 Subject: [PATCH 01/15] Typo fix on Readme.me Minor fix on bullet point in new features list --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 10b7b32f65..11286b7f28 100644 --- a/README.md +++ b/README.md @@ -43,7 +43,7 @@ Cleanflight also has additional features not found in baseflight. * Graupner HoTT telemetry. * Multiple simultaneous telemetry providers. * Configurable serial ports for Serial RX, Telemetry, MSP, GPS - Use most devices on any port, softserial too. -+ more many minor bug fixes. +* And many more minor bug fixes. For a list of features, changes and some discussion please review the thread on MultiWii forums and consult the documentation. From ee248b46a91df3c00e89f4923fcae423b38bee51 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Wed, 2 Sep 2015 12:40:38 +0800 Subject: [PATCH 02/15] - Add Colibri Race md file --- docs/Board - ColibriRace.md | 127 ++++++++++++++++++++++++++++++++++++ 1 file changed, 127 insertions(+) create mode 100755 docs/Board - ColibriRace.md diff --git a/docs/Board - ColibriRace.md b/docs/Board - ColibriRace.md new file mode 100755 index 0000000000..1c6708a50e --- /dev/null +++ b/docs/Board - ColibriRace.md @@ -0,0 +1,127 @@ +# Board - Colibri RACE + +The Colibri RACE is a STM32F3 based flight control designed specifically to work with the TBS POWERCUBE multi rotor stack. + +## Hardware Features: +* STM32F303 based chipset for ultimate performance +* PPM, SBUS, DSM, DSMX input (5V and 3.3V provided over internal BUS). No inverters or hacks needed. +* 6 PWM ESC output channels (autoconnect, internal BUS) + * RGB LED strip support incl. power management + * Extension port for GPS / external compass / pressure sensor + * UART port for peripherals (Blackbox, FrSky telemetry etc.) + * Choose between plug & play sockets or solder pads for R/C and buzzer + * 5V buzzer output + * MPU6500 new generation accelerometer/gyro + * 3x status LED (DCDC pwr/ 3.3V pwr/ status) + * Battery monitoring for 12V, 5V and VBat supply + * Size: 36mmx36mm (30.5mm standard raster) + * Weight: 4.4g + + For more details please visit: + http://www.team-blacksheep.com/powercube + +## Serial Ports + + | Value | Identifier | Board Markings | Notes | + | ----- | ------------ | -------------- | ------------------------------------------| + | 1 | VCP | USB PORT | Main Port For MSP | + | 2 | USART1 | FREE PORT | PC4 and PC5(Tx and Rx respectively) | + | 3 | USART2 | PPM Serial | PA15 | + | 4 | USART3 | GPS PORT | PB10 and PB11(Tx and Rx respectively) | + +## Pinouts + + Full pinout details are available in the manual, here: + + http://www.team-blacksheep.com/colibri_race + + +### SWD - ICSP + + | Pin | Function | Notes | + | --- | -------------- | -------------------------------------------- | + | 1 | VCC_IN | 3.3 Volt | + | 2 | SWDIO | | + | 3 | nRESET | | + | 4 | SWCLK | | + | 5 | Ground | | + | 6 | SWO/TDO | | + +### Internal Bus + + | Pin | Function | Notes | + | --- | -------------- | -------------------------------------------- | + | 1 | PWM1 | MOTOR 1 | + | 2 | PWM2 | MOTOR 2 | + | 3 | PWM3 | MOTOR 3 | + | 4 | PWM4 | MOTOR 4 | + | 5 | PWM5 | MOTOR 5 (For Y6 or Hex X) | + | 6 | PWM6 | MOTOR 6 (For Y6 or Hex X) | + | 7 | BST SDA | Use For TBS CorePro Control Device | + | 8 | BST SCL | Use For TBS CorePro Control Device | + | 9 | PWM7 | Can be a normal GPIO (PA2) or PWM | + | 10 | PWM8 | Can be a normal GPIO (PA2) or PWM | + | 11 | 12.2V DCDC | If 12v is detected, the Blue LED will trun on| + | 12 | 5.1V DCDC | Voltage for MCU | + +### Servo + + | Pin | Function | Notes | + | --- | -------------- | -------------------------------------------- | + | 1 | Ground | | + | 2 | VCC_OUT | 5.1 Volt output to LCD Strip | + | 3 | PWM Servo | PB14 - PWM10 | + +### IO_1 - LED Strip + + | Pin | Function | Notes | + | --- | ----------------- | -------------------------------------------- | + | 1 | LED_STRIP | Enable `feature LED_STRIP` | + | 2 | VCC_OUT | 5.1 Volt output to LCD Strip | + | 3 | Ground | | + +### IO_2 - Sensor Interface + + | Pin | Function | Notes | + | --- | ----------------- | -------------------------------------------- | + | 1 | VCC_OUT | 4.7 Volt output to the device | + | 2 | Ground | | + | 3 | UART3 TX | GPS | + | 4 | UART3 RX | GPS | + | 5 | SDA | mag, pressure, or other i2c device | + | 6 | SCL | mag, pressure, or other i2c device | + +### IO_3 - RC input + + When RX_PPM/RX_SERIAL is used the IO_3 pinout is as follows. + if RX_SERIAL, Telemetry need to set to UART2 and choose your device. + + | Pin | Function | Notes | + | --- | ----------------- | -------------------------------------------- | + | 2 | PPM/Serial | Can PPM or Serial input | + | 2 | VCC_OUT | 3.3 Volt output to the device | + | 3 | Ground | | + | 4 | VCC_OUT | 5.1 Volt output to the device | + +### IO_4 - Buzzer + + | Pin | Function | Notes | + | --- | ----------------- | -------------------------------------------- | + | 1 | BUZZER | Normal high (5.1v) | + | 2 | VCC_OUT | 5.1 Volt output to the device | + +### IO_5 - Free UART + + | Pin | Function | Notes | + | --- | ----------------- | -------------------------------------------- | + | 1 | UART1 TX | Free UART | + | 2 | UART1 RX | Free UART | + | 3 | Ground | | + | 4 | VCC_OUT | 4.7 Volt output to the device | + +### IO_6 - IR TX (externsion) + + | Pin | Function | Notes | + | --- | ----------------- | -------------------------------------------- | + | 1 | IR TX | | + | 2 | Ground | | From 45d31f22902982b39cc325d709f51fc2447d09f4 Mon Sep 17 00:00:00 2001 From: sppnk Date: Thu, 3 Sep 2015 16:47:00 +0200 Subject: [PATCH 03/15] Fixing servo command bug The limits for RATE were bad, so we had a "Parse error" when changing rate values other than 100. --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eddbf960af..f8d79d07fd 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1130,7 +1130,7 @@ static void cliServo(char *cmdline) arguments[MAX] < PWM_PULSE_MIN || arguments[MAX] > PWM_PULSE_MAX || arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] || arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] || - arguments[RATE] < 100 || arguments[RATE] > 100 || + arguments[RATE] < -100 || arguments[RATE] > 100 || arguments[FORWARD] >= MAX_SUPPORTED_RC_CHANNEL_COUNT || arguments[ANGLE_AT_MIN] < 0 || arguments[ANGLE_AT_MIN] > 180 || arguments[ANGLE_AT_MAX] < 0 || arguments[ANGLE_AT_MAX] > 180 From f83e8f25929607593751b62253c683894af47eed Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Fri, 4 Sep 2015 16:02:08 +0800 Subject: [PATCH 04/15] - Colibri Race md file update. --- docs/Board - ColibriRace.md | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/docs/Board - ColibriRace.md b/docs/Board - ColibriRace.md index 1c6708a50e..a047f4940b 100755 --- a/docs/Board - ColibriRace.md +++ b/docs/Board - ColibriRace.md @@ -41,9 +41,9 @@ The Colibri RACE is a STM32F3 based flight control designed specifically to work | Pin | Function | Notes | | --- | -------------- | -------------------------------------------- | | 1 | VCC_IN | 3.3 Volt | - | 2 | SWDIO | | + | 2 | SWDIO | | | 3 | nRESET | | - | 4 | SWCLK | | + | 4 | SWCLK | | | 5 | Ground | | | 6 | SWO/TDO | | @@ -51,17 +51,17 @@ The Colibri RACE is a STM32F3 based flight control designed specifically to work | Pin | Function | Notes | | --- | -------------- | -------------------------------------------- | - | 1 | PWM1 | MOTOR 1 | - | 2 | PWM2 | MOTOR 2 | + | 1 | PWM1 | MOTOR 1 | + | 2 | PWM2 | MOTOR 2 | | 3 | PWM3 | MOTOR 3 | - | 4 | PWM4 | MOTOR 4 | - | 5 | PWM5 | MOTOR 5 (For Y6 or Hex X) | - | 6 | PWM6 | MOTOR 6 (For Y6 or Hex X) | + | 4 | PWM4 | MOTOR 4 | + | 5 | PWM5 | MOTOR 5 (For Y6 or Hex X) | + | 6 | PWM6 | MOTOR 6 (For Y6 or Hex X) | | 7 | BST SDA | Use For TBS CorePro Control Device | | 8 | BST SCL | Use For TBS CorePro Control Device | | 9 | PWM7 | Can be a normal GPIO (PA2) or PWM | | 10 | PWM8 | Can be a normal GPIO (PA2) or PWM | - | 11 | 12.2V DCDC | If 12v is detected, the Blue LED will trun on| + | 11 | 12.2V DCDC | If 12v is detected, the Blue LED will turn on| | 12 | 5.1V DCDC | Voltage for MCU | ### Servo @@ -93,12 +93,11 @@ The Colibri RACE is a STM32F3 based flight control designed specifically to work ### IO_3 - RC input - When RX_PPM/RX_SERIAL is used the IO_3 pinout is as follows. - if RX_SERIAL, Telemetry need to set to UART2 and choose your device. + IO_3 is used for RX_PPM/RX_SERIAL. Under the `PORT` tab, set RX_SERIAL to UART2 when using RX_SERIAL. | Pin | Function | Notes | | --- | ----------------- | -------------------------------------------- | - | 2 | PPM/Serial | Can PPM or Serial input | + | 1 | PPM/Serial | Can PPM or Serial input | | 2 | VCC_OUT | 3.3 Volt output to the device | | 3 | Ground | | | 4 | VCC_OUT | 5.1 Volt output to the device | From c8f4a479e0d663ac3e12ced08a94cf22c7433b01 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Fri, 4 Sep 2015 16:57:58 +0800 Subject: [PATCH 05/15] - Colibri Race md file update. --- docs/Board - ColibriRace.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Board - ColibriRace.md b/docs/Board - ColibriRace.md index a047f4940b..e37cb789e6 100755 --- a/docs/Board - ColibriRace.md +++ b/docs/Board - ColibriRace.md @@ -118,7 +118,7 @@ The Colibri RACE is a STM32F3 based flight control designed specifically to work | 3 | Ground | | | 4 | VCC_OUT | 4.7 Volt output to the device | -### IO_6 - IR TX (externsion) +### IO_6 - IR TX (extension) | Pin | Function | Notes | | --- | ----------------- | -------------------------------------------- | From f7530df974e1570a6572ceb40d1b80367fcd6bc6 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Fri, 4 Sep 2015 23:44:41 +1200 Subject: [PATCH 06/15] Fix 1 millisecond backwards time leap in time measured by micros() This race condition caused periodic flight instability when micros() was called precisely on a 1000 nanosecond boundary. --- src/main/drivers/system.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 0785250c23..ba4dd6b0a4 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -99,6 +99,12 @@ uint32_t micros(void) do { ms = sysTickUptime; cycle_cnt = SysTick->VAL; + + /* + * If the SysTick timer expired during the previous instruction, we need to give it a little time for that + * interrupt to be delivered before we can recheck sysTickUptime: + */ + asm volatile("\tnop\n"); } while (ms != sysTickUptime); return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks; } From 3b9cd37a57ade925be22b322d96a8c36656b2f33 Mon Sep 17 00:00:00 2001 From: ProDrone Date: Thu, 27 Aug 2015 14:31:51 +0200 Subject: [PATCH 07/15] Prevent arming when in CLI mode --- src/main/io/serial_cli.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eddbf960af..ad663a144f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1573,6 +1573,7 @@ void cliEnter(serialPort_t *serialPort) setPrintfSerialPort(cliPort); cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n"); cliPrompt(); + ENABLE_ARMING_FLAG(PREVENT_ARMING); } static void cliExit(char *cmdline) From a46832fd853f28c66aee7435159fee67b4d15db0 Mon Sep 17 00:00:00 2001 From: ProDrone Date: Tue, 25 Aug 2015 21:47:49 +0200 Subject: [PATCH 08/15] Changed behavior of PR #1233 and doc update Stick channels only have AUTO and HOLD mode. AUX channels only have SET and HOLD mode. Added check to parameter in CLI. Modified rx.md to reflect changes (and more). +1 squashed commit: - A cleaner approach for the same functionality Basically addressing all comments from Hydra --- docs/Rx.md | 13 +++++++++---- src/main/config/config.c | 4 ++-- src/main/io/serial_cli.c | 39 +++++++++++++++++++++------------------ src/main/rx/rx.c | 8 +++++--- src/main/rx/rx.h | 8 ++++++++ 5 files changed, 45 insertions(+), 27 deletions(-) diff --git a/docs/Rx.md b/docs/Rx.md index a2f6511feb..a331e7dcfc 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -144,13 +144,14 @@ Signal loss can be detected when: ### RX loss configuration The `rxfail` cli command is used to configure per-channel rx-loss behaviour. -You can use the `rxfail` command to change this behaviour, a channel can either be AUTOMATIC, HOLD or SET. +You can use the `rxfail` command to change this behaviour. +A flight channel can either be AUTOMATIC or HOLD, an AUX channel can either be SET or HOLD. -* AUTOMATIC - Flight channels are set to safe values (low throttle, mid position for yaw/pitch/roll), all AUX channels HOLD last value. +* AUTOMATIC - Flight channels are set to safe values (low throttle, mid position for yaw/pitch/roll). * HOLD - Channel holds the last value. * SET - Channel is set to a specific configured value. -The default mode for all channels is AUTOMATIC. +The default mode is AUTOMATIC for flight channels and HOLD for AUX channels. The rxfail command can be used in conjunction with mode ranges to trigger various actions. @@ -232,8 +233,12 @@ Set the RX for 'No Pulses'. Turn OFF TX and RX, Turn ON RX. Press and release ### Graupner GR-24 PWM -Set failsafe on channels 1-4 set to OFF in the receiver settings (via transmitter menu). +Set failsafe on the throttle channel in the receiver settings (via transmitter menu) to a value below `rx_min_usec` using channel mode FAILSAFE. +This is the prefered way, since this is *much faster* detected by the FC then a channel that sends no pulses (OFF). +__NOTE:__ +One or more control channels may be set to OFF to signal a failsafe condition to the FC, all other channels *must* be set to either HOLD or OFF. +Do __NOT USE__ the mode indicated with FAILSAFE instead, as this combination is NOT handled correctly by the FC. ## Receiver Channel Range Configuration. diff --git a/src/main/config/config.c b/src/main/config/config.c index 6764ad4abd..61ef67744f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -407,8 +407,8 @@ static void resetConf(void) for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; - channelFailsafeConfiguration->mode = RX_FAILSAFE_MODE_AUTO; - channelFailsafeConfiguration->step = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); + channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; + channelFailsafeConfiguration->step = (i == THROTTLE) ? masterConfig.rxConfig.rx_min_usec : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); } masterConfig.rxConfig.rssi_channel = 0; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eddbf960af..1e738a2820 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -170,7 +170,12 @@ static const char * const featureNames[] = { }; // sync this with rxFailsafeChannelMode_e -static char rxFailsafeModes[RX_FAILSAFE_MODE_COUNT] = { 'a', 'h', 's'}; +static const char rxFailsafeModeCharacters[] = "ahs"; + +static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT][RX_FAILSAFE_MODE_COUNT] = { + { RX_FAILSAFE_MODE_AUTO, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_INVALID }, + { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } +}; #ifndef CJMCU // sync this with sensors_e @@ -587,27 +592,22 @@ static void cliRxFail(char *cmdline) rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[channel]; uint16_t value; + rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX; rxFailsafeChannelMode_e mode = channelFailsafeConfiguration->mode; bool requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; - // TODO optimize to use rxFailsafeModes - less logic. ptr = strchr(ptr, ' '); if (ptr) { - switch (*(++ptr)) { - case 'a': - mode = RX_FAILSAFE_MODE_AUTO; - break; - - case 'h': - mode = RX_FAILSAFE_MODE_HOLD; - break; - - case 's': - mode = RX_FAILSAFE_MODE_SET; - break; - default: - cliShowParseError(); - return; + char *p = strchr(rxFailsafeModeCharacters, *(++ptr)); + if (p) { + uint8_t requestedMode = p - rxFailsafeModeCharacters; + mode = rxFailsafeModesTable[type][requestedMode]; + } else { + mode = RX_FAILSAFE_MODE_INVALID; + } + if (mode == RX_FAILSAFE_MODE_INVALID) { + cliShowParseError(); + return; } requireValue = mode == RX_FAILSAFE_MODE_SET; @@ -626,12 +626,15 @@ static void cliRxFail(char *cmdline) } channelFailsafeConfiguration->step = value; + } else if (requireValue) { + cliShowParseError(); + return; } channelFailsafeConfiguration->mode = mode; } - char modeCharacter = rxFailsafeModes[channelFailsafeConfiguration->mode]; + char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; // triple use of printf below // 1. acknowledge interpretation on command, diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 78016c43bb..6a74de89d9 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -346,27 +346,29 @@ static uint16_t getRxfailValue(uint8_t channel) rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; switch(channelFailsafeConfiguration->mode) { - default: case RX_FAILSAFE_MODE_AUTO: switch (channel) { case ROLL: case PITCH: case YAW: return rxConfig->midrc; + case THROTTLE: if (feature(FEATURE_3D)) return rxConfig->midrc; else return rxConfig->rx_min_usec; } - // fall though to HOLD if there's nothing specific to do. + /* no break */ + + default: + case RX_FAILSAFE_MODE_INVALID: case RX_FAILSAFE_MODE_HOLD: return rcData[channel]; case RX_FAILSAFE_MODE_SET: return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step); } - } STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7ab45f1066..c35b3d10ca 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -85,10 +85,18 @@ typedef enum { RX_FAILSAFE_MODE_AUTO = 0, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET, + RX_FAILSAFE_MODE_INVALID, } rxFailsafeChannelMode_e; #define RX_FAILSAFE_MODE_COUNT 3 +typedef enum { + RX_FAILSAFE_TYPE_FLIGHT = 0, + RX_FAILSAFE_TYPE_AUX, +} rxFailsafeChannelType_e; + +#define RX_FAILSAFE_TYPE_COUNT 2 + typedef struct rxFailsafeChannelConfiguration_s { uint8_t mode; // See rxFailsafeChannelMode_e uint8_t step; From 3a13edfdad83fb3eaeada195654fbadbe5acb495 Mon Sep 17 00:00:00 2001 From: ProDrone Date: Wed, 26 Aug 2015 21:31:48 +0200 Subject: [PATCH 09/15] Fix false RX loss detection on EEPROM read/write The problem is caused by hardware counters (timekeeping and PPM/PWM measurement) that keep running while the firmware is frozen. The result is misinterpretation of received data. EEPROM read & write now call a suspend and resume function to make RX ignore incoming wrong data during reads/writes (and flush the wrong data on resume). Fixes issue #1257 (+1 squashed commit) - Moved the check for skipSamples to the right place. As commented by hydra --- src/main/config/config.c | 8 ++++++++ src/main/rx/rx.c | 18 ++++++++++++++++++ src/main/rx/rx.h | 2 ++ 3 files changed, 28 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6764ad4abd..1c884eeadd 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -829,6 +829,8 @@ void readEEPROM(void) if (!isEEPROMContentValid()) failureMode(10); + suspendRxSignal(); + // Read flash memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); @@ -844,6 +846,8 @@ void readEEPROM(void) validateAndFixConfig(); activateConfig(); + + resumeRxSignal(); } void readEEPROMAndNotify(void) @@ -862,6 +866,8 @@ void writeEEPROM(void) uint32_t wordOffset; int8_t attemptsRemaining = 3; + suspendRxSignal(); + // prepare checksum/version constants masterConfig.version = EEPROM_CONF_VERSION; masterConfig.size = sizeof(master_t); @@ -903,6 +909,8 @@ void writeEEPROM(void) if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { failureMode(10); } + + resumeRxSignal(); } void ensureEEPROMContainsValidData(void) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 78016c43bb..0d77bedaeb 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -71,6 +71,7 @@ static bool rxFlightChannelsValid = false; static uint32_t rxUpdateAt = 0; static uint32_t needRxSignalBefore = 0; +static uint8_t skipRxSamples = 0; int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] @@ -79,6 +80,8 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] #define DELAY_50_HZ (1000000 / 50) #define DELAY_10_HZ (1000000 / 10) +#define SKIP_RC_SAMPLES_ON_SUSPEND 75 // approx. 1.5 seconds of samples at 50Hz +#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements rxRuntimeConfig_t rxRuntimeConfig; static rxConfig_t *rxConfig; @@ -261,6 +264,16 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime) } } +void suspendRxSignal(void) +{ + skipRxSamples = SKIP_RC_SAMPLES_ON_SUSPEND; +} + +void resumeRxSignal(void) +{ + skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME; +} + void updateRx(uint32_t currentTime) { resetRxSignalReceivedFlagIfNeeded(currentTime); @@ -467,6 +480,11 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) } } + if (skipRxSamples) { + skipRxSamples--; + return; + } + readRxChannelsApplyRanges(); detectAndApplySignalLossBehaviour(); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7ab45f1066..ef6c3df431 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -142,3 +142,5 @@ uint8_t serialRxFrameStatus(rxConfig_t *rxConfig); void updateRSSI(uint32_t currentTime); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration); +void suspendRxSignal(void); +void resumeRxSignal(void); From f0681de53d795a620bb1908d5f784ae3b3c3e0da Mon Sep 17 00:00:00 2001 From: ProDrone Date: Tue, 5 May 2015 16:11:05 +0200 Subject: [PATCH 10/15] Updates and feature additions to failsafe system. - Added failsafe flightmode and rc control box. To make failsafe procedure a separate flight mode and make it possible to trigger failsafe with an AUX switch. - Failsafe mode is activated when failsafe is active. RC link lost is simulated with the failsafe AUX switch. When NOT armed: failsafe switch to failsafe mode is shown in GUI (mode tab). - Activate failsafe mode with AUX switch. - Prevent arming when failsafe via AUX switch is active (safety issue). - Make failsafe disarm if motors armed and throttle was LOW (2D & 3D) for `failsafe_throttle_low_delay` time (__JustDisarmEvent__). Applied code changes to effectively add pull request: Make failsafe disarm if motors armed and throttle low #717. - Use failsafeIsMonitoring() to actually start monitoring. - Added `failsafe_kill_switch` to code. When set to 1 (0 is default), the failsafe switch will instantly disarm (__KillswitchEvent__) instead of executing the landings procedure. Arming is NOT locked after this, so the craft could be re-armed if needed. This is intended for racing quads where damage and danger must be minimized in case of a pilot error. - Added `failsafe_throttle_low_delay`, adapted documentation. Used to adjust the time throttle level must have been LOW to _only disarm_ instead of _full failsafe procedure_ (__JustDisarmEvent__). - Updated the failsafe documentation. - Re-enable arming at end of failsafe procedure. At the end of a handled failsafe event, that means: auto-landing, __JustDisarmEvent__ or __KillswitchEvent__, the RX link is monitored for valid data. Monitoring is a part of the failsafe handling, which means the craft is still in failsafe mode while this is done. Arming is re-enabled (allowed) when there is a valid RX link for more then XX seconds, where XX depends on the handled event like this: 1. XX = 30 seconds after auto landing. 2. XX = 3 seconds after __JustDisarmEvent__. 3. XX = 0 seconds after __KillswitchEvent__. NOTE: When armed via an AUX switch, you will have to switch to the disarmed position at the very end to be able to re-arm. The failsafe mode will not end until you do. - __KillswitchEvent__ has now priority over __JustDisarmEvent__ - Apply rxfail values instantly when failsafe switch is ON - Added missing cases to display.c Show M when failsafe is monitoring for RX recovery (AND disarming when armed with a switch). === Reworked the code from counter-based to time-based. - AUX failsafe switch now has identical behavior to RX loss. - Added RX failure and RX recovery timing. - __KillswitchEvent__ skips RX failure detection delay (direct disarm). === [UNIT TESTS] Adapted failsafe related unittests from counter-based to time-based - Added failsafeOnValidDataFailed() to some tests - Removed duplicate test setup from rc_controls_unittest.cc - Removed magic numbers from rx_ranges_unittest.cc and rx_rx_unittest.cc - Reworked all test-cases for flight_failsafe_unittest.cc --- docs/Failsafe.md | 146 +++++---- src/main/config/config.c | 7 + src/main/config/config.h | 1 + src/main/config/runtime_config.h | 1 + src/main/flight/failsafe.c | 179 +++++++---- src/main/flight/failsafe.h | 30 +- src/main/io/display.c | 6 + src/main/io/rc_controls.h | 1 + src/main/io/serial_cli.c | 2 + src/main/io/serial_msp.c | 10 +- src/main/main.c | 4 +- src/main/mw.c | 3 + src/main/rx/rx.c | 6 +- src/test/unit/flight_failsafe_unittest.cc | 362 +++++++++++++++------- src/test/unit/rc_controls_unittest.cc | 20 ++ src/test/unit/rx_ranges_unittest.cc | 8 +- src/test/unit/rx_rx_unittest.cc | 7 +- 17 files changed, 542 insertions(+), 251 deletions(-) diff --git a/docs/Failsafe.md b/docs/Failsafe.md index b1ae95b068..4084aad4e9 100644 --- a/docs/Failsafe.md +++ b/docs/Failsafe.md @@ -5,77 +5,39 @@ There are two types of failsafe: 1. Receiver based failsafe 2. Flight controller based failsafe -Receiver based failsafe is where you, from your transmitter and receiver, configure channels to output desired signals if your receiver detects signal loss. +Receiver based failsafe is where you, from your transmitter and receiver, configure channels to output desired signals if your receiver detects signal loss and goes to __rx-failsafe-state__. The idea is that you set throttle and other controls so the aircraft descends in a controlled manner. See your receiver's documentation for this method. -Flight controller based failsafe is where the flight controller attempts to detect signal loss from your receiver. +Flight controller based failsafe is where the flight controller attempts to detect signal loss and/or the __rx-failsafe-state__ of your receiver and upon detection goes to __fc-failsafe-state__. The idea is that the flight controller starts using substitutes for all controls, which are set by you, using the CLI command `rxfail` (see `rxfail` document) or the cleanflight-configurator GUI. It is possible to use both types at the same time, which may be desirable. Flight controller failsafe can even help if your receiver signal wires come loose, get damaged or your receiver malfunctions in a way the receiver itself cannot detect. +Alternatively you may configure a transmitter switch to activate failsafe mode. This is useful for fieldtesting the failsafe system and as a **_`PANIC`_** switch when you lose orientation. + ## Flight controller failsafe system -The failsafe system is not activated until 5 seconds after the flight controller boots up. This is to prevent failsafe from activating, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data. Note that you need to activate the 'FAILSAFE' feature in order to activate failsafe on flight controller. +The __failsafe-auto-landing__ system is not activated until 5 seconds after the flight controller boots up. This is to prevent __failsafe-auto-landing__ from activating, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data. -After the failsafe has forced a landing, the flight controller cannot be armed and has to be reset. - -The failsafe system attempts to detect when your receiver loses signal. It then attempts to prevent your aircraft from flying away uncontrollably by enabling an auto-level mode and setting the throttle that should allow the craft to come to a safer landing. +The __failsafe-detection__ system attempts to detect when your receiver loses signal *continuously* but the __failsafe-auto-landing__ starts *only when your craft is __armed__*. It then attempts to prevent your aircraft from flying away uncontrollably by enabling an auto-level mode and setting the throttle that should allow the craft to come to a safer landing. -The failsafe is activated when: +**The failsafe is activated when the craft is armed and either:** -Either: +* The control (stick) channels do not have valid signals AND the failsafe guard time specified by `failsafe_delay` has elapsed. +* A transmitter switch that is configured to control the failsafe mode is switched ON (and 'failsafe_kill_switch' is set to 0). -a) no valid channel data from the RX is received via Serial RX. +Failsafe intervention will be aborted when it was due to: -b) the first 4 channels do not have valid signals. - -And when: - -c) the failsafe guard time specified by `failsafe_delay` has elapsed. +* a lost RC signal and the RC signal has recovered. +* a transmitter failsafe switch was set to ON position and the switch is set to OFF position (and 'failsafe_kill_switch' is set to 0). Note that: +* At the end of a failsafe intervention, the flight controller will be disarmed and re-arming will be locked. From that moment on it is no longer possible to abort or re-arm and the flight controller has to be reset. +* When 'failsafe_kill_switch' is set to 1 and the rc switch configured for failsafe is set to ON, the craft is instantly disarmed (but re-arming is not locked). Similar effect can be achieved by setting 'failsafe_throttle' to 1000 and 'failsafe_off_delay' to 0 (but arming is locked). +* Prior to starting a failsafe intervention it is checked if the throttle position was below 'min_throttle' level for the last 'failsafe_throttle_low_delay' seconds. If it was the craft is assumed to be on the ground and is only disarmed. It may be re-armed without a power cycle. -d) The failsafe system will be activated regardless of current throttle position. - -e) The craft may already be on the ground with motors stopped and that motors and props could spin again - the software does not currently detect if the craft is on the ground. Take care when using MOTOR_STOP feature. - -### Testing - -Bench test the failsafe system before flying - remove props while doing so. - -1. Arm the craft. -1. Turn off transmitter or unplug RX. -1. Observe motors spin at configured throttle setting for configured duration. -1. Observe motors turn off after configured duration. -1. Ensure that when you turn on your TX again or reconnect the RX that you cannot re-arm once the motors have stopped. -1. Power cycle the FC. -1. Arm the craft. -1. Turn off transmitter or unplug RX. -1. Observe motors spin at configured throttle setting for configured duration. -1. Turn on TX or reconnect RX. -1. Ensure that your switch positions don't now cause the craft to disarm (otherwise it would fall out of the sky on regained signal). -1. Observe that normal flight behavior is resumed. -1. Disarm. - -Field test the failsafe system - -1. Perform bench testing first! -1. On a calm day go to an unpopulated area away from buildings or test indoors in a safe controlled environment - e.g. inside a big net. -1. Arm the craft. -1. Hover over something soft (long grass, ferns, heather, foam, etc.). -1. Descend the craft and observe throttle position and record throttle value from your TX channel monitor. Ideally 1500 should be hover. So your value should be less than 1500. -1. Stop, disarm. -1. Set failsafe throttle to the recorded value. -1. Arm, hover over something soft again. -1. Turn off TX (!) -1. Observe craft descends and motors continue to spin for the configured duration. -1. Observe FC disarms after the configured duration. -1. Remove flight battery. - -If craft descends too quickly then increase failsafe throttle setting. - -Ensure that the duration is long enough for your craft to land at the altitudes you normally fly at. - - +Some notes about **SAFETY**: +* The failsafe system will be activated regardless of current throttle position. So when the failsafe intervention is aborted (RC signal restored/failsafe switch set to OFF) the current stick position will direct the craft ! +* The craft may already be on the ground with motors stopped and that motors and props could spin again - the software does not currently detect if the craft is on the ground. Take care when using `MOTOR_STOP` feature. **Props will spin up without warning**, when armed with `MOTOR_STOP` feature ON (props are not spinning) **_and_** failsafe is activated ! ## Configuration @@ -83,27 +45,27 @@ When configuring the flight controller failsafe, use the following steps: 1. Configure your receiver to do one of the following: -a) Upon signal loss, send no signal/pulses over the channels - -b) Send an invalid signal over the channels (for example, send values lower than 'rx_min_usec') +* Upon signal loss, send no signal/pulses over the channels +* Send an invalid signal over the channels (for example, send values lower than 'rx_min_usec') and -c) Ensure your receiver does not send out channel data that would cause a disarm by switch or sticks to be registered by the FC. This is especially important for those using a switch to arm. +* Ensure your receiver does not send out channel data that would cause a disarm by switch or sticks to be registered by the FC. This is especially important for those using a switch to arm. See your receiver's documentation for direction on how to accomplish one of these. +* Configure one of the transmitter switches to activate the failsafe mode. + 2. Set 'failsafe_off_delay' to an appropriate value based on how high you fly 3. Set 'failsafe_throttle' to a value that allows the aircraft to descend at approximately one meter per second (default is 1000 which should be throttle off). -4. Enable 'FAILSAFE' feature in Cleanflight GUI or via CLI using `feature FAILSAFE` These are the basic steps for flight controller failsafe configuration; see Failsafe Settings below for additional settings that may be changed. -##Failsafe Settings +## Failsafe Settings Failsafe delays are configured in 0.1 second steps. @@ -123,5 +85,63 @@ Delay after failsafe activates before motors finally turn off. This is the amou Throttle level used for landing. Specify a value that causes the aircraft to descend at about 1M/sec. Default is set to 1000 which should correspond to throttle off. -Use standard RX usec values. See RX documentation. +### `failsafe_kill_switch` +Configure the rc switched failsafe action: the same action as when the rc link is lost (set to 0) or disarms instantly (set to 1). Also see above. + +### `failsafe_throttle_low_delay` + +Time throttle level must have been below 'min_throttle' to _only disarm_ instead of _full failsafe procedure_. + +Use standard RX usec values. See Rx documentation. + +### `rx_min_usec` + +The lowest channel value considered valid. e.g. PWM/PPM pulse length + +### `rx_max_usec` + +The highest channel value considered valid. e.g. PWM/PPM pulse length + +The `rx_min_usec` and `rx_max_usec` settings helps detect when your RX stops sending any data, enters failsafe mode or when the RX looses signal. + +With a Graupner GR-24 configured for PWM output with failsafe on channels 1-4 set to OFF in the receiver settings then this setting, at its default value, will allow failsafe to be activated. + +## Testing + +**Bench test the failsafe system before flying - _remove props while doing so_.** + +1. Arm the craft. +1. Turn off transmitter or unplug RX. +1. Observe motors spin at configured throttle setting for configured duration. +1. Observe motors turn off after configured duration. +1. Ensure that when you turn on your TX again or reconnect the RX that you cannot re-arm once the motors have stopped. +1. Power cycle the FC. +1. Arm the craft. +1. Turn off transmitter or unplug RX. +1. Observe motors spin at configured throttle setting for configured duration. +1. Turn on TX or reconnect RX. +1. Ensure that your switch positions don't now cause the craft to disarm (otherwise it would fall out of the sky on regained signal). +1. Observe that normal flight behavior is resumed. +1. Disarm. + +**Field test the failsafe system.** + +1. Perform bench testing first! +1. On a calm day go to an unpopulated area away from buildings or test indoors in a safe controlled environment - e.g. inside a big net. +1. Arm the craft. +1. Hover over something soft (long grass, ferns, heather, foam, etc.). +1. Descend the craft and observe throttle position and record throttle value from your TX channel monitor. Ideally 1500 should be hover. So your value should be less than 1500. +1. Stop, disarm. +1. Set failsafe throttle to the recorded value. +1. Arm, hover over something soft again. +1. Turn off TX (!) +1. Observe craft descends and motors continue to spin for the configured duration. +1. Observe FC disarms after the configured duration. +1. Remove flight battery. + +If craft descends too quickly then increase failsafe throttle setting. + +Ensure that the duration is long enough for your craft to land at the altitudes you normally fly at. + +Using a configured transmitter switch to activate failsafe mode, instead of switching off your TX, is good primary testing method in addition to the above procedure. diff --git a/src/main/config/config.c b/src/main/config/config.c index 6764ad4abd..4550fd1aa7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -348,6 +348,11 @@ static void setControlRateProfile(uint8_t profileIndex) currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex]; } +uint16_t getCurrentMinthrottle(void) +{ + return masterConfig.escAndServoConfig.minthrottle; +} + // Default settings static void resetConf(void) { @@ -483,6 +488,8 @@ static void resetConf(void) masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. + masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss + masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition #ifdef USE_SERVOS // servos diff --git a/src/main/config/config.h b/src/main/config/config.h index fabc135da5..0de14d60d9 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -73,3 +73,4 @@ void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); +uint16_t getCurrentMinthrottle(void); diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 090a57ca76..c5a1bff6ac 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -41,6 +41,7 @@ typedef enum { AUTOTUNE_MODE = (1 << 7), PASSTHRU_MODE = (1 << 8), SONAR_MODE = (1 << 9), + FAILSAFE_MODE = (1 << 10), } flightModeFlags_e; extern uint16_t flightModeFlags; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a1090883a9..f02b318f8a 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -23,10 +23,12 @@ #include "common/axis.h" #include "rx/rx.h" +#include "drivers/system.h" #include "io/beeper.h" #include "io/escservo.h" #include "io/rc_controls.h" #include "config/runtime_config.h" +#include "config/config.h" #include "flight/failsafe.h" @@ -47,10 +49,19 @@ static failsafeConfig_t *failsafeConfig; static rxConfig_t *rxConfig; +static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC + static void failsafeReset(void) { - failsafeState.counter = 0; + failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.validRxDataReceivedAt = 0; + failsafeState.validRxDataFailedAt = 0; + failsafeState.throttleLowPeriod = 0; + failsafeState.landingShouldBeFinishedAt = 0; + failsafeState.receivingRxDataPeriod = 0; + failsafeState.receivingRxDataPeriodPreset = 0; failsafeState.phase = FAILSAFE_IDLE; + failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; } /* @@ -62,10 +73,11 @@ void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse) failsafeReset(); } -void failsafeInit(rxConfig_t *intialRxConfig) +void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle) { rxConfig = intialRxConfig; + deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; failsafeState.monitoring = false; @@ -77,13 +89,6 @@ failsafePhase_e failsafePhase() return failsafeState.phase; } -#define FAILSAFE_COUNTER_THRESHOLD 20 - -bool failsafeIsReceivingRxData(void) -{ - return failsafeState.counter <= FAILSAFE_COUNTER_THRESHOLD; -} - bool failsafeIsMonitoring(void) { return failsafeState.monitoring; @@ -99,25 +104,17 @@ void failsafeStartMonitoring(void) failsafeState.monitoring = true; } -static bool failsafeHasTimerElapsed(void) -{ - return failsafeState.counter > (5 * failsafeConfig->failsafe_delay); -} - -static bool failsafeShouldForceLanding(bool armed) -{ - return failsafeHasTimerElapsed() && armed; -} - static bool failsafeShouldHaveCausedLandingByNow(void) { - return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay); + return (millis() > failsafeState.landingShouldBeFinishedAt); } static void failsafeActivate(void) { failsafeState.active = true; failsafeState.phase = FAILSAFE_LANDING; + ENABLE_FLIGHT_MODE(FAILSAFE_MODE); + failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; failsafeState.events++; } @@ -130,27 +127,41 @@ static void failsafeApplyControlInput(void) rcData[THROTTLE] = failsafeConfig->failsafe_throttle; } +bool failsafeIsReceivingRxData(void) +{ + return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP); +} + void failsafeOnValidDataReceived(void) { - if (failsafeState.counter > FAILSAFE_COUNTER_THRESHOLD) - failsafeState.counter -= FAILSAFE_COUNTER_THRESHOLD; - else - failsafeState.counter = 0; + failsafeState.validRxDataReceivedAt = millis(); + if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) { + failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; + } +} + +void failsafeOnValidDataFailed(void) +{ + failsafeState.validRxDataFailedAt = millis(); + if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) { + failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; + } } void failsafeUpdateState(void) { - bool receivingRxData = failsafeIsReceivingRxData(); - bool armed = ARMING_FLAG(ARMED); - beeperMode_e beeperMode = BEEPER_SILENCE; - - if (receivingRxData) { - failsafeState.phase = FAILSAFE_IDLE; - failsafeState.active = false; - } else { - beeperMode = BEEPER_RX_LOST; + if (!failsafeIsMonitoring()) { + return; } + bool receivingRxData = failsafeIsReceivingRxData(); + bool armed = ARMING_FLAG(ARMED); + bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); + beeperMode_e beeperMode = BEEPER_SILENCE; + + if (!receivingRxData) { + beeperMode = BEEPER_RX_LOST; + } bool reprocessState; @@ -159,50 +170,100 @@ void failsafeUpdateState(void) switch (failsafeState.phase) { case FAILSAFE_IDLE: - if (!receivingRxData && armed) { - failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; - - reprocessState = true; + if (armed) { + // Track throttle command below minimum time + if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) { + failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; + } + // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) + if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) { + // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON + failsafeActivate(); + failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData + reprocessState = true; + } else if (!receivingRxData) { + if (millis() > failsafeState.throttleLowPeriod) { + // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds + failsafeActivate(); + failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData + } else { + failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; + } + reprocessState = true; + } + } else { + // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) + if (failsafeSwitchIsOn) { + ENABLE_FLIGHT_MODE(FAILSAFE_MODE); + } else { + DISABLE_FLIGHT_MODE(FAILSAFE_MODE); + } + // Throttle low period expired (= low long enough for JustDisarm) + failsafeState.throttleLowPeriod = 0; } break; case FAILSAFE_RX_LOSS_DETECTED: - - if (failsafeShouldForceLanding(armed)) { + if (receivingRxData) { + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + } else { // Stabilize, and set Throttle to specified level failsafeActivate(); - - reprocessState = true; } + reprocessState = true; break; case FAILSAFE_LANDING: + if (receivingRxData) { + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; + } if (armed) { failsafeApplyControlInput(); beeperMode = BEEPER_RX_LOST_LANDING; } - if (failsafeShouldHaveCausedLandingByNow() || !armed) { - + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData failsafeState.phase = FAILSAFE_LANDED; - reprocessState = true; - } break; case FAILSAFE_LANDED: - - if (!armed) { - break; - } - - // This will prevent the automatic rearm if failsafe shuts it down and prevents - // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm - ENABLE_ARMING_FLAG(PREVENT_ARMING); - - failsafeState.active = false; + ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link mwDisarm(); + failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData + failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; + reprocessState = true; + break; + + case FAILSAFE_RX_LOSS_MONITORING: + // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time. + if (receivingRxData) { + if (millis() > failsafeState.receivingRxDataPeriod) { + // rx link is good now, when arming via ARM switch, it must be OFF first + if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { + DISABLE_ARMING_FLAG(PREVENT_ARMING); + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; + } + } + } else { + failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; + } + break; + + case FAILSAFE_RX_LOSS_RECOVERED: + // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. + // This is to prevent that JustDisarm is activated on the next iteration. + // Because that would have the effect of shutting down failsafe handling on intermittent connections. + failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.phase = FAILSAFE_IDLE; + failsafeState.active = false; + DISABLE_FLIGHT_MODE(FAILSAFE_MODE); + reprocessState = true; break; default: @@ -214,11 +275,3 @@ void failsafeUpdateState(void) beeper(beeperMode); } } - -/** - * Should be called once when RX data is processed by the system. - */ -void failsafeOnRxCycleStarted(void) -{ - failsafeState.counter++; -} diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index abe5707c87..4e2c4b52d6 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -18,26 +18,50 @@ #pragma once #define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5) +#define MILLIS_PER_TENTH_SECOND 100 +#define MILLIS_PER_SECOND 1000 +#define PERIOD_OF_1_SECONDS 1 * MILLIS_PER_SECOND +#define PERIOD_OF_3_SECONDS 3 * MILLIS_PER_SECOND +#define PERIOD_OF_30_SECONDS 30 * MILLIS_PER_SECOND +#define PERIOD_RXDATA_FAILURE 200 // millis +#define PERIOD_RXDATA_RECOVERY 200 // millis + typedef struct failsafeConfig_s { uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. + uint8_t failsafe_kill_switch; // failsafe switch action is 0: identical to rc link loss, 1: disarms instantly + uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure". } failsafeConfig_t; typedef enum { FAILSAFE_IDLE = 0, FAILSAFE_RX_LOSS_DETECTED, FAILSAFE_LANDING, - FAILSAFE_LANDED + FAILSAFE_LANDED, + FAILSAFE_RX_LOSS_MONITORING, + FAILSAFE_RX_LOSS_RECOVERED } failsafePhase_e; +typedef enum { + FAILSAFE_RXLINK_DOWN = 0, + FAILSAFE_RXLINK_UP +} failsafeRxLinkState_e; + typedef struct failsafeState_s { - int16_t counter; int16_t events; bool monitoring; bool active; + uint32_t rxDataFailurePeriod; + uint32_t validRxDataReceivedAt; + uint32_t validRxDataFailedAt; + uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period + uint32_t landingShouldBeFinishedAt; + uint32_t receivingRxDataPeriod; // period for the required period of valid rxData + uint32_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData failsafePhase_e phase; + failsafeRxLinkState_e rxLinkState; } failsafeState_t; void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse); @@ -51,7 +75,7 @@ bool failsafeIsActive(void); bool failsafeIsReceivingRxData(void); void failsafeOnValidDataReceived(void); -void failsafeOnRxCycleStarted(void); +void failsafeOnValidDataFailed(void); diff --git a/src/main/io/display.c b/src/main/io/display.c index f921d24cf1..e7e0a4d66f 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -231,6 +231,12 @@ void updateFailsafeStatus(void) case FAILSAFE_LANDED: failsafeIndicator = 'L'; break; + case FAILSAFE_RX_LOSS_MONITORING: + failsafeIndicator = 'M'; + break; + case FAILSAFE_RX_LOSS_RECOVERED: + failsafeIndicator = 'r'; + break; } i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0); i2c_OLED_send_char(failsafeIndicator); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 0249334264..8be2586484 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -47,6 +47,7 @@ typedef enum { BOXSERVO2, BOXSERVO3, BOXBLACKBOX, + BOXFAILSAFE, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eddbf960af..0863497364 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -433,6 +433,8 @@ const clivalue_t valueTable[] = { { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, 0, 200 }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, 0, 200 }, { "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX }, + { "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_kill_switch, 0, 1 }, + { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle_low_delay, 0, 300 }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, PWM_PULSE_MIN, PWM_PULSE_MAX }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, PWM_PULSE_MIN, PWM_PULSE_MAX }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index ce80091e04..b6e1c15784 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -131,7 +131,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -346,6 +346,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXSERVO2, "SERVO2;", 24 }, { BOXSERVO3, "SERVO3;", 25 }, { BOXBLACKBOX, "BLACKBOX;", 26 }, + { BOXFAILSAFE, "FAILSAFE;", 27 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -698,6 +699,10 @@ void mspInit(serialConfig_t *serialConfig) } #endif + if (feature(FEATURE_FAILSAFE)){ + activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; + } + memset(mspPorts, 0x00, sizeof(mspPorts)); mspAllocateSerialPorts(serialConfig); } @@ -819,7 +824,8 @@ static bool processOutCommand(uint8_t cmdMSP) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE | IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX; + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); if (flag) diff --git a/src/main/main.c b/src/main/main.c index a9e48be909..0357fa68c8 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -104,7 +104,7 @@ void telemetryInit(void); void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled); void mspInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig); -void failsafeInit(rxConfig_t *intialRxConfig); +void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle); pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); #ifdef USE_SERVOS void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers); @@ -404,7 +404,7 @@ void init(void) cliInit(&masterConfig.serialConfig); #endif - failsafeInit(&masterConfig.rxConfig); + failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig); diff --git a/src/main/mw.c b/src/main/mw.c index 5a376f8f23..d9c0d7b4ac 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -342,6 +342,9 @@ void mwArm(void) if (ARMING_FLAG(ARMED)) { return; } + if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { + return; + } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); headFreeModeHold = heading; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 78016c43bb..1af91509d3 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -441,10 +441,11 @@ static void detectAndApplySignalLossBehaviour(void) rxFlightChannelsValid = rxHaveValidFlightChannels(); - if (rxFlightChannelsValid) { + if ((rxFlightChannelsValid) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { failsafeOnValidDataReceived(); } else { rxSignalReceived = false; + failsafeOnValidDataFailed(); for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { rcData[channel] = getRxfailValue(channel); @@ -457,8 +458,6 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) { rxUpdateAt = currentTime + DELAY_50_HZ; - failsafeOnRxCycleStarted(); - if (!feature(FEATURE_RX_MSP)) { // rcData will have already been updated by MSP_SET_RAW_RC @@ -469,7 +468,6 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) readRxChannelsApplyRanges(); detectAndApplySignalLossBehaviour(); - } void parseRcChannels(const char *input, rxConfig_t *rxConfig) diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 66981e6056..cee78f6653 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -31,6 +31,7 @@ extern "C" { #include "config/runtime_config.h" #include "io/beeper.h" + #include "io/rc_controls.h" #include "rx/rx.h" #include "flight/failsafe.h" @@ -42,6 +43,9 @@ extern "C" { #include "gtest/gtest.h" uint32_t testFeatureMask = 0; +uint16_t flightModeFlags = 0; +uint16_t testMinThrottle = 0; +throttleStatus_e throttleStatus = THROTTLE_HIGH; enum { COUNTER_MW_DISARM = 0, @@ -56,24 +60,34 @@ void resetCallCounters(void) { memset(&callCounts, 0, sizeof(callCounts)); } -#define TEST_MID_RC 1495 // something other than the default 1500 will suffice. +#define TEST_MID_RC 1495 // something other than the default 1500 will suffice. +#define TEST_MIN_CHECK 1100; +#define PERIOD_OF_10_SCONDS 10000 +#define DE_ACTIVATE_ALL_BOXES 0 rxConfig_t rxConfig; failsafeConfig_t failsafeConfig; +uint32_t sysTickUptime; void configureFailsafe(void) { memset(&rxConfig, 0, sizeof(rxConfig)); rxConfig.midrc = TEST_MID_RC; + rxConfig.mincheck = TEST_MIN_CHECK; memset(&failsafeConfig, 0, sizeof(failsafeConfig)); failsafeConfig.failsafe_delay = 10; // 1 second failsafeConfig.failsafe_off_delay = 50; // 5 seconds + failsafeConfig.failsafe_kill_switch = false; + failsafeConfig.failsafe_throttle = 1200; + failsafeConfig.failsafe_throttle_low_delay = 50; // 5 seconds + sysTickUptime = 0; } // // Stepwise tests // +/****************************************************************************************/ TEST(FlightFailsafeTest, TestFailsafeInitialState) { // given @@ -91,6 +105,7 @@ TEST(FlightFailsafeTest, TestFailsafeInitialState) EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); } +/****************************************************************************************/ TEST(FlightFailsafeTest, TestFailsafeStartMonitoring) { // when @@ -102,14 +117,16 @@ TEST(FlightFailsafeTest, TestFailsafeStartMonitoring) EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); } +/****************************************************************************************/ TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle) { // given ENABLE_ARMING_FLAG(ARMED); // when - failsafeOnRxCycleStarted(); - failsafeOnValidDataReceived(); + failsafeOnValidDataFailed(); // set last invalid sample at current time + sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to + failsafeOnValidDataReceived(); // cause a recovered link // and failsafeUpdateState(); @@ -119,47 +136,11 @@ TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle) EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); } -/* - * FIXME failsafe assumes that calls to failsafeUpdateState() happen at a set frequency (50hz) - * but that is NOT the case when using a RX_SERIAL or RX_MSP as in that case the rx data is processed as soon - * as it arrives which may be more or less frequent. - * - * Since the failsafe uses a counter the counter would not be updated at the same frequency that the maths - * in the failsafe code is expecting the failsafe will either be triggered to early or too late when using - * RX_SERIAL or RX_MSP. - * - * uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) - * - * static bool failsafeHasTimerElapsed(void) - * { - * return failsafeState.counter > (5 * failsafeConfig->failsafe_delay); - * } - * - * static bool failsafeShouldHaveCausedLandingByNow(void) - * { - * return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay); - * } - * - * void failsafeOnValidDataReceived(void) - * { - * if (failsafeState.counter > 20) - * failsafeState.counter -= 20; - * else - * failsafeState.counter = 0; - * } - * - * 1000ms / 50hz = 20 - */ - -#define FAILSAFE_UPDATE_HZ 50 - +/****************************************************************************************/ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData) { // when - int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10; - - for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) { - failsafeOnRxCycleStarted(); + for (sysTickUptime = 0; sysTickUptime < PERIOD_OF_10_SCONDS; sysTickUptime++) { failsafeOnValidDataReceived(); failsafeUpdateState(); @@ -170,108 +151,213 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData) } } +/****************************************************************************************/ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding) { - // given ENABLE_ARMING_FLAG(ARMED); - // - // currently 20 cycles must occur before the lack of an update triggers RX loss detection. - // - // FIXME see comments about RX_SERIAL/RX_MSP above, the test should likely deal with time rather than counters. - int failsafeCounterThreshold = 20; + // and + failsafeStartMonitoring(); + throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure + sysTickUptime = 0; // restart time from 0 + failsafeOnValidDataReceived(); // set last valid sample at current time // when - for (int i = 0; i < failsafeCounterThreshold; i++) { - - failsafeOnRxCycleStarted(); - // no call to failsafeOnValidDataReceived(); + for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig.failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) { + failsafeOnValidDataFailed(); failsafeUpdateState(); // then + EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); - EXPECT_EQ(false, failsafeIsActive()); } - // when - for (int i = 0; i < FAILSAFE_UPDATE_HZ - failsafeCounterThreshold; i++) { - - failsafeOnRxCycleStarted(); - // no call to failsafeOnValidDataReceived(); - - failsafeUpdateState(); - - // then - EXPECT_EQ(FAILSAFE_RX_LOSS_DETECTED, failsafePhase()); - EXPECT_EQ(false, failsafeIsActive()); - } - - // - // one more cycle currently needed before the counter is re-checked. - // + // given + sysTickUptime++; // adjust time to point just past the failure time to + failsafeOnValidDataFailed(); // cause a lost link + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(FAILSAFE_LANDING, failsafePhase()); + EXPECT_EQ(true, failsafeIsActive()); +} + +/****************************************************************************************/ +TEST(FlightFailsafeTest, TestFailsafeCausesLanding) +{ + // given + sysTickUptime += failsafeConfig.failsafe_off_delay * MILLIS_PER_TENTH_SECOND; + sysTickUptime++; // when - failsafeOnRxCycleStarted(); // no call to failsafeOnValidDataReceived(); failsafeUpdateState(); // then EXPECT_EQ(true, failsafeIsActive()); - EXPECT_EQ(FAILSAFE_LANDING, failsafePhase()); -} - -TEST(FlightFailsafeTest, TestFailsafeCausesLanding) -{ - // given - int callsToMakeToSimulateFiveSeconds = FAILSAFE_UPDATE_HZ * 5; - - // when - for (int i = 0; i < callsToMakeToSimulateFiveSeconds - 1; i++) { - - failsafeOnRxCycleStarted(); - // no call to failsafeOnValidDataReceived(); - - failsafeUpdateState(); - - // then - EXPECT_EQ(FAILSAFE_LANDING, failsafePhase()); - EXPECT_EQ(true, failsafeIsActive()); - - } - - // when - failsafeOnRxCycleStarted(); - // no call to failsafeOnValidDataReceived(); - failsafeUpdateState(); - - // then - EXPECT_EQ(false, failsafeIsActive()); - EXPECT_EQ(FAILSAFE_LANDED, failsafePhase()); + EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); // given - DISABLE_ARMING_FLAG(ARMED); + failsafeOnValidDataFailed(); // set last invalid sample at current time + sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to + failsafeOnValidDataReceived(); // cause a recovered link + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(true, failsafeIsActive()); + EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); + EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + + // given + sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time + failsafeOnValidDataReceived(); // when - failsafeOnRxCycleStarted(); - // no call to failsafeOnValidDataReceived(); failsafeUpdateState(); // then EXPECT_EQ(false, failsafeIsActive()); - EXPECT_EQ(FAILSAFE_LANDED, failsafePhase()); + EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); - + EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); } +/****************************************************************************************/ +TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) +{ + // given + DISABLE_ARMING_FLAG(ARMED); + resetCallCounters(); + + // and + failsafeStartMonitoring(); + throttleStatus = THROTTLE_LOW; // throttle LOW to go for a failsafe just-disarm procedure + sysTickUptime = 0; // restart time from 0 + failsafeOnValidDataReceived(); // set last valid sample at current time + + // when + for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig.failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) { + failsafeOnValidDataFailed(); + + failsafeUpdateState(); + + // then + EXPECT_EQ(false, failsafeIsActive()); + EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); + } + + // given + sysTickUptime++; // adjust time to point just past the failure time to + failsafeOnValidDataFailed(); // cause a lost link + ENABLE_ARMING_FLAG(ARMED); // armed from here (disarmed state has cleared throttleLowPeriod). + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(true, failsafeIsActive()); + EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); + EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + + // given + failsafeOnValidDataFailed(); // set last invalid sample at current time + sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to + failsafeOnValidDataReceived(); // cause a recovered link + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(true, failsafeIsActive()); + EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); + EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + + // given + sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time + failsafeOnValidDataReceived(); + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(false, failsafeIsActive()); + EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. + EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); +} + +/****************************************************************************************/ +TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) +{ + // given + ENABLE_ARMING_FLAG(ARMED); + resetCallCounters(); + failsafeStartMonitoring(); + + // and + throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure + failsafeConfig.failsafe_kill_switch = 1; // configure AUX switch as kill switch + ACTIVATE_RC_MODE(BOXFAILSAFE); // and activate it + 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 + failsafeOnValidDataFailed(); // cause a lost link + + // when + failsafeUpdateState(); // kill switch handling should come first + + // then + EXPECT_EQ(true, failsafeIsActive()); + EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); + EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); + + // given + failsafeOnValidDataFailed(); // set last invalid sample at current time + 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) + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(true, failsafeIsActive()); + EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); + EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); + + // given + sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time + failsafeOnValidDataReceived(); + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(false, failsafeIsActive()); + EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. + EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); +} + +/****************************************************************************************/ // // Additional non-stepwise tests // - +/****************************************************************************************/ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected) { // given @@ -288,28 +374,58 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected failsafeStartMonitoring(); // and - int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10; + sysTickUptime = 0; // restart time from 0 + failsafeOnValidDataReceived(); // set last valid sample at current time - for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) { - failsafeOnRxCycleStarted(); - // no call to failsafeOnValidDataReceived(); + // when + for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) { + failsafeOnValidDataFailed(); failsafeUpdateState(); // then - EXPECT_EQ(true, failsafeIsMonitoring()); EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); } -} + // given + sysTickUptime++; // adjust time to point just past the failure time to + failsafeOnValidDataFailed(); // cause a lost link + + // when + failsafeUpdateState(); + + // then + EXPECT_EQ(true, failsafeIsMonitoring()); + EXPECT_EQ(false, failsafeIsActive()); + EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); + EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); + EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); +} // STUBS extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t armingFlags; +int16_t rcCommand[4]; +uint32_t rcModeActivationMask; int16_t debug[DEBUG16_VALUE_COUNT]; +bool isUsingSticksToArm = true; + + +// Return system uptime in milliseconds (rollover in 49 days) +uint32_t millis(void) +{ + return sysTickUptime; +} + +throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +{ + UNUSED(rxConfig); + UNUSED(deadband3d_throttle); + return throttleStatus; +} void delay(uint32_t) {} @@ -325,4 +441,26 @@ void beeper(beeperMode_e mode) { UNUSED(mode); } +uint16_t enableFlightMode(flightModeFlags_e mask) +{ + flightModeFlags |= (mask); + return flightModeFlags; +} + +uint16_t disableFlightMode(flightModeFlags_e mask) +{ + flightModeFlags &= ~(mask); + return flightModeFlags; +} + +uint16_t getCurrentMinthrottle(void) +{ + return testMinThrottle; +} + +bool isUsingSticksForArming(void) +{ + return isUsingSticksToArm; +} + } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 3c5bebccbc..073fed3244 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -298,6 +298,26 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle) TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp) { // given + controlRateConfig_t controlRateConfig = { + .rcRate8 = 90, + .rcExpo8 = 0, + .thrMid8 = 0, + .thrExpo8 = 0, + .rates = {0,0,0}, + .dynThrPID = 0, + .rcYawExpo8 = 0, + .tpa_breakpoint = 0 + }; + + // and + memset(&rxConfig, 0, sizeof (rxConfig)); + rxConfig.mincheck = DEFAULT_MIN_CHECK; + rxConfig.maxcheck = DEFAULT_MAX_CHECK; + rxConfig.midrc = 1500; + + // and + adjustmentStateMask = 0; + memset(&adjustmentStates, 0, sizeof(adjustmentStates)); configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig); // and diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 021a2ad90c..d559d52693 100755 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -31,7 +31,11 @@ extern "C" { #include "unittest_macros.h" #include "gtest/gtest.h" +#define DE_ACTIVATE_ALL_BOXES 0 + extern "C" { +uint32_t rcModeActivationMask; + extern uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range); } @@ -39,6 +43,8 @@ extern uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfig TEST(RxChannelRangeTest, TestRxChannelRanges) { + rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF + // No signal, special condition EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1000, 2000)), 0); EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1300, 1700)), 0); @@ -184,7 +190,7 @@ void failsafeOnValidDataReceived(void) { } -void failsafeOnRxCycleStarted(void) +void failsafeOnValidDataFailed(void) { } diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index c7cb0ad2cd..db3e0f2f11 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -25,6 +25,8 @@ extern "C" { #include "rx/rx.h" + uint32_t rcModeActivationMask; + void rxInit(rxConfig_t *rxConfig); void rxResetFlightChannelStatus(void); bool rxHaveValidFlightChannels(void); @@ -35,6 +37,8 @@ extern "C" { #include "unittest_macros.h" #include "gtest/gtest.h" +#define DE_ACTIVATE_ALL_BOXES 0 + typedef struct testData_s { bool isPPMDataBeingReceived; bool isPWMDataBeingReceived; @@ -46,6 +50,7 @@ TEST(RxTest, TestValidFlightChannels) { // given memset(&testData, 0, sizeof(testData)); + rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF // and rxConfig_t rxConfig; @@ -131,7 +136,7 @@ TEST(RxTest, TestInvalidFlightChannels) // STUBS extern "C" { - void failsafeOnRxCycleStarted() {} + void failsafeOnValidDataFailed() {} void failsafeOnValidDataReceived() {} bool feature(uint32_t mask) { From 0061457811a36b6a14b3011c798aed044aa64732 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 21 Aug 2015 00:24:01 +0100 Subject: [PATCH 11/15] Add COLIBRI_RACE to automated build system. --- .travis.yml | 1 + fake_travis_build.sh | 22 +++++++++++++++++----- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 41bdfec804..333cca912f 100755 --- a/.travis.yml +++ b/.travis.yml @@ -4,6 +4,7 @@ env: - PUBLISHDOCS=True - TARGET=CC3D - TARGET=CC3D OPBL=yes + - TARGET=COLIBRI_RACE - TARGET=CHEBUZZF3 - TARGET=CJMCU - TARGET=EUSTM32F103RC diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 4287b38460..3d2f160f2e 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -1,10 +1,22 @@ #!/bin/bash -targets=("PUBLISHMETA=True" "RUNTESTS=True" \ -"TARGET=CC3D" "TARGET=CC3D OPBL=yes" "TARGET=CHEBUZZF3" "TARGET=CJMCU" \ -"TARGET=EUSTM32F103RC" "TARGET=SPRACINGF3" "TARGET=NAZE" "TARGET=NAZE32PRO" \ -"TARGET=OLIMEXINO" "TARGET=PORT103R" "TARGET=SPARKY" "TARGET=STM32F3DISCOVERY" \ -"TARGET=ALIENWIIF1" "TARGET=ALIENWIIF3") +targets=("PUBLISHMETA=True" \ + "RUNTESTS=True" \ + "TARGET=CC3D" \ + "TARGET=CC3D OPBL=yes" \ + "TARGET=CHEBUZZF3" \ + "TARGET=CJMCU" \ + "TARGET=COLIBRI_RACE" \ + "TARGET=EUSTM32F103RC" \ + "TARGET=SPRACINGF3" \ + "TARGET=NAZE" \ + "TARGET=NAZE32PRO" \ + "TARGET=OLIMEXINO" \ + "TARGET=PORT103R" \ + "TARGET=SPARKY" \ + "TARGET=STM32F3DISCOVERY" \ + "TARGET=ALIENWIIF1" \ + "TARGET=ALIENWIIF3") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) From 84ae209c1e3ec25a9a16ae19106be879a361a5a0 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 5 Sep 2015 16:43:54 +0100 Subject: [PATCH 12/15] Update smix reverse documentation, closes #1140 --- docs/Mixer.md | 33 ++++++++++++++++++++------------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/docs/Mixer.md b/docs/Mixer.md index 7f5edcc38f..144156e4e8 100644 --- a/docs/Mixer.md +++ b/docs/Mixer.md @@ -127,6 +127,18 @@ Custom servo mixing rules can be applied to each servo. Rules are applied in th Note: the `smix` command may show a servo mix that is not active, custom servo mixes are only active for models that use custom mixers. +## Servo Reversing + +Servos are reversed using the `smix reverse` command. + +e.g. when using the TRI mixer to reverse the tail servo on a tricopter use this: + +`smix reverse 5 2 r` + +i.e. when mixing rudder servo slot (`5`) using Stabilised YAW input source (`2`) reverse the direction (`r`) + +`smix reverse` is a per-profile setting. So ensure you configure it for your profiles as required. + ### Example 1: A KK2.0 wired motor setup Here's an example of a X configuration quad, but the motors are still wired using the KK board motor numbering scheme. @@ -179,17 +191,12 @@ mmix 0 1.000 0.000 1.333 0.000 mmix 1 1.000 -1.000 -0.667 0.000 mmix 2 1.000 1.000 -0.667 0.000 smix reset -smix 0 6 3 100 0 0 100 0 +smix 0 5 2 100 0 0 100 0 +profile 0 +smix reverse 5 2 r +profile 1 +smix reverse 5 2 r +profile 2 +smix reverse 5 2 r + ``` - - -## Servo Reversing - -Servos are reversed using the `smix reverse` command. - -e.g. when using the TRI mixer to reverse the tail servo on a tricopter use this: - -`smix reverse 5 2 r` - -i.e. when mixing rudder servo slot (`5`) using Stabilised YAW input source (`2`) reverse the direction (`r`) - From df92691410cf8cf2b4639c09b14abd1cf4fe6ac7 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 5 Sep 2015 18:01:36 +0100 Subject: [PATCH 13/15] update dump command to show per-profile servo reverse settings. See #1140. --- src/main/io/serial_cli.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eddbf960af..8887f401bc 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1477,15 +1477,6 @@ static void cliDump(char *cmdline) ); } - // print servo directions - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - if (servoDirection(i, channel) < 0) { - printf("smix reverse %d %d r\r\n", i , channel); - } - } - } - #endif cliPrint("\r\n\r\n# feature\r\n"); @@ -1549,6 +1540,16 @@ static void cliDump(char *cmdline) cliServo(""); + // print servo directions + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + if (servoDirection(i, channel) < 0) { + printf("smix reverse %d %d r\r\n", i , channel); + } + } + } + + printSectionBreak(); dumpValues(PROFILE_VALUE); From 28aad588cb73e6db3846b0d3d703565c856603cb Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 5 Sep 2015 18:03:02 +0100 Subject: [PATCH 14/15] Fix vtail mixer defaults. Closes #988 Previously the vtail mixer was a copy of the atail mixer which was incorrect. The new defaults cater for frames like the Armattan Morphling Vtail. --- src/main/flight/mixer.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index cdb7960287..a0b27bc231 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -174,10 +174,10 @@ static const motorMixer_t mixerOctoFlatX[] = { }; static const motorMixer_t mixerVtail4[] = { - { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L + { 1.0f, -0.58f, 0.58f, 1.0f }, // REAR_R + { 1.0f, -0.46f, -0.39f, -0.5f }, // FRONT_R + { 1.0f, 0.58f, 0.58f, -1.0f }, // REAR_L + { 1.0f, 0.46f, -0.39f, 0.5f }, // FRONT_L }; static const motorMixer_t mixerAtail4[] = { From cb147ec85ebbeef92dd41ba9d143991d242d7dcd Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 5 Sep 2015 20:34:40 +0100 Subject: [PATCH 15/15] Fix CJMCU build, broken by df92691410cf8cf2b4639c09b14abd1cf4fe6ac7 --- src/main/io/serial_cli.c | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 42e16ac263..598fc73fce 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -111,8 +111,12 @@ static void cliRateProfile(char *cmdline); static void cliReboot(void); static void cliSave(char *cmdline); static void cliSerial(char *cmdline); + +#ifdef USE_SERVOS static void cliServo(char *cmdline); static void cliServoMix(char *cmdline); +#endif + static void cliSet(char *cmdline); static void cliGet(char *cmdline); static void cliStatus(char *cmdline); @@ -1056,11 +1060,9 @@ static void cliColor(char *cmdline) } #endif +#ifdef USE_SERVOS static void cliServo(char *cmdline) { -#ifndef USE_SERVOS - UNUSED(cmdline); -#else enum { SERVO_ARGUMENT_COUNT = 8 }; int16_t arguments[SERVO_ARGUMENT_COUNT]; @@ -1147,14 +1149,12 @@ static void cliServo(char *cmdline) servo->rate = arguments[6]; servo->forwardFromChannel = arguments[7]; } -#endif } +#endif +#ifdef USE_SERVOS static void cliServoMix(char *cmdline) { -#ifndef USE_SERVOS - UNUSED(cmdline); -#else int i; uint8_t len; char *ptr; @@ -1282,8 +1282,8 @@ static void cliServoMix(char *cmdline) cliShowParseError(); } } -#endif } +#endif #ifdef USE_FLASHFS @@ -1403,7 +1403,7 @@ static const char* const sectionBreak = "\r\n"; static void cliDump(char *cmdline) { - unsigned int i, channel; + unsigned int i; char buf[16]; uint32_t mask; @@ -1536,11 +1536,14 @@ static void cliDump(char *cmdline) cliRxRange(""); +#ifdef USE_SERVOS cliPrint("\r\n# servo\r\n"); cliServo(""); // print servo directions + unsigned int channel; + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { if (servoDirection(i, channel) < 0) { @@ -1548,7 +1551,7 @@ static void cliDump(char *cmdline) } } } - +#endif printSectionBreak();