From 2e755ae5d630d2d77264eca7b3a3027b844d281a Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 17 Jul 2017 21:42:20 +0900 Subject: [PATCH 1/9] Add adc pin resource handling --- src/main/fc/cli.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 121729aa2..d673913ae 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3031,6 +3031,12 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_ESCSERIAL { OWNER_ESCSERIAL, PG_ESCSERIAL_CONFIG, offsetof(escSerialConfig_t, ioTag), 0 }, #endif +#ifdef USE_ADC + { OWNER_ADC_BATT, PG_ADC_CONFIG, offsetof(adcConfig_t, vbat.ioTag), 0 }, + { OWNER_ADC_RSSI, PG_ADC_CONFIG, offsetof(adcConfig_t, rssi.ioTag), 0 }, + { OWNER_ADC_CURR, PG_ADC_CONFIG, offsetof(adcConfig_t, current.ioTag), 0 }, + { OWNER_ADC_EXT, PG_ADC_CONFIG, offsetof(adcConfig_t, external1.ioTag), 0 }, +#endif }; static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) From 9ea8789869caa89c0f60ff748fd4e8370a9739d6 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 18 Jul 2017 15:08:51 +0900 Subject: [PATCH 2/9] For DYSF4PRO, use PC3 for RSSI ADC input The RSSI pad has direct connection to PC3. --- src/main/target/OMNIBUSF4/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 0b871810c..07ac74114 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -199,7 +199,11 @@ #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 // Direct from CRNT pad (part of onboard sensor for Pro) #define VBAT_ADC_PIN PC2 // 11:1 (10K + 1K) divider +#ifdef DYSF4PRO +#define RSSI_ADC_PIN PC3 // Direct from RSSI pad +#else #define RSSI_ADC_PIN PA0 // Direct from RSSI pad +#endif #define TRANSPONDER From 6869d789411900603927c3a3c5946cbe0a87e9a6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 18 Jul 2017 11:32:05 +0100 Subject: [PATCH 3/9] Better structure alignment to avoid padding --- src/main/drivers/accgyro/accgyro.h | 2 +- src/main/flight/failsafe.c | 2 +- src/main/flight/failsafe.h | 2 +- src/main/flight/pid.c | 4 ++-- src/main/flight/pid.h | 4 ++-- src/main/io/osd.h | 2 +- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index b92b2bb62..4900638e6 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -55,9 +55,9 @@ typedef struct gyroDev_s { extiCallbackRec_t exti; busDevice_t bus; float scale; // scalefactor - int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int32_t gyroZero[XYZ_AXIS_COUNT]; int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment + int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t temperature; uint8_t lpf; gyroRateKHz_e gyroRateKHz; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 445d260c4..180d3567c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -61,8 +61,8 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_delay = 10, // 1sec .failsafe_off_delay = 10, // 1sec .failsafe_throttle = 1000, // default throttle off. - .failsafe_kill_switch = 0, // default failsafe switch action is identical to rc link loss .failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition + .failsafe_kill_switch = 0, // default failsafe switch action is identical to rc link loss .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT // default full failsafe procedure is 0: auto-landing ); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d11d61421..82392e98e 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -33,8 +33,8 @@ 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". + uint8_t failsafe_kill_switch; // failsafe switch action is 0: identical to rc link loss, 1: disarms instantly uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it } failsafeConfig_t; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b1929cc5e..16d383745 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -90,11 +90,11 @@ void resetPidProfile(pidProfile_t *pidProfile) .pidSumLimit = PIDSUM_LIMIT, .pidSumLimitYaw = PIDSUM_LIMIT_YAW, .yaw_lpf_hz = 0, - .itermWindupPointPercent = 50, - .dterm_filter_type = FILTER_BIQUAD, .dterm_lpf_hz = 100, // filtering ON by default .dterm_notch_hz = 260, .dterm_notch_cutoff = 160, + .dterm_filter_type = FILTER_BIQUAD, + .itermWindupPointPercent = 50, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 1b31f5475..f6de2aa12 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -74,11 +74,11 @@ typedef struct pid8_s { typedef struct pidProfile_s { pid8_t pid[PID_ITEM_COUNT]; - uint8_t dterm_filter_type; // Filter selection for dterm - uint16_t dterm_lpf_hz; // Delta Filter in hz uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy + uint16_t dterm_lpf_hz; // Delta Filter in hz uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff + uint8_t dterm_filter_type; // Filter selection for dterm uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation uint16_t pidSumLimit; uint16_t pidSumLimitYaw; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7f9ae5434..795cfde38 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -131,9 +131,9 @@ typedef struct osdConfig_s { bool enabled_stats[OSD_STAT_COUNT]; // Alarms - uint8_t rssi_alarm; uint16_t cap_alarm; uint16_t alt_alarm; + uint8_t rssi_alarm; osd_unit_e units; From 3385884e563385176a2d273531d3a2da6da68a28 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 18 Jul 2017 12:32:28 +0100 Subject: [PATCH 4/9] Removed mpuConfiguration.gyroReadXRegister --- src/main/drivers/accgyro/accgyro_mpu.c | 9 +-------- src/main/drivers/accgyro/accgyro_mpu.h | 1 - src/main/drivers/accgyro/accgyro_mpu3050.c | 18 +++++++++++++++++- 3 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 2323a095e..b7532b154 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -206,7 +206,7 @@ bool mpuGyroRead(gyroDev_t *gyro) { uint8_t data[6]; - const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, gyro->mpuConfiguration.gyroReadXRegister, 6, data); + const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_GYRO_XOUT_H, 6, data); if (!ack) { return false; } @@ -253,7 +253,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = mpu6000SpiDetect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; @@ -271,7 +270,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; @@ -288,7 +286,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = mpu9250SpiDetect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; @@ -306,7 +303,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = icm20689SpiDetect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; @@ -354,15 +350,12 @@ void mpuDetect(gyroDev_t *gyro) return; } - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - // If an MPU3050 is connected sig will contain 0. uint8_t inquiryResult; ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_3050; - gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; return; } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 6fe8b0836..2825120e6 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -135,7 +135,6 @@ typedef struct mpuConfiguration_s { mpuReadRegisterFnPtr readFn; mpuWriteRegisterFnPtr writeFn; mpuResetFnPtr resetFn; - uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each } mpuConfiguration_t; enum gyro_fsr_e { diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c index 1b8acd19a..10d1f0e68 100644 --- a/src/main/drivers/accgyro/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro/accgyro_mpu3050.c @@ -64,6 +64,22 @@ static void mpu3050Init(gyroDev_t *gyro) gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); } +static bool mpu3050GyroRead(gyroDev_t *gyro) +{ + uint8_t data[6]; + + const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_GYRO_OUT, 6, data); + if (!ack) { + return false; + } + + gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]); + gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]); + gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]); + + return true; +} + static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) { uint8_t buf[2]; @@ -82,7 +98,7 @@ bool mpu3050Detect(gyroDev_t *gyro) return false; } gyro->initFn = mpu3050Init; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpu3050GyroRead; gyro->temperatureFn = mpu3050ReadTemperature; // 16.4 dps/lsb scalefactor From 2636ffddb9c15fb05d6b5226243739cf1f14a304 Mon Sep 17 00:00:00 2001 From: Andrey Mironov Date: Tue, 18 Jul 2017 17:55:08 +0300 Subject: [PATCH 5/9] Renamed occurrences of CAMERA_CONTROL to USE_CAMERA_CONTROL --- src/main/fc/cli.c | 2 +- src/main/fc/fc_init.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 622602e19..25d0c1954 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3032,7 +3032,7 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_ESCSERIAL { OWNER_ESCSERIAL, PG_ESCSERIAL_CONFIG, offsetof(escSerialConfig_t, ioTag), 0 }, #endif -#ifdef CAMERA_CONTROL +#ifdef USE_CAMERA_CONTROL { OWNER_CAMERA_CONTROL, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, ioTag), 0 }, #endif }; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 3d3897c7b..dab033672 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -470,7 +470,7 @@ void init(void) rtc6705IOInit(); #endif -#ifdef CAMERA_CONTROL +#ifdef USE_CAMERA_CONTROL cameraControlInit(); #endif From 84b828a93c1c8eed14b281a4a32688c934b9f341 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 19 Jul 2017 00:50:05 +0900 Subject: [PATCH 6/9] Target configuration improvements 1. Add timer entries for UART1 pins so that unused side, more specifically TX side when Serial RX is used, can be used as a timer for other uses such as software serial. 2. Delete AVOID_UART1_FOR_PWM_PPM so that - OMNIBUSF4/F4SD users will have an option to use UART1 with SBUS/PPM jumper mod, and - DYSF4PRO users to provide UART1 even when PPM is used. 3. Tidy on S6_OUT (consolidation) --- src/main/target/OMNIBUSF4/target.c | 6 +++--- src/main/target/OMNIBUSF4/target.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index e194ed6f3..27bbf82d8 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -46,11 +46,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #if defined(OMNIBUSF4SD) DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) #else DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT #endif + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT + DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, TIMER_OUTPUT_NONE, 0), // UART1_TX + DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, TIMER_OUTPUT_NONE, 0), // UART1_RX }; - diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 07ac74114..6cf9fecfb 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -210,7 +210,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_OSD) -#define AVOID_UART1_FOR_PWM_PPM + #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) @@ -219,8 +219,8 @@ #define TARGET_IO_PORTD BIT(2) #ifdef OMNIBUSF4SD -#define USABLE_TIMER_CHANNEL_COUNT 13 +#define USABLE_TIMER_CHANNEL_COUNT 15 #else -#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USABLE_TIMER_CHANNEL_COUNT 14 #endif #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) From 9b189865d1d192a15f201b880218331dcd13abb7 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sat, 15 Jul 2017 08:25:39 +0200 Subject: [PATCH 7/9] MSP changes for the new Power & Battery tab in configurator --- src/main/fc/fc_msp.c | 93 +++++++++++++++++++++++++++++--------------- 1 file changed, 61 insertions(+), 32 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 666319f9c..bb836d1cf 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -706,7 +706,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_VOLTAGE_METERS: // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount; i++) { + for (int i = 0; i < supportedVoltageMeterCount - (12-getMotorCount()); i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -719,7 +719,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_CURRENT_METERS: // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount; i++) { + for (int i = 0; i < supportedCurrentMeterCount - (12-getMotorCount()); i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; @@ -2131,46 +2131,75 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif case MSP_SET_VOLTAGE_METER_CONFIG: { - int id = sbufReadU8(src); + int8_t config_count = sbufReadU8(src); - // - // find and configure an ADC voltage sensor - // - int voltageSensorADCIndex; - for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { - if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { - break; + while (config_count > 0) { + + int8_t subframe_length = sbufReadU8(src); + if (subframe_length > 4) { + for (int8_t j = 0; j < subframe_length; j++) { + sbufReadU8(src); + } + } else { + int8_t id = sbufReadU8(src); + + // + // find and configure an ADC voltage sensor + // + int8_t voltageSensorADCIndex; + for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { + if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { + break; + } + } + + if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); + } else { + // if we had any other types of voltage sensor to configure, this is where we'd do it. + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + } } - } - - if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); - } else { - // if we had any other types of voltage sensor to configure, this is where we'd do it. - return -1; + config_count--; } break; } case MSP_SET_CURRENT_METER_CONFIG: { - int id = sbufReadU8(src); + int8_t config_count = sbufReadU8(src); - switch (id) { - case CURRENT_METER_ID_BATTERY_1: - currentSensorADCConfigMutable()->scale = sbufReadU16(src); - currentSensorADCConfigMutable()->offset = sbufReadU16(src); - break; + while (config_count > 0) { + + int8_t subframe_length = sbufReadU8(src); + if (subframe_length > 5) { + for (int8_t j = 0; j < subframe_length; j++) { + sbufReadU8(src); + } + } else { + int id = sbufReadU8(src); + + switch (id) { + case CURRENT_METER_ID_BATTERY_1: + currentSensorADCConfigMutable()->scale = sbufReadU16(src); + currentSensorADCConfigMutable()->offset = sbufReadU16(src); + break; #ifdef USE_VIRTUAL_CURRENT_METER - case CURRENT_METER_ID_VIRTUAL_1: - currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); - currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); - break; + case CURRENT_METER_ID_VIRTUAL_1: + currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); + currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); + break; #endif - - default: - return -1; + default: + sbufReadU16(src); + sbufReadU16(src); + break; + } + } + config_count--; } break; From f61bb3aac8759b6120585017f5a9e8b3955bb274 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sun, 16 Jul 2017 21:17:21 +0200 Subject: [PATCH 8/9] Replaced magic number with define VOLTAGE_METER_ID_ESC_COUNT --- src/main/fc/fc_msp.c | 4 ++-- src/main/sensors/voltage.h | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bb836d1cf..5b2c0facb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -706,7 +706,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_VOLTAGE_METERS: // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount - (12-getMotorCount()); i++) { + for (int i = 0; i < supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -719,7 +719,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_CURRENT_METERS: // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount - (12-getMotorCount()); i++) { + for (int i = 0; i < supportedCurrentMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index 4011e6714..0c8d3d682 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -60,6 +60,8 @@ typedef enum { #define MAX_VOLTAGE_SENSOR_ADC 1 // VBAT - some boards have external, 12V, 9V and 5V meters. #endif +#define VOLTAGE_METER_ID_ESC_COUNT 12 + typedef enum { VOLTAGE_SENSOR_ADC_VBAT = 0, VOLTAGE_SENSOR_ADC_12V = 1, From 3c917794bffdeaf2cb2151ee2c077479a928bb09 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Tue, 18 Jul 2017 22:20:52 +0200 Subject: [PATCH 9/9] Changes according review comments & fixed linker error SPRACINGF3OSD --- src/main/fc/fc_msp.c | 112 +++++++++++++++++++------------------------ 1 file changed, 48 insertions(+), 64 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5b2c0facb..7799082fb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -704,9 +704,14 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce break; } - case MSP_VOLTAGE_METERS: + case MSP_VOLTAGE_METERS: { // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { + uint8_t count = supportedVoltageMeterCount; +#ifndef USE_OSD_SLAVE + count = supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); +#endif + + for (int i = 0; i < count; i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -716,10 +721,15 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255)); } break; + } - case MSP_CURRENT_METERS: + case MSP_CURRENT_METERS: { // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { + uint8_t count = supportedVoltageMeterCount; +#ifndef USE_OSD_SLAVE + count = supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); +#endif + for (int i = 0; i < count; i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; @@ -730,6 +740,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero } break; + } case MSP_VOLTAGE_METER_CONFIG: // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter, @@ -2131,77 +2142,50 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif case MSP_SET_VOLTAGE_METER_CONFIG: { - int8_t config_count = sbufReadU8(src); + int8_t id = sbufReadU8(src); - while (config_count > 0) { - - int8_t subframe_length = sbufReadU8(src); - if (subframe_length > 4) { - for (int8_t j = 0; j < subframe_length; j++) { - sbufReadU8(src); - } - } else { - int8_t id = sbufReadU8(src); - - // - // find and configure an ADC voltage sensor - // - int8_t voltageSensorADCIndex; - for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { - if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { - break; - } - } - - if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); - } else { - // if we had any other types of voltage sensor to configure, this is where we'd do it. - sbufReadU8(src); - sbufReadU8(src); - sbufReadU8(src); - } + // + // find and configure an ADC voltage sensor + // + int8_t voltageSensorADCIndex; + for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { + if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { + break; } - config_count--; + } + + if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); + } else { + // if we had any other types of voltage sensor to configure, this is where we'd do it. + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); } break; } case MSP_SET_CURRENT_METER_CONFIG: { - int8_t config_count = sbufReadU8(src); + int id = sbufReadU8(src); - while (config_count > 0) { - - int8_t subframe_length = sbufReadU8(src); - if (subframe_length > 5) { - for (int8_t j = 0; j < subframe_length; j++) { - sbufReadU8(src); - } - } else { - int id = sbufReadU8(src); - - switch (id) { - case CURRENT_METER_ID_BATTERY_1: - currentSensorADCConfigMutable()->scale = sbufReadU16(src); - currentSensorADCConfigMutable()->offset = sbufReadU16(src); - break; + switch (id) { + case CURRENT_METER_ID_BATTERY_1: + currentSensorADCConfigMutable()->scale = sbufReadU16(src); + currentSensorADCConfigMutable()->offset = sbufReadU16(src); + break; #ifdef USE_VIRTUAL_CURRENT_METER - case CURRENT_METER_ID_VIRTUAL_1: - currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); - currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); - break; + case CURRENT_METER_ID_VIRTUAL_1: + currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); + currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); + break; #endif - default: - sbufReadU16(src); - sbufReadU16(src); - break; - } - } - config_count--; + default: + sbufReadU16(src); + sbufReadU16(src); + break; } - break; }