From 490268d2fca0dd405fcb193f2be46d86ee065902 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 3 Aug 2015 18:16:28 +0100 Subject: [PATCH] Failsafe - Allow AUX channels to HOLD their last value or be SET to a specific value when RX loss is detected. Tested with SBus, PWM & PPM. --- src/main/config/config.c | 5 ++++- src/main/io/serial_cli.c | 35 +++++++++++++++++++++++++++++++---- src/main/io/serial_msp.c | 17 ++++++++++------- src/main/rx/rx.c | 21 +++++++++++++++++---- src/main/rx/rx.h | 12 +++++++++++- 5 files changed, 73 insertions(+), 17 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 68f02c661..1af8599eb 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -404,7 +404,10 @@ static void resetConf(void) masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection for (i = 0; i < MAX_AUX_CHANNEL_COUNT; i++) { - masterConfig.rxConfig.rx_fail_usec_steps[i] = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_aux_channel_configurations[i]; + + channelFailsafeConfiguration->mode = RX_FAILSAFE_MODE_HOLD; + channelFailsafeConfiguration->step = 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 c402f2f89..400e1075d 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -571,11 +571,32 @@ static void cliRxFail(char *cmdline) cliRxFail(itoa(channel, buf, 10)); } } else { - uint16_t value; char *ptr = cmdline; channel = atoi(ptr++); if ((channel < MAX_AUX_CHANNEL_COUNT)) { + + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_aux_channel_configurations[channel]; + + uint16_t value; + rxFailsafeChannelMode_e mode; + + ptr = strchr(ptr, ' '); + if (ptr) { + switch (*(++ptr)) { + case 'h': + mode = RX_FAILSAFE_MODE_HOLD; + break; + + case 's': + mode = RX_FAILSAFE_MODE_SET; + break; + default: + cliShowParseError(); + return; + } + } + ptr = strchr(ptr, ' '); if (ptr) { value = atoi(++ptr); @@ -584,15 +605,21 @@ static void cliRxFail(char *cmdline) cliPrint("Value out of range\r\n"); return; } - masterConfig.rxConfig.rx_fail_usec_steps[channel] = value; + + channelFailsafeConfiguration->mode = mode; + channelFailsafeConfiguration->step = value; } + + char modeCharacter = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET ? 's' : 'h'; // triple use of printf below // 1. acknowledge interpretation on command, // 2. query current setting on single item, // 3. recursive use for full list. - printf("rxfail %u %d\r\n", + + printf("rxfail %u %c %d\r\n", channel, - RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.rx_fail_usec_steps[channel]) + modeCharacter, + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step) ); } else { printf("channel must be < %u\r\n", MAX_AUX_CHANNEL_COUNT); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 64c573f73..57962ef6b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1164,9 +1164,10 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RXFAIL_CONFIG: - headSerialReply(2 * (rxRuntimeConfig.channelCount-4)); - for (i = 4; i < rxRuntimeConfig.channelCount; i++) { - serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.rx_fail_usec_steps[i-4])); + headSerialReply(3 * (rxRuntimeConfig.channelCount - NON_AUX_CHANNEL_COUNT)); + for (i = NON_AUX_CHANNEL_COUNT; i < rxRuntimeConfig.channelCount; i++) { + serialize8(masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].mode); + serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].step)); } break; @@ -1597,12 +1598,14 @@ static bool processInCommand(void) case MSP_SET_RXFAIL_CONFIG: { - uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t); - if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT-4) { + uint8_t channelCount = currentPort->dataSize / 3; + if (channelCount > MAX_AUX_CHANNEL_COUNT) { headSerialError(0); } else { - for (i = 4; i < channelCount; i++) - masterConfig.rxConfig.rx_fail_usec_steps[i-4] = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); + for (i = NON_AUX_CHANNEL_COUNT; i < channelCount; i++) { + masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].mode = read8(); + masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); + } } } break; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 594eed9f1..f4603460b 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -319,11 +319,24 @@ static uint16_t getRxfailValue(uint8_t channel) case YAW: return rxConfig->midrc; case THROTTLE: - if (feature(FEATURE_3D)) return rxConfig->midrc; - else return rxConfig->rx_min_usec; - default: - return RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig->rx_fail_usec_steps[channel - NON_AUX_CHANNEL_COUNT]); + if (feature(FEATURE_3D)) + return rxConfig->midrc; + else + return rxConfig->rx_min_usec; } + + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_aux_channel_configurations[channel - NON_AUX_CHANNEL_COUNT]; + + switch(channelFailsafeConfiguration->mode) { + default: + case RX_FAILSAFE_MODE_HOLD: + return rcData[channel]; + + case RX_FAILSAFE_MODE_SET: + return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step); + + } + } static void processRxChannels(void) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index e73170c31..71d93b848 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -81,6 +81,16 @@ extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2 #define RSSI_SCALE_MAX 255 #define RSSI_SCALE_DEFAULT 30 +typedef enum { + RX_FAILSAFE_MODE_HOLD = 0, + RX_FAILSAFE_MODE_SET +} rxFailsafeChannelMode_e; + +typedef struct rxFailsafeChannelConfiguration_s { + uint8_t mode; // See rxFailsafeChannelMode_e + uint8_t step; +} rxFailsafeChannelConfiguration_t; + typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. @@ -94,7 +104,7 @@ typedef struct rxConfig_s { uint16_t rx_min_usec; uint16_t rx_max_usec; - uint8_t rx_fail_usec_steps[MAX_AUX_CHANNEL_COUNT]; + rxFailsafeChannelConfiguration_t failsafe_aux_channel_configurations[MAX_AUX_CHANNEL_COUNT]; } rxConfig_t;