diff --git a/src/main/config/config.c b/src/main/config/config.c index c8be92c9f6..823f4542f5 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -403,6 +403,10 @@ static void resetConf(void) masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection + for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT-4; i++) { + masterConfig.rxConfig.rx_fail_usec_steps[i] = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); + } + masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3d7e0d94a0..a38b3f4fa8 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -97,6 +97,7 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort); static serialPort_t *cliPort; static void cliAux(char *cmdline); +static void cliRxFail(char *cmdline); static void cliAdjustmentRange(char *cmdline); static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); @@ -256,6 +257,7 @@ const clicmd_t cmdTable[] = { "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), + CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial), #ifdef USE_SERVOS @@ -558,6 +560,43 @@ static bool isEmpty(const char *string) return *string == '\0'; } +static void cliRxFail(char *cmdline) +{ + uint8_t channel; + char buf[3]; + + if (isEmpty(cmdline)) { + // print out rxConfig failsafe settings + for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT-4; channel++) { + cliRxFail(itoa(channel, buf, 10)); + } + } else { + uint16_t value; + char *ptr = cmdline; + + channel = atoi(ptr++); + if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT-4)) { + ptr = strchr(ptr, ' '); + if (ptr) { + value = atoi(++ptr); + value = CHANNEL_VALUE_TO_RXFAIL_STEP(value); + if (value > MAX_RXFAIL_RANGE_STEP) { + cliPrint("Value out of range\r\n"); + return; + } + masterConfig.rxConfig.rx_fail_usec_steps[channel] = value; + } + // 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", channel, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.rx_fail_usec_steps[channel])); + } else { + printf("channel must be < %u\r\n", MAX_SUPPORTED_RC_CHANNEL_COUNT-4); + } + } +} + static void cliAux(char *cmdline) { int i, val = 0; @@ -1379,6 +1418,9 @@ static void cliDump(char *cmdline) #endif printSectionBreak(); dumpValues(MASTER_VALUE); + + cliPrint("\r\n# rxfail\r\n"); + cliRxFail(""); } if (dumpMask & DUMP_PROFILE) { diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f9c0d4bb61..64c573f731 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -227,6 +227,10 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings #define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings + +#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings +#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // @@ -1159,6 +1163,13 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.failsafeConfig.failsafe_throttle); 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])); + } + break; + case MSP_RSSI_CONFIG: headSerialReply(1); serialize8(masterConfig.rxConfig.rssi_channel); @@ -1584,6 +1595,18 @@ static bool processInCommand(void) masterConfig.failsafeConfig.failsafe_throttle = read16(); break; + case MSP_SET_RXFAIL_CONFIG: + { + uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t); + if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT-4) { + headSerialError(0); + } else { + for (i = 4; i < channelCount; i++) + masterConfig.rxConfig.rx_fail_usec_steps[i-4] = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); + } + } + break; + case MSP_SET_RSSI_CONFIG: masterConfig.rxConfig.rssi_channel = read8(); break; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 40a2b3f8a3..cfc7ffe9a6 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -33,6 +33,7 @@ #include "drivers/serial.h" #include "drivers/adc.h" #include "io/serial.h" +#include "io/rc_controls.h" #include "flight/failsafe.h" @@ -67,6 +68,7 @@ uint16_t rssi = 0; // range: [0;1023] static bool rxDataReceived = false; static bool rxSignalReceived = false; static bool shouldCheckPulse = true; +static bool rxPwmFlightChannelsAreGood = false; static uint32_t rxUpdateAt = 0; static uint32_t needRxSignalBefore = 0; @@ -109,6 +111,7 @@ STATIC_UNIT_TESTED void rxCheckPulse(uint8_t channel, uint16_t pulseDuration) goodChannelMask = 0; failsafeOnValidDataReceived(); rxSignalReceived = true; + rxPwmFlightChannelsAreGood = true; } } @@ -308,6 +311,21 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT; } +static uint16_t getRxfailValue(uint8_t chan) +{ + switch (chan) { + case ROLL: + case PITCH: + 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[chan-4]); + } +} + static void processRxChannels(void) { uint8_t chan; @@ -316,10 +334,12 @@ static void processRxChannels(void) return; // rcData will have already been updated by MSP_SET_RAW_RC } + rxPwmFlightChannelsAreGood = false; + for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) { if (!rcReadRawFunc) { - rcData[chan] = rxConfig->midrc; + rcData[chan] = getRxfailValue(chan); continue; } @@ -332,9 +352,10 @@ static void processRxChannels(void) rxCheckPulse(chan, sample); } - // validate the range - if (sample < rxConfig->rx_min_usec || sample > rxConfig->rx_max_usec) - sample = rxConfig->midrc; + // validate the range and check if rx signal is received + if (sample < rxConfig->rx_min_usec || sample > rxConfig->rx_max_usec || !rxSignalReceived) { + sample = getRxfailValue(chan); + } if (isRxDataDriven()) { rcData[chan] = sample; @@ -342,6 +363,14 @@ static void processRxChannels(void) rcData[chan] = calculateNonDataDrivenChannel(chan, sample); } } + + // Using PARALLEL PWM and one of the 4 control channels is out of range: + // Probably one of the cables came loose, all channels are set to rxfail values + if ((rxPwmFlightChannelsAreGood == false) && feature(FEATURE_RX_PARALLEL_PWM)) { + for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) { + rcData[chan] = getRxfailValue(chan); + } + } } static void processDataDrivenRx(void) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 8321109cc4..372f6e14d7 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -27,6 +27,10 @@ #define PWM_PULSE_MIN 750 // minimum PWM pulse width which is considered valid #define PWM_PULSE_MAX 2250 // maximum PWM pulse width which is considered valid +#define RXFAIL_STEP_TO_CHANNEL_VALUE(step) (PWM_PULSE_MIN + 25 * step) +#define CHANNEL_VALUE_TO_RXFAIL_STEP(channelValue) ((constrain(channelValue, PWM_PULSE_MIN, PWM_PULSE_MAX) - PWM_PULSE_MIN) / 25) +#define MAX_RXFAIL_RANGE_STEP ((PWM_PULSE_MAX - PWM_PULSE_MIN) / 25) + #define DEFAULT_SERVO_MIN 1000 #define DEFAULT_SERVO_MIDDLE 1500 #define DEFAULT_SERVO_MAX 2000 @@ -90,6 +94,7 @@ typedef struct rxConfig_s { uint16_t rx_min_usec; uint16_t rx_max_usec; + uint8_t rx_fail_usec_steps[MAX_SUPPORTED_RC_CHANNEL_COUNT-4]; } rxConfig_t;