diff --git a/src/main/config/config.c b/src/main/config/config.c index 46bf70e463..afdf373164 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -248,6 +248,8 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) void resetBatteryConfig(batteryConfig_t *batteryConfig) { batteryConfig->vbatscale = VBAT_SCALE_DEFAULT; + batteryConfig->vbatresdivval = VBAT_RESDIVVAL_DEFAULT; + batteryConfig->vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT; batteryConfig->vbatmaxcellvoltage = 43; batteryConfig->vbatmincellvoltage = 33; batteryConfig->vbatwarningcellvoltage = 35; diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 156fb6faac..33a04aac25 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -339,8 +339,15 @@ void ppmInConfig(const timerHardware_t *timerHardwarePtr) timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb); } -uint16_t pwmRead(uint8_t channel) +uint16_t ppmRead(uint8_t channel) { return captures[channel]; } +uint16_t pwmRead(uint8_t channel) +{ + uint16_t capture = captures[channel]; + captures[channel] = PPM_RCVR_TIMEOUT; + return capture; +} + diff --git a/src/main/drivers/pwm_rx.h b/src/main/drivers/pwm_rx.h index a81727f3c2..23ede57b01 100644 --- a/src/main/drivers/pwm_rx.h +++ b/src/main/drivers/pwm_rx.h @@ -30,6 +30,7 @@ void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel); uint16_t pwmRead(uint8_t channel); +uint16_t ppmRead(uint8_t channel); bool isPPMDataBeingReceived(void); void resetPPMDataReceivedState(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4801224bcc..fc37589f7b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -503,6 +503,10 @@ const clivalue_t valueTable[] = { { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, { "blackbox_device", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_device, 0, 1 }, #endif + + { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], -32768, 32767 }, + { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], -32768, 32767 }, + { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], -32768, 32767 }, }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 17ba6efaf8..894eb27a42 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -34,23 +34,29 @@ #include "rx/rx.h" #include "rx/pwm.h" -static uint16_t pwmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) +static uint16_t pwmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) { UNUSED(rxRuntimeConfigPtr); - return pwmRead(chan); + return pwmRead(channel); +} + +static uint16_t ppmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) +{ + UNUSED(rxRuntimeConfigPtr); + return ppmRead(channel); } void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfigPtr, rcReadRawDataPtr *callback) { UNUSED(rxRuntimeConfigPtr); // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled - *callback = pwmReadRawRC; - if (feature(FEATURE_RX_PARALLEL_PWM)) { rxRuntimeConfigPtr->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; + *callback = pwmReadRawRC; } if (feature(FEATURE_RX_PPM)) { rxRuntimeConfigPtr->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; + *callback = ppmReadRawRC; } } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 6a5dc32e3b..ae91db6ee8 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -96,7 +96,7 @@ void useRxConfig(rxConfig_t *rxConfigToUse) static uint8_t validFlightChannelMask; STATIC_UNIT_TESTED void rxResetFlightChannelStatus(void) { - validFlightChannelMask = 0; + validFlightChannelMask = REQUIRED_CHANNEL_MASK; } STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void) @@ -108,11 +108,11 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void) STATIC_UNIT_TESTED void rxUpdateFlightChannelStatus(uint8_t channel, uint16_t pulseDuration) { if (channel < NON_AUX_CHANNEL_COUNT && - pulseDuration >= rxConfig->rx_min_usec && - pulseDuration <= rxConfig->rx_max_usec + (pulseDuration < rxConfig->rx_min_usec || + pulseDuration > rxConfig->rx_max_usec) ) { - // if signal is valid - mark channel as OK - validFlightChannelMask |= (1 << channel); + // if signal is invalid - mark channel as BAD + validFlightChannelMask &= ~(1 << channel); } } @@ -135,6 +135,10 @@ void rxInit(rxConfig_t *rxConfig) rcData[i] = rxConfig->midrc; } + if (!feature(FEATURE_3D)) { + rcData[0] = rxConfig->rx_min_usec; + } + #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { serialRxInit(rxConfig); @@ -362,7 +366,7 @@ STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChanne void processRxChannels(void) { - uint8_t chan; + uint8_t channel; if (feature(FEATURE_RX_MSP)) { return; // rcData will have already been updated by MSP_SET_RAW_RC @@ -370,33 +374,39 @@ void processRxChannels(void) rxResetFlightChannelStatus(); - for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) { + for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { if (!rcReadRawFunc) { - rcData[chan] = getRxfailValue(chan); + rcData[channel] = getRxfailValue(channel); continue; } - uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, chan); + uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel); // apply the rx calibration - if (chan < NON_AUX_CHANNEL_COUNT) { - sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[chan]); + if (channel < NON_AUX_CHANNEL_COUNT) { + sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]); } - rxUpdateFlightChannelStatus(chan, sample); + rxUpdateFlightChannelStatus(channel, sample); + + if (!rxHaveValidFlightChannels()) { + // abort on first indication of control channel failure to prevent aux channel changes + // caused by rx's where aux channels are set to goto a predefined value on failsafe. + break; + } if (sample < rxConfig->rx_min_usec || sample > rxConfig->rx_max_usec || !rxSignalReceived) { - sample = getRxfailValue(chan); + sample = getRxfailValue(channel); } if (isRxDataDriven()) { - rcData[chan] = sample; + rcData[channel] = sample; } else { - rcData[chan] = calculateNonDataDrivenChannel(chan, sample); + rcData[channel] = calculateNonDataDrivenChannel(channel, sample); } } @@ -410,8 +420,12 @@ void processRxChannels(void) rxSignalReceived = false; } - for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) { - rcData[chan] = getRxfailValue(chan); + for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { + if (isRxDataDriven()) { + rcData[channel] = getRxfailValue(channel); + } else { + rcData[channel] = calculateNonDataDrivenChannel(channel, getRxfailValue(channel)); + } } } } diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 8df093b48f..5b8fe1e902 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -43,7 +43,8 @@ #define SUMD_BAUDRATE 115200 static bool sumdFrameDone = false; -static uint32_t sumdChannels[SUMD_MAX_CHANNEL]; +static uint16_t sumdChannels[SUMD_MAX_CHANNEL]; +static uint16_t crc; static void sumdDataReceive(uint16_t c); static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); @@ -67,6 +68,22 @@ bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return sumdPort != NULL; } +#define CRC_POLYNOME 0x1021 + +// CRC calculation, adds a 8 bit unsigned to 16 bit crc +static void CRC16(uint8_t value) +{ + uint8_t i; + + crc = crc ^ (int16_t)value << 8; + for (i = 0; i < 8; i++) { + if (crc & 0x8000) + crc = (crc << 1) ^ CRC_POLYNOME; + else + crc = (crc << 1); + } +} + static uint8_t sumd[SUMD_BUFFSIZE] = { 0, }; static uint8_t sumdChannelCount; @@ -86,17 +103,23 @@ static void sumdDataReceive(uint16_t c) if (c != SUMD_SYNCBYTE) return; else + { sumdFrameDone = false; // lazy main loop didnt fetch the stuff + crc = 0; + } } if (sumdIndex == 2) sumdChannelCount = (uint8_t)c; if (sumdIndex < SUMD_BUFFSIZE) sumd[sumdIndex] = (uint8_t)c; sumdIndex++; - if (sumdIndex == sumdChannelCount * 2 + 5) { - sumdIndex = 0; - sumdFrameDone = true; - } + if (sumdIndex < sumdChannelCount * 2 + 4) + CRC16((uint8_t)c); + else + if (sumdIndex == sumdChannelCount * 2 + 5) { + sumdIndex = 0; + sumdFrameDone = true; + } } #define SUMD_OFFSET_CHANNEL_1_HIGH 3 @@ -119,6 +142,10 @@ uint8_t sumdFrameStatus(void) sumdFrameDone = false; + // verify CRC + if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) | + (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW]))) + return frameStatus; switch (sumd[1]) { case SUMD_FRAME_STATE_FAILSAFE: diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index aaf1fd1895..58c4a77748 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -58,7 +58,7 @@ uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return ((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * 10); + return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier); } static void updateBatteryVoltage(void) diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 197ebe99cb..a836c7c1fe 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -20,6 +20,8 @@ #include "rx/rx.h" #define VBAT_SCALE_DEFAULT 110 +#define VBAT_RESDIVVAL_DEFAULT 10 +#define VBAT_RESDIVMULTIPLIER_DEFAULT 1 #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 @@ -32,6 +34,8 @@ typedef enum { typedef struct batteryConfig_s { uint8_t vbatscale; // adjust this to match battery voltage to reported value + uint8_t vbatresdivval; // resistor divider R2 (default NAZE 10(K)) + uint8_t vbatresdivmultiplier; // multiplier for scale (e.g. 2.5:1 ratio with multiplier of 4 can use '100' instead of '25' in ratio) to get better precision uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 948e76ec30..f7d9229463 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -44,6 +44,8 @@ TEST(BatteryTest, BatteryADCToVoltage) // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values: batteryConfig_t batteryConfig = { .vbatscale = VBAT_SCALE_DEFAULT, + .vbatresdivval = VBAT_RESDIVVAL_DEFAULT, + .vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT, .vbatmaxcellvoltage = 43, .vbatmincellvoltage = 33, .vbatwarningcellvoltage = 35, @@ -102,6 +104,8 @@ TEST(BatteryTest, BatteryState) // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values: batteryConfig_t batteryConfig = { .vbatscale = VBAT_SCALE_DEFAULT, + .vbatresdivval = VBAT_RESDIVVAL_DEFAULT, + .vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT, .vbatmaxcellvoltage = 43, .vbatmincellvoltage = 33, .vbatwarningcellvoltage = 35, @@ -159,6 +163,8 @@ TEST(BatteryTest, CellCount) // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values: batteryConfig_t batteryConfig = { .vbatscale = VBAT_SCALE_DEFAULT, + .vbatresdivval = VBAT_RESDIVVAL_DEFAULT, + .vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT, .vbatmaxcellvoltage = 43, .vbatmincellvoltage = 33, .vbatwarningcellvoltage = 35,