From a30897c44016e5c443b3c8b353753d8ed10db61d Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 31 Jul 2015 23:03:55 +1000 Subject: [PATCH 1/6] Add ability to get/set mag calibration data --- src/main/io/serial_cli.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3d7e0d94a0..115d69a133 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -499,6 +499,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)) From 3879b6c5662f41f134360417487b7999d4ddb7b9 Mon Sep 17 00:00:00 2001 From: EvilBadger Date: Fri, 31 Jul 2015 12:33:30 +0100 Subject: [PATCH 2/6] Modified VBatt functionality to enable better precision. --- src/main/config/config.c | 2 ++ src/main/sensors/battery.c | 2 +- src/main/sensors/battery.h | 4 ++++ src/test/unit/battery_unittest.cc | 6 ++++++ 4 files changed, 13 insertions(+), 1 deletion(-) 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/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, From aaa7c7c5d3fde33fa66df433d7ca9c4d2cfaf53d Mon Sep 17 00:00:00 2001 From: ProDrone Date: Fri, 7 Aug 2015 16:24:25 +0200 Subject: [PATCH 3/6] SUMD channels are received as 16 bit values instead of 32 Preserve 32 bytes of RAM. --- src/main/rx/sumd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 8df093b48f..3e096a42f6 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -43,7 +43,7 @@ #define SUMD_BAUDRATE 115200 static bool sumdFrameDone = false; -static uint32_t sumdChannels[SUMD_MAX_CHANNEL]; +static uint16_t sumdChannels[SUMD_MAX_CHANNEL]; static void sumdDataReceive(uint16_t c); static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); From 95840ae512e09bc669e9dc964af6e890f2fd360e Mon Sep 17 00:00:00 2001 From: ProDrone Date: Thu, 6 Aug 2015 00:05:30 +0200 Subject: [PATCH 4/6] rcData[] is keeping the right values now. Logic for valid flightchannel detection is inverted in order to detect the first flightchannel failure instead of waiting to check them all. Clear PWM channel capture on read. This invalidates the control channels on read. They are validated by receiving a good value BEFORE the aux channels are received. This is done because control channels configures to go OFF on failsafe are detected with a PWM capture time-out. Time-out took so long that all aux channels where overwritten by their RX configured failsafe values BEFORE the invalid (=OFF) control channel was detected. --- src/main/drivers/pwm_rx.c | 4 +++- src/main/rx/rx.c | 48 +++++++++++++++++++++++++-------------- 2 files changed, 34 insertions(+), 18 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 156fb6faac..fcd5f87f0e 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -341,6 +341,8 @@ void ppmInConfig(const timerHardware_t *timerHardwarePtr) uint16_t pwmRead(uint8_t channel) { - return captures[channel]; + uint16_t capture = captures[channel]; + captures[channel] = PPM_RCVR_TIMEOUT; + return capture; } 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)); + } } } } From f26af1d844857a216c539d789a238116e77fcf3f Mon Sep 17 00:00:00 2001 From: ProDrone Date: Fri, 7 Aug 2015 19:36:01 +0200 Subject: [PATCH 5/6] Added CRC check to SUMD handler Officially the CRC check is part of the SUMD protocol. Framing errors are already covered and i expect these to occur around the same time as CRC errors, so i am not sure if someone will ever notice the difference... --- src/main/rx/sumd.c | 35 +++++++++++++++++++++++++++++++---- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 8df093b48f..68d3d84e4d 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -44,6 +44,7 @@ static bool sumdFrameDone = false; static uint32_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: From d2c40076db67d76ac4798d3b07e068ecf0089884 Mon Sep 17 00:00:00 2001 From: ProDrone Date: Wed, 12 Aug 2015 00:58:44 +0200 Subject: [PATCH 6/6] PPM and PWM now have their own ___ReadRawRC functions. Because a required change for PWM disturbed the PPM mode. --- src/main/drivers/pwm_rx.c | 5 +++++ src/main/drivers/pwm_rx.h | 1 + src/main/rx/pwm.c | 14 ++++++++++---- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index fcd5f87f0e..33a04aac25 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -339,6 +339,11 @@ void ppmInConfig(const timerHardware_t *timerHardwarePtr) timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb); } +uint16_t ppmRead(uint8_t channel) +{ + return captures[channel]; +} + uint16_t pwmRead(uint8_t channel) { uint16_t capture = captures[channel]; 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/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; } }