From 70f97be4cc5aa29291d5deb8a41c15cbda60de5a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 5 Nov 2016 14:14:25 +0000 Subject: [PATCH 01/51] Fixed up linker scripts in preparation for parameter groups --- src/main/target/link/stm32_flash_f103_128k.ld | 8 ++++--- .../target/link/stm32_flash_f103_128k_opbl.ld | 8 ++++--- src/main/target/link/stm32_flash_f103_256k.ld | 8 ++++--- src/main/target/link/stm32_flash_f103_64k.ld | 8 ++++--- src/main/target/link/stm32_flash_f303_128k.ld | 8 ++++--- src/main/target/link/stm32_flash_f303_256k.ld | 8 ++++--- src/main/target/link/stm32_flash_f405.ld | 10 ++++++++- src/main/target/link/stm32_flash_f405_opbl.ld | 21 ++++++++++--------- src/main/target/link/stm32_flash_f411.ld | 14 +++++++------ src/main/target/link/stm32_flash_f411_opbl.ld | 16 +++++++------- src/main/target/link/stm32_flash_f745.ld | 4 +++- 11 files changed, 70 insertions(+), 43 deletions(-) diff --git a/src/main/target/link/stm32_flash_f103_128k.ld b/src/main/target/link/stm32_flash_f103_128k.ld index 48441c237c..eaeaa48843 100644 --- a/src/main/target/link/stm32_flash_f103_128k.ld +++ b/src/main/target/link/stm32_flash_f103_128k.ld @@ -12,9 +12,11 @@ /* Specify the memory areas. */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */ - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K + FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f103_128k_opbl.ld b/src/main/target/link/stm32_flash_f103_128k_opbl.ld index a8fad3a5f4..c25b881d98 100644 --- a/src/main/target/link/stm32_flash_f103_128k_opbl.ld +++ b/src/main/target/link/stm32_flash_f103_128k_opbl.ld @@ -12,10 +12,12 @@ /* Specify the memory areas. */ MEMORY { - FLASH (rx) : ORIGIN = 0x08003000, LENGTH = 126K - 0x03000 /* last 2kb used for config storage first 12k for OP Bootloader*/ + /* First 12K (0x3000 bytes) used for OP Bootloader, last 2K used for config storage */ + FLASH (rx) : ORIGIN = 0x08003000, LENGTH = 114K + FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f103_256k.ld b/src/main/target/link/stm32_flash_f103_256k.ld index 0f0c2fe88c..b95a15f070 100644 --- a/src/main/target/link/stm32_flash_f103_256k.ld +++ b/src/main/target/link/stm32_flash_f103_256k.ld @@ -12,9 +12,11 @@ /* Specify the memory areas. */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */ - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 48K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K + FLASH_CONFIG (r) : ORIGIN = 0x0803F000, LENGTH = 4K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 48K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f103_64k.ld b/src/main/target/link/stm32_flash_f103_64k.ld index a72e2b6082..32ac49552f 100644 --- a/src/main/target/link/stm32_flash_f103_64k.ld +++ b/src/main/target/link/stm32_flash_f103_64k.ld @@ -12,9 +12,11 @@ /* Specify the memory areas. */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 62K /* last 2kb used for config storage */ - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 62K + FLASH_CONFIG (r) : ORIGIN = 0x0800F800, LENGTH = 2K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f303_128k.ld b/src/main/target/link/stm32_flash_f303_128k.ld index a13c304581..0050c64b5d 100644 --- a/src/main/target/link/stm32_flash_f303_128k.ld +++ b/src/main/target/link/stm32_flash_f303_128k.ld @@ -12,9 +12,11 @@ /* Specify the memory areas. */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */ - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K + FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld index 2db0e7713f..0d71ef1261 100644 --- a/src/main/target/link/stm32_flash_f303_256k.ld +++ b/src/main/target/link/stm32_flash_f303_256k.ld @@ -12,9 +12,11 @@ /* Specify the memory areas. */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */ - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K + FLASH_CONFIG (r) : ORIGIN = 0x0803F000, LENGTH = 4K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld index a792a296c6..7bd4c75660 100644 --- a/src/main/target/link/stm32_flash_f405.ld +++ b/src/main/target/link/stm32_flash_f405.ld @@ -12,10 +12,18 @@ /* Entry Point */ ENTRY(Reset_Handler) +/* +0x08000000 to 0x08100000 1024K full flash, +0x08000000 to 0x080FC000 1008K firmware, +0x080FC000 to 0x08100000 16K config, +*/ + /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1M + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1008K + FLASH_CONFIG (r) : ORIGIN = 0x080FC000, LENGTH = 16K + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld index a9e2b7dd84..4fc707c55c 100644 --- a/src/main/target/link/stm32_flash_f405_opbl.ld +++ b/src/main/target/link/stm32_flash_f405_opbl.ld @@ -12,21 +12,22 @@ /* Entry Point */ ENTRY(Reset_Handler) -/* Specify the memory areas */ - /* -0x08000000 to 0x08100000 1024kb full flash, -0x08000000 to 0x08020000 128kb OPBL, -0x08020000 to 0x08080000 384kb firmware, -0x08080000 to 0x080A0000 128kb config, +0x08000000 to 0x08100000 1024K full flash, +0x08000000 to 0x08004000 16K OPBL, +0x08004000 to 0x080FC000 992K firmware, +0x080FC000 to 0x08100000 16K config, */ +/* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 0x000A0000 - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 0x00020000 - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 992K + FLASH_CONFIG (r): ORIGIN = 0x080FC000, LENGTH = 16K + + CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld index d145ff3afc..789c757e9a 100644 --- a/src/main/target/link/stm32_flash_f411.ld +++ b/src/main/target/link/stm32_flash_f411.ld @@ -13,17 +13,19 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x08080000 512kb full flash, -0x08000000 to 0x08060000 384kb firmware, -0x08060000 to 0x08080000 128kb config, +0x08000000 to 0x08080000 512K full flash, +0x08000000 to 0x0807C000 469K firmware, +0x0807C000 to 0x08080000 16K config, */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x00080000 - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 496K + FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 16K + + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld index 5787e6d20b..5658d5e797 100644 --- a/src/main/target/link/stm32_flash_f411_opbl.ld +++ b/src/main/target/link/stm32_flash_f411_opbl.ld @@ -13,18 +13,20 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x08080000 512kb full flash, -0x08000000 to 0x08010000 64kb OPBL, -0x08010000 to 0x08060000 320kb firmware, -0x08060000 to 0x08080000 128kb config, +0x08000000 to 0x08080000 512K full flash, +0x08000000 to 0x08004000 16K OPBL, +0x08010000 to 0x0807C000 480K firmware, +0x0807C000 to 0x08080000 16K config, */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 0x00070000 - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 480K + FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 16K + + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld index 62270ea0f0..6c60e5c2c8 100644 --- a/src/main/target/link/stm32_flash_f745.ld +++ b/src/main/target/link/stm32_flash_f745.ld @@ -15,7 +15,9 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1M + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1008K + FLASH_CONFIG (r) : ORIGIN = 0x080FC000, LENGTH = 16K + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 320K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } From 5df102d6f5d5c4e2c853413fd697d453735dec38 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 20 Nov 2016 18:56:18 +1300 Subject: [PATCH 02/51] Adjusted current meter setting ranges to allow for negative offset. Also added biquad filter for current and did some cleanup. --- src/main/io/serial_cli.c | 4 +- src/main/sensors/battery.c | 111 ++++++++++++++++++++++--------------- src/main/sensors/battery.h | 2 +- 3 files changed, 69 insertions(+), 48 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 526bcb6cf4..9cda846bbb 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -789,8 +789,8 @@ const clivalue_t valueTable[] = { { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } }, - { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } }, - { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } }, + { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -16000, 16000 } }, + { "current_meter_offset", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { -16000, 16000 } }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, { "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.batterynotpresentlevel, .config.minmax = { 0, 200 } }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index dee83f43ff..2250460a31 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -44,7 +44,12 @@ #include "common/utils.h" -#define VBATT_LPF_FREQ 0.4f +#define VBAT_LPF_FREQ 0.4f +#define IBAT_LPF_FREQ 0.4f + +#define VBAT_STABLE_MAX_DELTA 2 + +#define ADCVREF 3300 // in mV // Battery monitoring stuff uint8_t batteryCellCount; @@ -72,32 +77,35 @@ static void updateBatteryVoltage(void) static biquadFilter_t vbatFilter; static bool vbatFilterIsInitialised; - // store the battery voltage with some other recent battery voltage readings - uint16_t vbatSample; + if (!vbatFilterIsInitialised) { + biquadFilterInitLPF(&vbatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update + vbatFilterIsInitialised = true; + } #ifdef USE_ESC_TELEMETRY if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) { - vbatSample = vbatLatest = getEscTelemetryVbat(); + vbatLatest = getEscTelemetryVbat(); + if (debugMode == DEBUG_BATTERY) { + debug[0] = -1; + } + vbat = biquadFilterApply(&vbatFilter, vbatLatest); } else #endif { - vbatSample = vbatLatest = batteryAdcToVoltage(adcGetChannel(ADC_BATTERY)); + uint16_t vbatSample = adcGetChannel(ADC_BATTERY); + if (debugMode == DEBUG_BATTERY) { + debug[0] = vbatSample; + } + vbat = batteryAdcToVoltage(biquadFilterApply(&vbatFilter, vbatSample)); + vbatLatest = batteryAdcToVoltage(vbatSample); } - if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; - - if (!vbatFilterIsInitialised) { - biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update - vbatFilterIsInitialised = true; + if (debugMode == DEBUG_BATTERY) { + debug[1] = vbat; } - vbat = biquadFilterApply(&vbatFilter, vbatSample); - - if (debugMode == DEBUG_BATTERY) debug[1] = vbat; } -#define VBAT_STABLE_MAX_DELTA 2 - void updateBattery(void) { uint16_t vbatPrevious = vbatLatest; @@ -125,11 +133,12 @@ void updateBattery(void) batteryCriticalVoltage = 0; } - if (debugMode == DEBUG_BATTERY) debug[2] = batteryState; - if (debugMode == DEBUG_BATTERY) debug[3] = batteryCellCount; + if (debugMode == DEBUG_BATTERY) { + debug[2] = batteryState; + debug[3] = batteryCellCount; + } - switch(batteryState) - { + switch(batteryState) { case BATTERY_OK: if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { batteryState = BATTERY_WARNING; @@ -180,7 +189,6 @@ void batteryInit(batteryConfig_t *initialBatteryConfig) batteryCriticalVoltage = 0; } -#define ADCVREF 3300 // in mV static int32_t currentSensorToCentiamps(uint16_t src) { int32_t millivolts; @@ -191,50 +199,63 @@ static int32_t currentSensorToCentiamps(uint16_t src) return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps } +static void updateBatteryCurrent(void) +{ + static biquadFilter_t ibatFilter; + static bool ibatFilterIsInitialised; + + if (!ibatFilterIsInitialised) { + biquadFilterInitLPF(&ibatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update + ibatFilterIsInitialised = true; + } + + uint16_t ibatSample = adcGetChannel(ADC_CURRENT); + amperage = currentSensorToCentiamps(biquadFilterApply(&ibatFilter, ibatSample)); +} + +static void updateCurrentDrawn(int32_t lastUpdateAt) +{ + int mAhDrawnRaw = (amperage * lastUpdateAt) / 1000; + mAhDrawn = mAhDrawn + mAhDrawnRaw / (3600 * 100); +} + void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { - #ifdef USE_ESC_TELEMETRY - UNUSED(lastUpdateAt); - #endif - - static int64_t mAhdrawnRaw = 0; - static int32_t amperageRaw = 0; - - int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; - int32_t throttleFactor = 0; - switch(batteryConfig->currentMeterType) { case CURRENT_SENSOR_ADC: - amperageRaw -= amperageRaw / 8; - amperageRaw += adcGetChannel(ADC_CURRENT); - amperage = amperageLatest = currentSensorToCentiamps(amperageRaw / 8); + updateBatteryCurrent(); + + updateCurrentDrawn(lastUpdateAt); + break; case CURRENT_SENSOR_VIRTUAL: amperage = (int32_t)batteryConfig->currentMeterOffset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); - if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) + int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; + if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { throttleOffset = 0; - throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); + } + int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; } - break; - case CURRENT_SENSOR_NONE: - amperage = 0; + + updateCurrentDrawn(lastUpdateAt); + break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_TELEMETRY - if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC && isEscTelemetryActive()) { + if (isEscTelemetryActive()) { amperage = getEscTelemetryCurrent(); mAhDrawn = getEscTelemetryConsumption(); } - #endif - break; - } - if (!feature(FEATURE_ESC_TELEMETRY)) { - mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; - mAhDrawn = mAhdrawnRaw / (3600 * 100); + break; + #endif + case CURRENT_SENSOR_NONE: + amperage = 0; + + break; } } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index cac334f850..b2dafca270 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -53,7 +53,7 @@ typedef struct batteryConfig_s { batterySensor_e batteryMeterType; // type of battery meter uses, either ADC or ESC int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A - uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps + int16_t currentMeterOffset; // offset of the current sensor in millivolt steps currentSensor_e currentMeterType; // type of current meter used, either ADC, Virtual or ESC // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code. From 695f47944b67c331e4fc720f57ded0dc5ac92db2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 09:20:53 +0000 Subject: [PATCH 03/51] Tidy PID controller --- src/main/fc/mw.c | 4 +--- src/main/flight/pid.c | 46 +++++++++++++++---------------------------- src/main/flight/pid.h | 7 +------ 3 files changed, 18 insertions(+), 39 deletions(-) diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index db0b280c35..f5775476b6 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -101,8 +101,6 @@ static bool armingCalibrationWasInitialised; float setpointRate[3]; float rcInput[3]; -extern pidControllerFuncPtr pid_controller; - void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; @@ -695,7 +693,7 @@ void subTaskPidController(void) ¤tProfile->pidProfile, masterConfig.max_angle_inclination, &masterConfig.accelerometerTrims, - &masterConfig.rxConfig + masterConfig.rxConfig.midrc ); if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;} } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c2a2364934..90c1a47082 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -21,7 +21,6 @@ #include -#include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" @@ -36,10 +35,6 @@ #include "flight/navigation.h" #include "flight/gtune.h" -#include "io/gps.h" - -#include "rx/rx.h" - #include "sensors/gyro.h" #include "sensors/acceleration.h" @@ -47,8 +42,7 @@ extern float rcInput[3]; extern float setpointRate[3]; uint32_t targetPidLooptime; -bool pidStabilisationEnabled; -uint8_t PIDweight[3]; +static bool pidStabilisationEnabled; float axisPIDf[3]; @@ -59,10 +53,7 @@ uint8_t PIDweight[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -int32_t errorGyroI[3]; -float errorGyroIf[3]; - -pidControllerFuncPtr pid_controller; // which pid controller are we using +static float errorGyroIf[3]; void setTargetPidLooptime(uint32_t pidLooptime) { @@ -72,7 +63,6 @@ void setTargetPidLooptime(uint32_t pidLooptime) void pidResetErrorGyroState(void) { for (int axis = 0; axis < 3; axis++) { - errorGyroI[axis] = 0; errorGyroIf[axis] = 0.0f; } } @@ -125,24 +115,18 @@ static void pidInitFilters(const pidProfile_t *pidProfile) { // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) + const rollAndPitchTrims_t *angleTrim, uint16_t midrc) { - float errorRate = 0, rD = 0, PVRate = 0, dynC; - float ITerm,PTerm,DTerm; static float lastRateError[2]; static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3]; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float pidInitFilters(pidProfile); + float horizonLevelStrength = 1; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, midrc)); const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection @@ -172,7 +156,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { + const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + for (int axis = 0; axis < 3; axis++) { static uint8_t configP[3], configI[3], configD[3]; @@ -222,16 +207,16 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } } - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y + const float errorRate = setpointRate[axis] - PVRate; // r - y // -----calculate P component and add Dynamic Part based on stick input - PTerm = Kp[axis] * errorRate * tpaFactor; + float PTerm = Kp[axis] * errorRate * tpaFactor; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -245,12 +230,13 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; + const float ITerm = errorGyroIf[axis]; //-----calculate D-term (Yaw D not yet supported) + float DTerm; if (axis != YAW) { static float previousSetpoint[3]; - dynC = c[axis]; + float dynC = c[axis]; if (pidProfile->setpointRelaxRatio < 100) { dynC = c[axis]; if (setpointRate[axis] > 0) { @@ -262,8 +248,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } } previousSetpoint[axis] = setpointRate[axis]; - rD = dynC * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; + const float rD = dynC * setpointRate[axis] - PVRate; // cr - y + float delta = rD - lastRateError[axis]; lastRateError[axis] = rD; // Divide delta by targetLooptime to get differential (ie dr/dt) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 0719f78b30..ab75e9463d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -90,14 +90,9 @@ typedef struct pidProfile_s { #endif } pidProfile_t; -struct controlRateConfig_s; union rollAndPitchTrims_u; -struct rxConfig_s; -typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype - void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + const union rollAndPitchTrims_u *angleTrim, uint16_t midrc); extern float axisPIDf[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; From ac6b83ad7d02fa7fb0fc32de036ed3861bf840e2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 09:48:24 +0000 Subject: [PATCH 04/51] Replaced getdT() by static dT --- src/main/flight/pid.c | 39 +++++++++++++++++++-------------------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 90c1a47082..cb116542ee 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -72,14 +72,6 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; } -float getdT(void) -{ - static float dT; - if (!dT) dT = (float)targetPidLooptime * 0.000001f; - - return dT; -} - const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; pt1Filter_t deltaFilter[3]; @@ -90,23 +82,23 @@ bool dtermNotchInitialised; bool dtermBiquadLpfInitialised; firFilterDenoise_t dtermDenoisingState[3]; -static void pidInitFilters(const pidProfile_t *pidProfile) { - int axis; +static void pidInitFilters(const pidProfile_t *pidProfile) +{ static uint8_t lowpassFilterType; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); - for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); + for (int axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); dtermNotchInitialised = true; } if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) { if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { - for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + for (int axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } if (pidProfile->dterm_filter_type == FILTER_FIR) { - for (axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + for (int axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } lowpassFilterType = pidProfile->dterm_filter_type; } @@ -118,7 +110,14 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio const rollAndPitchTrims_t *angleTrim, uint16_t midrc) { static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3]; + static float Kp[3], Ki[3], Kd[3], c[3]; + static float rollPitchMaxVelocity, yawMaxVelocity; + static float previousSetpoint[3], relaxFactor[3]; + static float dT; + + if (!dT) { + dT = (float)targetPidLooptime * 0.000001f; + } pidInitFilters(pidProfile); @@ -169,8 +168,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); - yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); - rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); + yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT; + rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT; configP[axis] = pidProfile->P8[axis]; configI[axis] = pidProfile->I8[axis]; @@ -227,7 +226,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // limit maximum integrator value to prevent WindUp float itermScaler = setpointRateScaler * kiThrottleGain; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * dT * itermScaler, -250.0f, 250.0f); // I coefficient (I8) moved before integration to make limiting independent from PID settings const float ITerm = errorGyroIf[axis]; @@ -253,7 +252,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio lastRateError[axis] = rD; // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); + delta *= (1.0f / dT); if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; @@ -264,7 +263,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio if (pidProfile->dterm_filter_type == FILTER_BIQUAD) delta = biquadFilterApply(&dtermFilterLpf[axis], delta); else if (pidProfile->dterm_filter_type == FILTER_PT1) - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, dT); else delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta); } @@ -274,7 +273,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // -----calculate total PID output axisPIDf[axis] = PTerm + ITerm + DTerm; } else { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT); axisPIDf[axis] = PTerm + ITerm; From e30823bfec8c8d7d89c30349fd18e7577855e35d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:04:36 +0000 Subject: [PATCH 05/51] Use function pointers to simplify gyro filter calls --- src/main/common/filter.c | 17 ++++++++++ src/main/common/filter.h | 4 +++ src/main/sensors/gyro.c | 69 +++++++++++++++++++++++----------------- 3 files changed, 61 insertions(+), 29 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 6fdf6e5c59..72b5652914 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -22,6 +22,7 @@ #include "common/filter.h" #include "common/maths.h" +#include "common/utils.h" #define M_LN2_FLOAT 0.69314718055994530942f #define M_PI_FLOAT 3.14159265358979323846f @@ -29,6 +30,16 @@ #define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ + +// NULL filter + +float nullFilterApply(const void *filter, float input) +{ + UNUSED(filter); + return input; +} + + // PT1 Low Pass filter void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) @@ -178,6 +189,12 @@ float firFilterApply(const firFilter_t *filter) return ret; } +float firFilterUpdateAndApply(firFilter_t *filter, float input) +{ + firFilterUpdate(filter, input); + return firFilterApply(filter); +} + /* * Returns average of the last items. */ diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 2b0e75a431..a925f68864 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -62,6 +62,9 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; +typedef float (*filterApplyFnPtr)(const void *filter, float input); + +float nullFilterApply(const void *filter, float input); void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); @@ -77,6 +80,7 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl void firFilterUpdate(firFilter_t *filter, float input); void firFilterUpdateAverage(firFilter_t *filter, float input); float firFilterApply(const firFilter_t *filter); +float firFilterUpdateAndApply(firFilter_t *filter, float input); float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); float firFilterCalcMovingAverage(const firFilter_t *filter); float firFilterLastInput(const firFilter_t *filter); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5ed678e103..0a2667d90d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -53,7 +53,10 @@ static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; static float gyroSoftNotchQ1, gyroSoftNotchQ2; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; -static float gyroDt; + +static filterApplyFnPtr softLpfFilterApplyFn; +static filterApplyFnPtr notchFilter1ApplyFn; +static filterApplyFnPtr notchFilter2ApplyFn; void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, @@ -74,20 +77,40 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, void gyroInit(void) { - if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known - for (int axis = 0; axis < 3; axis++) { - if (gyroSoftLpfType == FILTER_BIQUAD) + + softLpfFilterApplyFn = nullFilterApply; + notchFilter1ApplyFn = nullFilterApply; + notchFilter2ApplyFn = nullFilterApply; + + if (gyroSoftLpfHz) { // Initialisation needs to happen once samplingrate is known + if (gyroSoftLpfType == FILTER_BIQUAD) { + softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < 3; axis++) { biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); - else if (gyroSoftLpfType == FILTER_PT1) - gyroDt = (float) gyro.targetLooptime * 0.000001f; - else + } + } else if (gyroSoftLpfType == FILTER_PT1) { + softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; + const float gyroDt = (float) gyro.targetLooptime * 0.000001f; + for (int axis = 0; axis < 3; axis++) { + pt1FilterInit(&gyroFilterPt1[axis], gyroSoftLpfHz, gyroDt); + } + } else { + softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; + for (int axis = 0; axis < 3; axis++) { firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); + } } } - if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) { + if (gyroSoftNotchHz1) { + notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + } + } + if (gyroSoftNotchHz1) { + notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } @@ -180,32 +203,20 @@ void gyroUpdate(void) gyroADC[Y] -= gyroZero[Y]; gyroADC[Z] -= gyroZero[Z]; - if (gyroSoftLpfHz) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - if (debugMode == DEBUG_GYRO) - debug[axis] = gyroADC[axis]; + if (debugMode == DEBUG_GYRO) + debug[axis] = gyroADC[axis]; - if (gyroSoftLpfType == FILTER_BIQUAD) - gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); - else if (gyroSoftLpfType == FILTER_PT1) - gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); - else - gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); + gyroADCf[axis] = softLpfFilterApplyFn(&gyroDenoiseState[axis], (float) gyroADC[axis]); - if (debugMode == DEBUG_NOTCH) - debug[axis] = lrintf(gyroADCf[axis]); + if (debugMode == DEBUG_NOTCH) + debug[axis] = lrintf(gyroADCf[axis]); - if (gyroSoftNotchHz1) - gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]); + gyroADCf[axis] = notchFilter1ApplyFn(&gyroFilterNotch_1[axis], gyroADCf[axis]); - if (gyroSoftNotchHz2) - gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]); + gyroADCf[axis] = notchFilter2ApplyFn(&gyroFilterNotch_2[axis], gyroADCf[axis]); - gyroADC[axis] = lrintf(gyroADCf[axis]); - } - } else { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) - gyroADCf[axis] = gyroADC[axis]; + gyroADC[axis] = lrintf(gyroADCf[axis]); } } From bf683bcdc6065fec708f32f4fe8a9712a9af9fb8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:19:15 +0000 Subject: [PATCH 06/51] Updated function signatures --- src/main/common/filter.c | 2 +- src/main/common/filter.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 72b5652914..5b580a14e1 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -33,7 +33,7 @@ // NULL filter -float nullFilterApply(const void *filter, float input) +float nullFilterApply(void *filter, float input) { UNUSED(filter); return input; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index a925f68864..0121c71b1d 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -62,9 +62,9 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; -typedef float (*filterApplyFnPtr)(const void *filter, float input); +typedef float (*filterApplyFnPtr)(void *filter, float input); -float nullFilterApply(const void *filter, float input); +float nullFilterApply(void *filter, float input); void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); From ae356d485b01230f3e89d5e8e1474e5a7e1290d6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:26:29 +0000 Subject: [PATCH 07/51] Fixed UNUSED #define --- src/main/common/utils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 23088d076d..2e27c930c2 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -32,7 +32,7 @@ #define EXPAND_I(x) x #define EXPAND(x) EXPAND_I(x) -#if !defined(USE_HAL_DRIVER) +#if !defined(UNUSED) #define UNUSED(x) (void)(x) #endif #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)])) From d76d65f19bd787dbaaee5243787649fd2c5b2090 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:44:59 +0000 Subject: [PATCH 08/51] Further UNUSED #define fix --- src/main/drivers/io.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index d60e3af649..0390a0add2 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -15,12 +15,12 @@ * along with Cleanflight. If not, see . */ - #include "common/utils.h" - #include "io.h" #include "io_impl.h" #include "rcc.h" +#include "common/utils.h" + #include "target.h" // io ports defs are stored in array by index now From edad1486be8839955dea50249c0007ccaca9c530 Mon Sep 17 00:00:00 2001 From: jamming Date: Tue, 22 Nov 2016 20:29:35 +0800 Subject: [PATCH 09/51] changes log --- src/main/target/KAKUTEF4/target.h | 9 +++++++++ src/main/target/KAKUTEF4/target.mk | 4 +++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index a23d58a37b..0a78914618 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -59,6 +59,15 @@ #define USE_BARO_MS5611 //#define USE_BARO_BMP280 +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PB14 + +#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 +#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 +#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER + #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index b334aa90ff..36ff917684 100644 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -6,4 +6,6 @@ TARGET_SRC = \ drivers/barometer_bmp280.c \ drivers/barometer_ms5611.c \ drivers/compass_ak8963.c \ - drivers/compass_hmc5883l.c \ No newline at end of file + drivers/compass_hmc5883l.c \ + drivers/max7456.c \ + io/osd.c \ No newline at end of file From 703deeb79fb40c107e6b3b811514e4952acc2e42 Mon Sep 17 00:00:00 2001 From: fishpepper Date: Tue, 22 Nov 2016 20:57:02 +0100 Subject: [PATCH 10/51] added support for ADC3 in F3 targets --- src/main/drivers/adc_impl.h | 3 ++- src/main/drivers/adc_stm32f30x.c | 17 ++++++++++++++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 36ec35c1f2..3c29f70da5 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -33,7 +33,8 @@ typedef enum ADCDevice { ADCDEV_1 = 0, #if defined(STM32F3) ADCDEV_2, - ADCDEV_MAX = ADCDEV_2, + ADCDEV_3, + ADCDEV_MAX = ADCDEV_3, #elif defined(STM32F4) || defined(STM32F7) ADCDEV_2, ADCDEV_3, diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index de158a69e2..13623a4e17 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -41,10 +41,11 @@ const adcDevice_t adcHardware[] = { { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 }, #ifdef ADC24_DMA_REMAP - { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 } + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 }, #else - { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 } + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 }, #endif + { .ADCx = ADC3, .rccADC = RCC_AHB(ADC34), .DMAy_Channelx = DMA2_Channel5 } }; const adcTagMap_t adcTagMap[] = { @@ -97,6 +98,9 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) if (instance == ADC2) return ADCDEV_2; + if (instance == ADC3) + return ADCDEV_3; + return ADCINVALID; } @@ -152,7 +156,14 @@ void adcInit(adcConfig_t *config) return; } - RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz + if ((device == ADCDEV_1) || (device == ADCDEV_2)) { + // enable clock for ADC1+2 + RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz + } else { + // enable clock for ADC3+4 + RCC_ADCCLKConfig(RCC_ADC34PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz + } + RCC_ClockCmd(adc.rccADC, ENABLE); dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, 0); From 874588fbce933d1ebf15b6914d940473400491e1 Mon Sep 17 00:00:00 2001 From: jamming Date: Wed, 23 Nov 2016 09:04:16 +0800 Subject: [PATCH 11/51] changes log --- src/main/target/KAKUTEF4/target.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index 36ff917684..4b0fbb6695 100644 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -7,5 +7,4 @@ TARGET_SRC = \ drivers/barometer_ms5611.c \ drivers/compass_ak8963.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c \ - io/osd.c \ No newline at end of file + drivers/max7456.c \ No newline at end of file From 6a7feb3615f0e4817c9f2d5570dfe99bd16bfa11 Mon Sep 17 00:00:00 2001 From: jamming Date: Wed, 23 Nov 2016 13:19:25 +0800 Subject: [PATCH 12/51] changes log --- src/main/target/KAKUTEF4/target.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index 4b0fbb6695..ee640c0f13 100644 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -7,4 +7,5 @@ TARGET_SRC = \ drivers/barometer_ms5611.c \ drivers/compass_ak8963.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c \ No newline at end of file + drivers/max7456.c + \ No newline at end of file From 4040a1dd7aaddcf4dac5c57f0052c4ea25ef3fa2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 23 Nov 2016 15:40:08 +0100 Subject: [PATCH 13/51] Update mixer scaler Match older mixer weight more close. --- src/main/flight/pid.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ab75e9463d..f96652c92d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -20,7 +20,7 @@ #include #define PID_CONTROLLER_BETAFLIGHT 1 -#define PID_MIXER_SCALING 1000.0f +#define PID_MIXER_SCALING 900.0f #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter #define PIDSUM_LIMIT 0.5f From 16dd47a2b036cbbccad48dce7bd2f8752002d61f Mon Sep 17 00:00:00 2001 From: fishpepper Date: Wed, 23 Nov 2016 21:40:00 +0100 Subject: [PATCH 14/51] fixed missing parameter --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5d400c6ee8..c84d59e332 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1444,7 +1444,7 @@ static void cliSerialPassthrough(char *cmdline) } printf("Relaying data to device on port %d, Reset your board to exit " - "serial passthrough mode.\r\n"); + "serial passthrough mode.\r\n", id); serialPassthrough(cliPort, passThroughPort, NULL, NULL); } From 9448bf1ff3ee87d8ef946b8ed44467051bd3c476 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 17 Nov 2016 15:54:43 +0100 Subject: [PATCH 15/51] Various AlienFlight updates - Timer definition updates for all AlienFlight targets - Add DMA channel information to AlienFlight F3 and F4 motor ports - Add hardware detection of brushed ESC's for all AlienFlight boards - Individual configuration for brushed and brushless AlienFlight boards - Alienflight documentation update --- src/main/target/ALIENFLIGHTF1/AlienFlight.md | 2 +- src/main/target/ALIENFLIGHTF1/config.c | 14 +++- .../target/ALIENFLIGHTF1/hardware_revision.c | 64 +++++++++++++++++++ .../target/ALIENFLIGHTF1/hardware_revision.h | 37 +++++++++++ src/main/target/ALIENFLIGHTF1/target.c | 31 ++++----- src/main/target/ALIENFLIGHTF1/target.h | 4 +- src/main/target/ALIENFLIGHTF3/AlienFlight.md | 2 +- src/main/target/ALIENFLIGHTF3/config.c | 15 ++++- .../target/ALIENFLIGHTF3/hardware_revision.c | 21 ++++-- .../target/ALIENFLIGHTF3/hardware_revision.h | 11 +++- src/main/target/ALIENFLIGHTF3/target.c | 22 +++---- src/main/target/ALIENFLIGHTF3/target.h | 2 +- src/main/target/ALIENFLIGHTF4/AlienFlight.md | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 19 ++++-- .../target/ALIENFLIGHTF4/hardware_revision.c | 22 +++++-- .../target/ALIENFLIGHTF4/hardware_revision.h | 15 +++-- src/main/target/ALIENFLIGHTF4/target.c | 26 ++++---- src/main/target/ALIENFLIGHTF4/target.h | 4 +- 18 files changed, 242 insertions(+), 71 deletions(-) create mode 100644 src/main/target/ALIENFLIGHTF1/hardware_revision.c create mode 100644 src/main/target/ALIENFLIGHTF1/hardware_revision.h diff --git a/src/main/target/ALIENFLIGHTF1/AlienFlight.md b/src/main/target/ALIENFLIGHTF1/AlienFlight.md index 560536128a..3510b52c1c 100644 --- a/src/main/target/ALIENFLIGHTF1/AlienFlight.md +++ b/src/main/target/ALIENFLIGHTF1/AlienFlight.md @@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards: - STM32F103CBT6 MCU (ALIENFLIGHTF1) - STM32F303CCT6 MCU (ALIENFLIGHTF3) - STM32F405RGT6 MCU (ALIENFLIGHTF4) -- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit - The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) - extra-wide traces on the PCB, for maximum power throughput (brushed variants) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 3f827c995b..049a06e882 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -20,6 +20,8 @@ #include +#include "drivers/pwm_output.h" + #include "fc/rc_controls.h" #include "flight/failsafe.h" @@ -31,13 +33,21 @@ #include "config/config_profile.h" #include "config/config_master.h" +#include "hardware_revision.h" // alternative defaults settings for AlienFlight targets void targetConfiguration(master_t *config) { config->rxConfig.spektrum_sat_bind = 5; config->rxConfig.spektrum_sat_bind_autoreset = 1; - config->motorConfig.motorPwmRate = 32000; + + if (hardwareMotorType == MOTOR_BRUSHED) { + config->motorConfig.minthrottle = 1000; + config->motorConfig.motorPwmRate = 32000; + config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED; + config->motorConfig.useUnsyncedPwm = true; + } + config->profile[0].pidProfile.P8[ROLL] = 90; config->profile[0].pidProfile.I8[ROLL] = 44; config->profile[0].pidProfile.D8[ROLL] = 60; @@ -52,5 +62,5 @@ void targetConfiguration(master_t *config) config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif + config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } diff --git a/src/main/target/ALIENFLIGHTF1/hardware_revision.c b/src/main/target/ALIENFLIGHTF1/hardware_revision.c new file mode 100644 index 0000000000..a492c075f6 --- /dev/null +++ b/src/main/target/ALIENFLIGHTF1/hardware_revision.c @@ -0,0 +1,64 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" + +#include "drivers/system.h" +#include "drivers/io.h" +#include "drivers/exti.h" +#include "hardware_revision.h" + +static const char * const hardwareRevisionNames[] = { + "Unknown", + "AlienFlight F1 V1", +}; + +uint8_t hardwareRevision = AFF1_REV_1; +uint8_t hardwareMotorType = MOTOR_UNKNOWN; + +static IO_t MotorDetectPin = IO_NONE; + +void detectHardwareRevision(void) +{ + MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN)); + IOInit(MotorDetectPin, OWNER_SYSTEM, 0); + IOConfigGPIO(MotorDetectPin, IOCFG_IPU); + + delayMicroseconds(10); // allow configuration to settle + + // Check presence of brushed ESC's + if (IORead(MotorDetectPin)) { + hardwareMotorType = MOTOR_BRUSHLESS; + } else { + hardwareMotorType = MOTOR_BRUSHED; + } +} + +void updateHardwareRevision(void) +{ +} + +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) +{ + return NULL; +} diff --git a/src/main/target/ALIENFLIGHTF1/hardware_revision.h b/src/main/target/ALIENFLIGHTF1/hardware_revision.h new file mode 100644 index 0000000000..8fcb761fcc --- /dev/null +++ b/src/main/target/ALIENFLIGHTF1/hardware_revision.h @@ -0,0 +1,37 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ +#pragma once + +typedef enum awf1HardwareRevision_t { + AFF1_UNKNOWN = 0, + AFF1_REV_1, // MPU6050 (I2C) +} awf1HardwareRevision_e; + +typedef enum awf4HardwareMotorType_t { + MOTOR_UNKNOWN = 0, + MOTOR_BRUSHED, + MOTOR_BRUSHLESS +} awf4HardwareMotorType_e; + +extern uint8_t hardwareRevision; +extern uint8_t hardwareMotorType; + +void updateHardwareRevision(void); +void detectHardwareRevision(void); + +struct extiConfig_s; +const struct extiConfig_s *selectMPUIntExtiConfigByHardwareRevision(void); diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index b7a0fa3288..02f8432156 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -20,22 +20,23 @@ #include #include "drivers/io.h" -#include "drivers/timer.h" #include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_PPM | TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM1 - RC1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM2 - RC2 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM3 - RC3 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM4 - RC4 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_PWM | TIM_USE_LED, TIMER_INPUT_ENABLED ), // PWM5 - RC5 + DEF_TIM(TIM3, CH2, PA7, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM6 - RC6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM7 - RC7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM8 - RC8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM9 - OUT1 + DEF_TIM(TIM1, CH4, PA11, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM10 - OUT2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM11 - OUT3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM12 - OUT4 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM13 - OUT5 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ) // PWM14 - OUT6 }; diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 875d5e5122..5bafc911c8 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -20,6 +20,9 @@ #define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1. #define TARGET_CONFIG +#define USE_HARDWARE_REVISION_DETECTION +#define MOTOR_PIN PA8 + #define LED0 PB3 #define LED1 PB4 @@ -70,7 +73,6 @@ // Hardware bind plug at PB5 (Pin 41) #define BINDPLUG_PIN PB5 -#define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 diff --git a/src/main/target/ALIENFLIGHTF3/AlienFlight.md b/src/main/target/ALIENFLIGHTF3/AlienFlight.md index 560536128a..3510b52c1c 100644 --- a/src/main/target/ALIENFLIGHTF3/AlienFlight.md +++ b/src/main/target/ALIENFLIGHTF3/AlienFlight.md @@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards: - STM32F103CBT6 MCU (ALIENFLIGHTF1) - STM32F303CCT6 MCU (ALIENFLIGHTF3) - STM32F405RGT6 MCU (ALIENFLIGHTF4) -- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit - The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) - extra-wide traces on the PCB, for maximum power throughput (brushed variants) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 0df64d8533..0599007af1 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -24,6 +24,7 @@ #include "drivers/sensor.h" #include "drivers/compass.h" +#include "drivers/pwm_output.h" #include "fc/rc_controls.h" @@ -39,16 +40,24 @@ #include "config/config_profile.h" #include "config/config_master.h" +#include "hardware_revision.h" // alternative defaults settings for AlienFlight targets void targetConfiguration(master_t *config) { - config->mag_hardware = MAG_NONE; // disabled by default config->rxConfig.spektrum_sat_bind = 5; config->rxConfig.spektrum_sat_bind_autoreset = 1; - config->motorConfig.motorPwmRate = 32000; config->gyro_sync_denom = 2; + config->mag_hardware = MAG_NONE; // disabled by default config->pid_process_denom = 1; + + if (hardwareMotorType == MOTOR_BRUSHED) { + config->motorConfig.minthrottle = 1000; + config->motorConfig.motorPwmRate = 32000; + config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED; + config->motorConfig.useUnsyncedPwm = true; + } + config->profile[0].pidProfile.P8[ROLL] = 90; config->profile[0].pidProfile.I8[ROLL] = 44; config->profile[0].pidProfile.D8[ROLL] = 60; @@ -63,5 +72,5 @@ void targetConfiguration(master_t *config) config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif + config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c index f8cabdc76e..f50d744fdc 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c @@ -30,13 +30,15 @@ static const char * const hardwareRevisionNames[] = { "Unknown", - "AlienFlight V1", - "AlienFlight V2" + "AlienFlight F3 V1", + "AlienFlight F3 V2" }; -uint8_t hardwareRevision = UNKNOWN; +uint8_t hardwareRevision = AFF3_UNKNOWN; +uint8_t hardwareMotorType = MOTOR_UNKNOWN; static IO_t HWDetectPin = IO_NONE; +static IO_t MotorDetectPin = IO_NONE; void detectHardwareRevision(void) { @@ -44,14 +46,25 @@ void detectHardwareRevision(void) IOInit(HWDetectPin, OWNER_SYSTEM, 0); IOConfigGPIO(HWDetectPin, IOCFG_IPU); - // Check hardware revision + MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN)); + IOInit(MotorDetectPin, OWNER_SYSTEM, 0); + IOConfigGPIO(MotorDetectPin, IOCFG_IPU); + delayMicroseconds(10); // allow configuration to settle + // Check hardware revision if (IORead(HWDetectPin)) { hardwareRevision = AFF3_REV_1; } else { hardwareRevision = AFF3_REV_2; } + + // Check presence of brushed ESC's + if (IORead(MotorDetectPin)) { + hardwareMotorType = MOTOR_BRUSHLESS; + } else { + hardwareMotorType = MOTOR_BRUSHED; + } } void updateHardwareRevision(void) diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h index 745a9f9f52..9b0618561f 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h @@ -17,12 +17,19 @@ #pragma once typedef enum awf3HardwareRevision_t { - UNKNOWN = 0, - AFF3_REV_1, // MPU6050 / MPU9150 (I2C) + AFF3_UNKNOWN = 0, + AFF3_REV_1, // MPU6050 (I2C) AFF3_REV_2 // MPU6500 / MPU9250 (SPI) } awf3HardwareRevision_e; +typedef enum awf4HardwareMotorType_t { + MOTOR_UNKNOWN = 0, + MOTOR_BRUSHED, + MOTOR_BRUSHLESS +} awf4HardwareMotorType_e; + extern uint8_t hardwareRevision; +extern uint8_t hardwareMotorType; void updateHardwareRevision(void); void detectHardwareRevision(void); diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index 65a745dd16..8f1138043d 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -26,16 +26,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // up to 10 Motor Outputs - DEF_TIM(TIM15, CH2, PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - DEF_TIM(TIM15, CH1, PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - DEF_TIM(TIM3, CH1, PA6, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - DEF_TIM(TIM3, CH2, PA4, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM9 - PA4 - *TIM3_CH2 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - DEF_TIM(TIM2, CH4, PA3, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED ), // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + DEF_TIM(TIM15, CH2, PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM1 - PB15 - DMA_NONE - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + DEF_TIM(TIM15, CH1, PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM2 - PB14 - DMA1_CH5 - TIM1_CH2N, *TIM15_CH1 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM3 - PA8 - DMA1_CH2 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM4 - PB0 - DMA1_CH2 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM5 - PA6 - DMA1_CH6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM6 - PA2 - DMA1_CH1 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM7 - PB1 - DMA1_CH3 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM8 - PA7 - DMA1_CH7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM9 - PA4 - DMA_NONE - *TIM3_CH2 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM10 - PA1 - DMA1_CH7 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, TIMER_INPUT_ENABLED ), // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index f834b5d619..63a3a9bb08 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -24,6 +24,7 @@ #define USE_HARDWARE_REVISION_DETECTION #define HW_PIN PB2 +#define MOTOR_PIN PB15 // LED's V1 #define LED0 PB4 @@ -114,7 +115,6 @@ // Hardware bind plug at PB12 (Pin 25) #define BINDPLUG_PIN PB12 -#define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 diff --git a/src/main/target/ALIENFLIGHTF4/AlienFlight.md b/src/main/target/ALIENFLIGHTF4/AlienFlight.md index 560536128a..3510b52c1c 100644 --- a/src/main/target/ALIENFLIGHTF4/AlienFlight.md +++ b/src/main/target/ALIENFLIGHTF4/AlienFlight.md @@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards: - STM32F103CBT6 MCU (ALIENFLIGHTF1) - STM32F303CCT6 MCU (ALIENFLIGHTF3) - STM32F405RGT6 MCU (ALIENFLIGHTF4) -- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit - The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) - extra-wide traces on the PCB, for maximum power throughput (brushed variants) diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index b5f3188055..57a1d62cbf 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -24,6 +24,7 @@ #include "drivers/sensor.h" #include "drivers/compass.h" +#include "drivers/pwm_output.h" #include "drivers/serial.h" #include "fc/rc_controls.h" @@ -50,7 +51,19 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(master_t *config) { + config->batteryConfig.currentMeterOffset = 2500; + config->batteryConfig.currentMeterScale = -667; + config->gyro_sync_denom = 1; config->mag_hardware = MAG_NONE; // disabled by default + config->pid_process_denom = 1; + + if (hardwareMotorType == MOTOR_BRUSHED) { + config->motorConfig.minthrottle = 1000; + config->motorConfig.motorPwmRate = 32000; + config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED; + config->motorConfig.useUnsyncedPwm = true; + } + if (hardwareRevision == AFF4_REV_1) { config->rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048; config->rxConfig.spektrum_sat_bind = 5; @@ -61,12 +74,8 @@ void targetConfiguration(master_t *config) config->serialConfig.portConfigs[SERIAL_PORT_USART2].functionMask = FUNCTION_TELEMETRY_FRSKY; config->telemetryConfig.telemetry_inversion = 0; intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->enabledFeatures); - config->batteryConfig.currentMeterOffset = 2500; - config->batteryConfig.currentMeterScale = -667; } - config->motorConfig.motorPwmRate = 32000; - config->gyro_sync_denom = 1; - config->pid_process_denom = 1; + config->profile[0].pidProfile.P8[ROLL] = 53; config->profile[0].pidProfile.I8[ROLL] = 45; config->profile[0].pidProfile.D8[ROLL] = 52; diff --git a/src/main/target/ALIENFLIGHTF4/hardware_revision.c b/src/main/target/ALIENFLIGHTF4/hardware_revision.c index bb58b1bcf2..4e55f9c7fb 100644 --- a/src/main/target/ALIENFLIGHTF4/hardware_revision.c +++ b/src/main/target/ALIENFLIGHTF4/hardware_revision.c @@ -25,18 +25,19 @@ #include "drivers/system.h" #include "drivers/io.h" -#include "drivers/exti.h" #include "hardware_revision.h" static const char * const hardwareRevisionNames[] = { "Unknown", - "AlienFlight V1", - "AlienFlight V2" + "AlienFlight F4 V1", + "AlienFlight F4 V2" }; -uint8_t hardwareRevision = UNKNOWN; +uint8_t hardwareRevision = AFF4_UNKNOWN; +uint8_t hardwareMotorType = MOTOR_UNKNOWN; static IO_t HWDetectPin = IO_NONE; +static IO_t MotorDetectPin = IO_NONE; void detectHardwareRevision(void) { @@ -44,14 +45,25 @@ void detectHardwareRevision(void) IOInit(HWDetectPin, OWNER_SYSTEM, 0); IOConfigGPIO(HWDetectPin, IOCFG_IPU); - // Check hardware revision + MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN)); + IOInit(MotorDetectPin, OWNER_SYSTEM, 0); + IOConfigGPIO(MotorDetectPin, IOCFG_IPU); + delayMicroseconds(10); // allow configuration to settle + // Check hardware revision if (IORead(HWDetectPin)) { hardwareRevision = AFF4_REV_1; } else { hardwareRevision = AFF4_REV_2; } + + // Check presence of brushed ESC's + if (IORead(MotorDetectPin)) { + hardwareMotorType = MOTOR_BRUSHLESS; + } else { + hardwareMotorType = MOTOR_BRUSHED; + } } void updateHardwareRevision(void) diff --git a/src/main/target/ALIENFLIGHTF4/hardware_revision.h b/src/main/target/ALIENFLIGHTF4/hardware_revision.h index dc9c8325c3..e421b88960 100644 --- a/src/main/target/ALIENFLIGHTF4/hardware_revision.h +++ b/src/main/target/ALIENFLIGHTF4/hardware_revision.h @@ -17,14 +17,19 @@ #pragma once typedef enum awf4HardwareRevision_t { - UNKNOWN = 0, - AFF4_REV_1, - AFF4_REV_2 + AFF4_UNKNOWN = 0, + AFF4_REV_1, // MPU6500 / MPU9250 (SPI), (V1.1 Current Sensor (ACS712), SDCard Reader) + AFF4_REV_2 // ICM-20602 (SPI), OpenSky RX (CC2510), Current Sensor (ACS712), SDCard Reader } awf4HardwareRevision_e; +typedef enum awf4HardwareMotorType_t { + MOTOR_UNKNOWN = 0, + MOTOR_BRUSHED, + MOTOR_BRUSHLESS +} awf4HardwareMotorType_e; + extern uint8_t hardwareRevision; +extern uint8_t hardwareMotorType; void updateHardwareRevision(void); void detectHardwareRevision(void); - -struct extiConfig_s; diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index a2d744fc68..10a23f8986 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -25,17 +25,17 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PWM1 - PA8 RC1 - DEF_TIM(TIM1, CH2, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DEF_TIM(TIM1, CH3, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DEF_TIM(TIM8, CH2, PB14, TIM_USE_PWM, 0, 0), // PWM4 - PA14 RC4 - DEF_TIM(TIM8, CH3, PB15, TIM_USE_PWM, 0, 0), // PWM5 - PA15 RC5 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1, 0), // PWM6 - PB8 OUT1 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1, 0), // PWM7 - PB9 OUT2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 1, 0), // PWM8 - PA0 OUT3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 1, 0), // PWM9 - PA1 OUT4 - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 1, 0), // PWM10 - PC6 OUT5 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 1, 0), // PWM11 - PC7 OUT6 - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, 1, 0), // PWM13 - PC8 OUT7 - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, 1, 0), // PWM13 - PC9 OUT8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED, 0), // PWM1 - PA8 RC1 + DEF_TIM(TIM1, CH2, PB0, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM2 - PB0 RC2 + DEF_TIM(TIM1, CH3, PB1, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM3 - PB1 RC3 + DEF_TIM(TIM8, CH2, PB14, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM4 - PA14 RC4 + DEF_TIM(TIM8, CH3, PB15, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM5 - PA15 RC5 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 - PB8 OUT1 - DMA1_ST7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM7 - PB9 OUT2 - DMA1_ST3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM8 - PA0 OUT3 - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM9 - PA1 OUT4 - DMA1_ST4 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM10 - PC6 OUT5 - (DMA1_ST4) + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM11 - PC7 OUT6 - DMA1_ST5 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM13 - PC8 OUT7 - (DMA1_ST7) + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM13 - PC9 OUT8 - (DMA1_ST2) }; diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 22380a7048..b0947c71ae 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -21,6 +21,7 @@ #define USE_HARDWARE_REVISION_DETECTION #define HW_PIN PC13 +#define MOTOR_PIN PB8 #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) @@ -82,10 +83,12 @@ // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz +//#ifndef USE_DSHOT #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_Channel_0 +//#endif // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING @@ -160,7 +163,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define BRUSHED_MOTORS #define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 From 83472a69890227610ef2041933d0ff2d65825ef6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 18:02:32 +0000 Subject: [PATCH 16/51] Fix dataflash read by only using serialWriteBuf in mspSerialEncode --- src/main/msp/msp_serial.c | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 9b38932bfd..74f34fd95e 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -125,20 +125,21 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) serialBeginWrite(msp->port); const int len = sbufBytesRemaining(&packet->buf); const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT; - const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd}; - serialWriteBuf(msp->port, hdr, sizeof(hdr)); - uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field + uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd}; + int hdrLen = 5; +#define CHECKSUM_STARTPOS 3 // checksum starts from mspLen field if (len >= JUMBO_FRAME_SIZE_LIMIT) { - serialWrite(msp->port, len & 0xff); - checksum ^= len & 0xff; - serialWrite(msp->port, (len >> 8) & 0xff); - checksum ^= (len >> 8) & 0xff; + hdrLen += 2; + hdr[5] = len & 0xff; + hdr[6] = (len >> 8) & 0xff; } + serialWriteBuf(msp->port, hdr, hdrLen); + uint8_t checksum = mspSerialChecksumBuf(0, hdr + CHECKSUM_STARTPOS, hdrLen - CHECKSUM_STARTPOS); if (len > 0) { serialWriteBuf(msp->port, sbufPtr(&packet->buf), len); checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len); } - serialWrite(msp->port, checksum); + serialWriteBuf(msp->port, &checksum, 1); serialEndWrite(msp->port); return sizeof(hdr) + len + 1; // header, data, and checksum } From 594b9d8ad30001401503632cd5a4aa693d8b23d4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 22:27:49 +0000 Subject: [PATCH 17/51] Fix gyro filter initialisation --- src/main/sensors/gyro.c | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a08a042610..d27c2eb17c 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,10 +44,6 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; -static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT]; -static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; -static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; static float gyroSoftNotchQ1, gyroSoftNotchQ2; @@ -55,8 +51,11 @@ static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; +static void *softLpfFilter[3]; static filterApplyFnPtr notchFilter1ApplyFn; +static void *notchFilter1[3]; static filterApplyFnPtr notchFilter2ApplyFn; +static void *notchFilter2[3]; void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, @@ -77,6 +76,11 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, void gyroInit(void) { + static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; + static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; + static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT]; + static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; + static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; softLpfFilterApplyFn = nullFilterApply; notchFilter1ApplyFn = nullFilterApply; @@ -86,18 +90,21 @@ void gyroInit(void) if (gyroSoftLpfType == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { - biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); + softLpfFilter[axis] = &gyroFilterLPF[axis]; + biquadFilterInitLPF(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); } } else if (gyroSoftLpfType == FILTER_PT1) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { - pt1FilterInit(&gyroFilterPt1[axis], gyroSoftLpfHz, gyroDt); + softLpfFilter[axis] = &gyroFilterPt1[axis]; + pt1FilterInit(softLpfFilter[axis], gyroSoftLpfHz, gyroDt); } } else { softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { - firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); + softLpfFilter[axis] = &gyroDenoiseState[axis]; + firFilterDenoiseInit(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); } } } @@ -105,13 +112,15 @@ void gyroInit(void) if (gyroSoftNotchHz1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + notchFilter1[axis] = &gyroFilterNotch_1[axis]; + biquadFilterInit(notchFilter1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } if (gyroSoftNotchHz1) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); + notchFilter2[axis] = &gyroFilterNotch_2[axis]; + biquadFilterInit(notchFilter2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } @@ -208,14 +217,14 @@ void gyroUpdate(void) if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; - gyroADCf[axis] = softLpfFilterApplyFn(&gyroDenoiseState[axis], (float) gyroADC[axis]); + gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); - gyroADCf[axis] = notchFilter1ApplyFn(&gyroFilterNotch_1[axis], gyroADCf[axis]); + gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]); - gyroADCf[axis] = notchFilter2ApplyFn(&gyroFilterNotch_2[axis], gyroADCf[axis]); + gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } From d85eed0933aeaee1c43e22360d0af4398f866416 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 24 Nov 2016 17:11:12 +0000 Subject: [PATCH 18/51] Move freestanding masterConfig items into structs --- src/main/blackbox/blackbox.c | 42 ++++++++++----------- src/main/blackbox/blackbox.h | 9 ++++- src/main/blackbox/blackbox_io.c | 22 +++++------ src/main/cms/cms_menu_blackbox.c | 2 +- src/main/config/config_master.h | 27 ++++--------- src/main/config/parameter_group_ids.h | 28 +++++++------- src/main/fc/config.c | 36 +++++++++--------- src/main/fc/fc_msp.c | 50 ++++++++++++------------- src/main/fc/fc_tasks.c | 2 +- src/main/fc/mw.c | 20 +++++----- src/main/fc/rc_controls.h | 7 ++++ src/main/flight/mixer.h | 1 + src/main/io/serial_cli.c | 40 ++++++++++---------- src/main/main.c | 10 ++--- src/main/sensors/compass.h | 5 +++ src/main/sensors/initialisation.c | 13 ++----- src/main/sensors/initialisation.h | 4 +- src/main/sensors/sensors.h | 11 ++++++ src/main/target/ALIENFLIGHTF3/config.c | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 2 +- src/main/target/BLUEJAYF4/config.c | 4 +- src/main/target/COLIBRI/config.c | 4 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 22 +++++------ src/main/target/MULTIFLITEPICO/config.c | 2 +- src/main/telemetry/mavlink.c | 2 +- 25 files changed, 190 insertions(+), 177 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b0dfce3fd5..4d10e90778 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -349,7 +349,7 @@ bool blackboxMayEditConfig() } static bool blackboxIsOnlyLoggingIntraframes() { - return masterConfig.blackbox_rate_num == 1 && masterConfig.blackbox_rate_denom == 32; + return masterConfig.blackboxConfig.rate_num == 1 && masterConfig.blackboxConfig.rate_denom == 32; } static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) @@ -369,7 +369,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1; case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: - return masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI; + return masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI; case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: @@ -407,7 +407,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) return masterConfig.rxConfig.rssi_channel > 0 || feature(FEATURE_RSSI_ADC); case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: - return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom; + return masterConfig.blackboxConfig.rate_num < masterConfig.blackboxConfig.rate_denom; case FLIGHT_LOG_FIELD_CONDITION_NEVER: return false; @@ -758,22 +758,22 @@ static void validateBlackboxConfig() { int div; - if (masterConfig.blackbox_rate_num == 0 || masterConfig.blackbox_rate_denom == 0 - || masterConfig.blackbox_rate_num >= masterConfig.blackbox_rate_denom) { - masterConfig.blackbox_rate_num = 1; - masterConfig.blackbox_rate_denom = 1; + if (masterConfig.blackboxConfig.rate_num == 0 || masterConfig.blackboxConfig.rate_denom == 0 + || masterConfig.blackboxConfig.rate_num >= masterConfig.blackboxConfig.rate_denom) { + masterConfig.blackboxConfig.rate_num = 1; + masterConfig.blackboxConfig.rate_denom = 1; } else { /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat * itself more frequently) */ - div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); + div = gcd(masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom); - masterConfig.blackbox_rate_num /= div; - masterConfig.blackbox_rate_denom /= div; + masterConfig.blackboxConfig.rate_num /= div; + masterConfig.blackboxConfig.rate_denom /= div; } // If we've chosen an unsupported device, change the device to serial - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { #ifdef USE_FLASHFS case BLACKBOX_DEVICE_FLASH: #endif @@ -785,7 +785,7 @@ static void validateBlackboxConfig() break; default: - masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; + masterConfig.blackboxConfig.device = BLACKBOX_DEVICE_SERIAL; } } @@ -867,7 +867,7 @@ bool startedLoggingInTestMode = false; void startInTestMode(void) { if(!startedLoggingInTestMode) { - if (masterConfig.blackbox_device == BLACKBOX_DEVICE_SERIAL) { + if (masterConfig.blackboxConfig.device == BLACKBOX_DEVICE_SERIAL) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); if (sharedBlackboxAndMspPort) { return; // When in test mode, we cannot share the MSP and serial logger port! @@ -1172,7 +1172,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name); - BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); + BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom); BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.motorConfig.minthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.motorConfig.maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); @@ -1270,10 +1270,10 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyro_soft_notch_cutoff_1, masterConfig.gyro_soft_notch_cutoff_2); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware); - BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); - BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware); - BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); + BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware); + BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.sensorSelectionConfig.baro_hardware); + BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.sensorSelectionConfig.mag_hardware); + BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.armingConfig.gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", masterConfig.rxConfig.rcInterpolation); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", masterConfig.rxConfig.rcInterpolationInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); @@ -1376,10 +1376,10 @@ static void blackboxCheckAndLogFlightMode() */ static bool blackboxShouldLogPFrame(uint32_t pFrameIndex) { - /* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of + /* Adding a magic shift of "masterConfig.blackboxConfig.rate_num - 1" in here creates a better spread of * recorded / skipped frames when the I frame's position is considered: */ - return (pFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num; + return (pFrameIndex + masterConfig.blackboxConfig.rate_num - 1) % masterConfig.blackboxConfig.rate_denom < masterConfig.blackboxConfig.rate_num; } static bool blackboxShouldLogIFrame() { @@ -1595,7 +1595,7 @@ void handleBlackbox(uint32_t currentTime) if (startedLoggingInTestMode) startedLoggingInTestMode = false; } else { // Only log in test mode if there is room! - if(masterConfig.blackbox_on_motor_test) { + if(masterConfig.blackboxConfig.on_motor_test) { // Handle Motor Test Mode if(inMotorTestMode()) { if(blackboxState==BLACKBOX_STATE_STOPPED) diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 5650f2995a..b7fc3c6aa8 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -19,6 +19,13 @@ #include "blackbox/blackbox_fielddefs.h" +typedef struct blackboxConfig_s { + uint8_t rate_num; + uint8_t rate_denom; + uint8_t device; + uint8_t on_motor_test; +} blackboxConfig_t; + void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); void initBlackbox(void); @@ -26,4 +33,4 @@ void handleBlackbox(uint32_t currentTime); void startBlackbox(void); void finishBlackbox(void); -bool blackboxMayEditConfig(); +bool blackboxMayEditConfig(void); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index a141f016ed..b9a810d14b 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -65,7 +65,7 @@ static struct { void blackboxWrite(uint8_t value) { - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { #ifdef USE_FLASHFS case BLACKBOX_DEVICE_FLASH: flashfsWriteByte(value); // Write byte asynchronously @@ -137,7 +137,7 @@ int blackboxPrint(const char *s) int length; const uint8_t *pos; - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { #ifdef USE_FLASHFS case BLACKBOX_DEVICE_FLASH: @@ -479,7 +479,7 @@ void blackboxWriteFloat(float value) */ void blackboxDeviceFlush(void) { - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { #ifdef USE_FLASHFS /* * This is our only output device which requires us to call flush() in order for it to write anything. The other @@ -502,7 +502,7 @@ void blackboxDeviceFlush(void) */ bool blackboxDeviceFlushForce(void) { - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { case BLACKBOX_DEVICE_SERIAL: // Nothing to speed up flushing on serial, as serial is continuously being drained out of its buffer return isSerialTransmitBufferEmpty(blackboxPort); @@ -530,7 +530,7 @@ bool blackboxDeviceFlushForce(void) */ bool blackboxDeviceOpen(void) { - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { case BLACKBOX_DEVICE_SERIAL: { serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX); @@ -604,7 +604,7 @@ bool blackboxDeviceOpen(void) */ void blackboxDeviceClose(void) { - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { case BLACKBOX_DEVICE_SERIAL: // Since the serial port could be shared with other processes, we have to give it back here closeSerialPort(blackboxPort); @@ -748,7 +748,7 @@ static bool blackboxSDCardBeginLog() */ bool blackboxDeviceBeginLog(void) { - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { #ifdef USE_SDCARD case BLACKBOX_DEVICE_SDCARD: return blackboxSDCardBeginLog(); @@ -772,7 +772,7 @@ bool blackboxDeviceEndLog(bool retainLog) (void) retainLog; #endif - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { #ifdef USE_SDCARD case BLACKBOX_DEVICE_SDCARD: // Keep retrying until the close operation queues @@ -794,7 +794,7 @@ bool blackboxDeviceEndLog(bool retainLog) bool isBlackboxDeviceFull(void) { - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { case BLACKBOX_DEVICE_SERIAL: return false; @@ -821,7 +821,7 @@ void blackboxReplenishHeaderBudget() { int32_t freeSpace; - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { case BLACKBOX_DEVICE_SERIAL: freeSpace = serialTxBytesFree(blackboxPort); break; @@ -867,7 +867,7 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes) } // Handle failure: - switch (masterConfig.blackbox_device) { + switch (masterConfig.blackboxConfig.device) { case BLACKBOX_DEVICE_SERIAL: /* * One byte of the tx buffer isn't available for user data (due to its circular list implementation), diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 7ad339ccba..18808b5c6d 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -90,7 +90,7 @@ static OSD_Entry cmsx_menuBlackboxEntries[] = { { "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, - { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackbox_rate_denom,1,32,1 }, 0 }, + { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackboxConfig.rate_denom,1,32,1 }, 0 }, #ifdef USE_FLASHFS { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 44c43109f9..827be582b0 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -21,6 +21,8 @@ #include "config/config_profile.h" +#include "blackbox/blackbox.h" + #include "cms/cms.h" #include "drivers/adc.h" @@ -57,7 +59,7 @@ #include "sensors/boardalignment.h" #include "sensors/barometer.h" #include "sensors/battery.h" - +#include "sensors/compass.h" // System-wide typedef struct master_s { @@ -65,7 +67,6 @@ typedef struct master_s { uint16_t size; uint8_t magic_be; // magic number, should be 0xBE - uint8_t mixerMode; uint32_t enabledFeatures; // motor/esc/servo related stuff @@ -84,11 +85,11 @@ typedef struct master_s { #endif // global sensor-related stuff + sensorSelectionConfig_t sensorSelectionConfig; sensorAlignmentConfig_t sensorAlignmentConfig; boardAlignment_t boardAlignment; int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) - uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_sync_denom; // Gyro sample divider @@ -106,11 +107,7 @@ typedef struct master_s { uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate gyroConfig_t gyroConfig; - - uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device - uint8_t baro_hardware; // Barometer hardware to use - int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ - // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. + compassConfig_t compassConfig; rollAndPitchTrims_t accelerometerTrims; // accelerometer trim @@ -132,17 +129,12 @@ typedef struct master_s { #endif uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). - flightDynamicsTrims_t accZero; - flightDynamicsTrims_t magZero; + sensorTrims_t sensorTrims; rxConfig_t rxConfig; inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers. - - uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right - uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value - uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 - uint8_t small_angle; + armingConfig_t armingConfig; // mixer-related configuration mixerConfig_t mixerConfig; @@ -213,10 +205,7 @@ typedef struct master_s { #endif #ifdef BLACKBOX - uint8_t blackbox_rate_num; - uint8_t blackbox_rate_denom; - uint8_t blackbox_device; - uint8_t blackbox_on_motor_test; + blackboxConfig_t blackboxConfig; #endif uint32_t beeper_off_flags; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index c2a7e81dd3..dee3b592d1 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -16,20 +16,20 @@ */ // FC configuration -#define PG_FAILSAFE_CONFIG 1 -#define PG_BOARD_ALIGNMENT 2 -#define PG_GIMBAL_CONFIG 3 -#define PG_MOTOR_MIXER 4 -#define PG_BLACKBOX_CONFIG 5 -#define PG_MOTOR_AND_SERVO_CONFIG 6 -#define PG_SENSOR_SELECTION_CONFIG 7 -#define PG_SENSOR_ALIGNMENT_CONFIG 8 -#define PG_SENSOR_TRIMS 9 -#define PG_GYRO_CONFIG 10 -#define PG_BATTERY_CONFIG 11 -#define PG_CONTROL_RATE_PROFILES 12 -#define PG_SERIAL_CONFIG 13 -#define PG_PID_PROFILE 14 +#define PG_FAILSAFE_CONFIG 1 // strruct OK +#define PG_BOARD_ALIGNMENT 2 // struct OK +#define PG_GIMBAL_CONFIG 3 // struct OK +#define PG_MOTOR_MIXER 4 // two structs mixerConfig_t servoMixerConfig_t +#define PG_BLACKBOX_CONFIG 5 // struct OK +#define PG_MOTOR_CONFIG 6 // struct OK +#define PG_SENSOR_SELECTION_CONFIG 7 // struct OK +#define PG_SENSOR_ALIGNMENT_CONFIG 8 // struct OK +#define PG_SENSOR_TRIMS 9 // struct OK +#define PG_GYRO_CONFIG 10 // PR outstanding, need to think about pid_process_denom +#define PG_BATTERY_CONFIG 11 // struct OK +#define PG_CONTROL_RATE_PROFILES 12 // struct OK, needs to be split out of rc_controls.h into rate_profile.h +#define PG_SERIAL_CONFIG 13 // struct OK +#define PG_PID_PROFILE 14 // struct OK, CF differences #define PG_GTUNE_CONFIG 15 #define PG_ARMING_CONFIG 16 #define PG_TRANSPONDER_CONFIG 17 diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 93c8832107..223fdfc9f6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -558,7 +558,7 @@ void createDefaultConfig(master_t *config) #endif config->version = EEPROM_CONF_VERSION; - config->mixerMode = MIXER_QUADX; + config->mixerConfig.mixerMode = MIXER_QUADX; // global settings config->current_profile_index = 0; // default profile @@ -584,22 +584,22 @@ void createDefaultConfig(master_t *config) config->debug_mode = DEBUG_NONE; - resetAccelerometerTrims(&config->accZero); + resetAccelerometerTrims(&config->sensorTrims.accZero); resetSensorAlignment(&config->sensorAlignmentConfig); config->boardAlignment.rollDegrees = 0; config->boardAlignment.pitchDegrees = 0; config->boardAlignment.yawDegrees = 0; - config->acc_hardware = ACC_DEFAULT; // default/autodetect + config->sensorSelectionConfig.acc_hardware = ACC_DEFAULT; // default/autodetect config->max_angle_inclination = 700; // 70 degrees config->yaw_control_direction = 1; config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable - config->mag_hardware = 1; + config->sensorSelectionConfig.mag_hardware = 1; - config->baro_hardware = 1; + config->sensorSelectionConfig.baro_hardware = 1; resetBatteryConfig(&config->batteryConfig); @@ -663,10 +663,10 @@ void createDefaultConfig(master_t *config) config->inputFilteringMode = INPUT_FILTERING_DISABLED; - config->gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support - config->disarm_kill_switch = 1; - config->auto_disarm_delay = 5; - config->small_angle = 25; + config->armingConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support + config->armingConfig.disarm_kill_switch = 1; + config->armingConfig.auto_disarm_delay = 5; + config->armingConfig.small_angle = 25; config->airplaneConfig.fixedwing_althold_dir = 1; @@ -697,7 +697,7 @@ void createDefaultConfig(master_t *config) resetRollAndPitchTrims(&config->accelerometerTrims); - config->mag_declination = 0; + config->compassConfig.mag_declination = 0; config->acc_lpf_hz = 10.0f; config->accDeadband.xy = 40; config->accDeadband.z = 40; @@ -768,17 +768,17 @@ void createDefaultConfig(master_t *config) #ifdef BLACKBOX #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) intFeatureSet(FEATURE_BLACKBOX, featuresPtr); - config->blackbox_device = BLACKBOX_DEVICE_FLASH; + config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH; #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) intFeatureSet(FEATURE_BLACKBOX, featuresPtr); - config->blackbox_device = BLACKBOX_DEVICE_SDCARD; + config->blackboxConfig.device = BLACKBOX_DEVICE_SDCARD; #else - config->blackbox_device = BLACKBOX_DEVICE_SERIAL; + config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL; #endif - config->blackbox_rate_num = 1; - config->blackbox_rate_denom = 1; - config->blackbox_on_motor_test = 0; // default off + config->blackboxConfig.rate_num = 1; + config->blackboxConfig.rate_denom = 1; + config->blackboxConfig.on_motor_test = 0; // default off #endif // BLACKBOX #ifdef SERIALRX_UART @@ -855,7 +855,7 @@ void activateConfig(void) #endif useFailsafeConfig(&masterConfig.failsafeConfig); - setAccelerationTrims(&masterConfig.accZero); + setAccelerationTrims(&masterConfig.sensorTrims.accZero); setAccelerationFilter(masterConfig.acc_lpf_hz); mixerUseConfigs( @@ -873,7 +873,7 @@ void activateConfig(void) imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f; imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f; imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal; - imuRuntimeConfig.small_angle = masterConfig.small_angle; + imuRuntimeConfig.small_angle = masterConfig.armingConfig.small_angle; imuConfigure( &imuRuntimeConfig, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index fb60f5591b..df38554c72 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -313,7 +313,7 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; } - if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { + if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) { activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; } @@ -360,7 +360,7 @@ void initActiveBoxIds(void) #endif #ifdef USE_SERVOS - if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { + if (masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { activeBoxIds[activeBoxIdCount++] = BOXSERVO1; activeBoxIds[activeBoxIdCount++] = BOXSERVO2; activeBoxIds[activeBoxIdCount++] = BOXSERVO3; @@ -563,7 +563,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn // DEPRECATED - Use MSP_API_VERSION case MSP_IDENT: sbufWriteU8(dst, MW_VERSION); - sbufWriteU8(dst, masterConfig.mixerMode); + sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode); sbufWriteU8(dst, MSP_PROTOCOL_VERSION); sbufWriteU32(dst, CAP_DYNBALANCE); // "capability" break; @@ -700,8 +700,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_ARMING_CONFIG: - sbufWriteU8(dst, masterConfig.auto_disarm_delay); - sbufWriteU8(dst, masterConfig.disarm_kill_switch); + sbufWriteU8(dst, masterConfig.armingConfig.auto_disarm_delay); + sbufWriteU8(dst, masterConfig.armingConfig.disarm_kill_switch); break; case MSP_LOOP_TIME: @@ -797,7 +797,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel); sbufWriteU8(dst, 0); - sbufWriteU16(dst, masterConfig.mag_declination / 10); + sbufWriteU16(dst, masterConfig.compassConfig.mag_declination / 10); sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale); sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage); @@ -886,7 +886,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_MIXER: - sbufWriteU8(dst, masterConfig.mixerMode); + sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode); break; case MSP_RX_CONFIG: @@ -932,7 +932,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_BF_CONFIG: - sbufWriteU8(dst, masterConfig.mixerMode); + sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode); sbufWriteU32(dst, featureMask()); @@ -1005,9 +1005,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_BLACKBOX_CONFIG: #ifdef BLACKBOX sbufWriteU8(dst, 1); //Blackbox supported - sbufWriteU8(dst, masterConfig.blackbox_device); - sbufWriteU8(dst, masterConfig.blackbox_rate_num); - sbufWriteU8(dst, masterConfig.blackbox_rate_denom); + sbufWriteU8(dst, masterConfig.blackboxConfig.device); + sbufWriteU8(dst, masterConfig.blackboxConfig.rate_num); + sbufWriteU8(dst, masterConfig.blackboxConfig.rate_denom); #else sbufWriteU8(dst, 0); // Blackbox not supported sbufWriteU8(dst, 0); @@ -1121,9 +1121,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_SENSOR_CONFIG: - sbufWriteU8(dst, masterConfig.acc_hardware); - sbufWriteU8(dst, masterConfig.baro_hardware); - sbufWriteU8(dst, masterConfig.mag_hardware); + sbufWriteU8(dst, masterConfig.sensorSelectionConfig.acc_hardware); + sbufWriteU8(dst, masterConfig.sensorSelectionConfig.baro_hardware); + sbufWriteU8(dst, masterConfig.sensorSelectionConfig.mag_hardware); break; case MSP_REBOOT: @@ -1247,8 +1247,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.accelerometerTrims.values.roll = sbufReadU16(src); break; case MSP_SET_ARMING_CONFIG: - masterConfig.auto_disarm_delay = sbufReadU8(src); - masterConfig.disarm_kill_switch = sbufReadU8(src); + masterConfig.armingConfig.auto_disarm_delay = sbufReadU8(src); + masterConfig.armingConfig.disarm_kill_switch = sbufReadU8(src); break; case MSP_SET_LOOP_TIME: @@ -1355,7 +1355,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.rxConfig.rssi_channel = sbufReadU8(src); sbufReadU8(src); - masterConfig.mag_declination = sbufReadU16(src) * 10; + masterConfig.compassConfig.mag_declination = sbufReadU16(src) * 10; masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI @@ -1476,9 +1476,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - masterConfig.acc_hardware = sbufReadU8(src); - masterConfig.baro_hardware = sbufReadU8(src); - masterConfig.mag_hardware = sbufReadU8(src); + masterConfig.sensorSelectionConfig.acc_hardware = sbufReadU8(src); + masterConfig.sensorSelectionConfig.baro_hardware = sbufReadU8(src); + masterConfig.sensorSelectionConfig.mag_hardware = sbufReadU8(src); break; case MSP_RESET_CONF: @@ -1510,9 +1510,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_BLACKBOX_CONFIG: // Don't allow config to be updated while Blackbox is logging if (blackboxMayEditConfig()) { - masterConfig.blackbox_device = sbufReadU8(src); - masterConfig.blackbox_rate_num = sbufReadU8(src); - masterConfig.blackbox_rate_denom = sbufReadU8(src); + masterConfig.blackboxConfig.device = sbufReadU8(src); + masterConfig.blackboxConfig.rate_num = sbufReadU8(src); + masterConfig.blackboxConfig.rate_denom = sbufReadU8(src); } break; #endif @@ -1657,7 +1657,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifndef USE_QUAD_MIXER_ONLY case MSP_SET_MIXER: - masterConfig.mixerMode = sbufReadU8(src); + masterConfig.mixerConfig.mixerMode = sbufReadU8(src); break; #endif @@ -1716,7 +1716,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_QUAD_MIXER_ONLY sbufReadU8(src); // mixerMode ignored #else - masterConfig.mixerMode = sbufReadU8(src); // mixerMode + masterConfig.mixerConfig.mixerMode = sbufReadU8(src); // mixerMode #endif featureClearAll(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index ba19b33470..c868a32130 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -153,7 +153,7 @@ static void taskUpdateRxMain(uint32_t currentTime) static void taskUpdateCompass(uint32_t currentTime) { if (sensors(SENSOR_MAG)) { - compassUpdate(currentTime, &masterConfig.magZero); + compassUpdate(currentTime, &masterConfig.sensorTrims.magZero); } } #endif diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index ba47924ce6..5f5d4dc63b 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -389,7 +389,7 @@ void mwArm(void) { static bool firstArmingCalibrationWasCompleted; - if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { + if (masterConfig.armingConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { gyroSetCalibrationCycles(); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; @@ -418,7 +418,7 @@ void mwArm(void) startBlackbox(); } #endif - disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero + disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero //beep to indicate arming #ifdef GPS @@ -548,7 +548,7 @@ void processRx(uint32_t currentTime) ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { - if (masterConfig.auto_disarm_delay != 0 + if (masterConfig.armingConfig.auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over @@ -561,9 +561,9 @@ void processRx(uint32_t currentTime) } } else { // throttle is not low - if (masterConfig.auto_disarm_delay != 0) { + if (masterConfig.armingConfig.auto_disarm_delay != 0) { // extend disarm time - disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; + disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000; } if (armedBeeperOn) { @@ -583,7 +583,7 @@ void processRx(uint32_t currentTime) } } - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch); + processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.armingConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); @@ -661,7 +661,7 @@ void processRx(uint32_t currentTime) DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } - if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { + if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } @@ -735,10 +735,10 @@ void subTaskMainSubprocesses(void) if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS - && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) + && !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) #endif - && masterConfig.mixerMode != MIXER_AIRPLANE - && masterConfig.mixerMode != MIXER_FLYING_WING + && masterConfig.mixerConfig.mixerMode != MIXER_AIRPLANE + && masterConfig.mixerConfig.mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 6c84dee566..6c7981bdfa 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -164,6 +164,13 @@ typedef struct rcControlsConfig_s { uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement } rcControlsConfig_t; +typedef struct armingConfig_s { + uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right + uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value + uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 + uint8_t small_angle; +} armingConfig_t; + bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ada8f1d734..3908bfd586 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -93,6 +93,7 @@ typedef struct mixer_s { } mixer_t; typedef struct mixerConfig_s { + uint8_t mixerMode; int8_t yaw_motor_direction; } mixerConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c84d59e332..19bb3e71e5 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -711,10 +711,10 @@ const clivalue_t valueTable[] = { { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 200, 32000 } }, - { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, - { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, - { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, .config.minmax = { 0, 60 } }, - { "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, .config.minmax = { 0, 180 } }, + { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, + { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, + { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.armingConfig.auto_disarm_delay, .config.minmax = { 0, 60 } }, + { "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.armingConfig.small_angle, .config.minmax = { 0, 180 } }, { "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, .config.minmax = { -1, 1 } }, @@ -860,7 +860,7 @@ const clivalue_t valueTable[] = { { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, - { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, + { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } }, { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0, 100 } }, { "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0, 100 } }, @@ -873,12 +873,12 @@ const clivalue_t valueTable[] = { { "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } }, { "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } }, { "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } }, - { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, + { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, #endif #ifdef MAG - { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, - { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, + { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, + { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.compassConfig.mag_declination, .config.minmax = { -18000, 18000 } }, #endif { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, @@ -922,10 +922,10 @@ const clivalue_t valueTable[] = { { "level_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 0.1, 3.0 } }, #ifdef BLACKBOX - { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } }, - { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } }, - { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } }, - { "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_on_motor_test, .config.lookup = { TABLE_OFF_ON } }, + { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackboxConfig.rate_num, .config.minmax = { 1, 32 } }, + { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackboxConfig.rate_denom, .config.minmax = { 1, 32 } }, + { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackboxConfig.device, .config.lookup = { TABLE_BLACKBOX_DEVICE } }, + { "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackboxConfig.on_motor_test, .config.lookup = { TABLE_OFF_ON } }, #endif #ifdef VTX @@ -936,9 +936,9 @@ const clivalue_t valueTable[] = { #endif #ifdef MAG - { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } }, - { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, - { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, + { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[X], .config.minmax = { -32768, 32767 } }, + { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, + { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, #endif #ifdef LED_STRIP { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledStripConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, @@ -2774,10 +2774,10 @@ static void printConfig(char *cmdline, bool doDiff) #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# mixer\r\n"); #endif - const bool equalsDefault = masterConfig.mixerMode == defaultConfig.mixerMode; + const bool equalsDefault = masterConfig.mixerConfig.mixerMode == defaultConfig.mixerConfig.mixerMode; const char *formatMixer = "mixer %s\r\n"; - cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerMode - 1]); - cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[masterConfig.mixerMode - 1]); + cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerConfig.mixerMode - 1]); + cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[masterConfig.mixerConfig.mixerMode - 1]); cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "\r\nmmix reset\r\n\r\n"); @@ -3072,7 +3072,7 @@ static void cliMixer(char *cmdline) len = strlen(cmdline); if (len == 0) { - cliPrintf("Mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]); + cliPrintf("Mixer: %s\r\n", mixerNames[masterConfig.mixerConfig.mixerMode - 1]); return; } else if (strncasecmp(cmdline, "list", len) == 0) { cliPrint("Available mixers: "); @@ -3091,7 +3091,7 @@ static void cliMixer(char *cmdline) return; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - masterConfig.mixerMode = i + 1; + masterConfig.mixerConfig.mixerMode = i + 1; break; } } diff --git a/src/main/main.c b/src/main/main.c index f57575873e..8cb9faf375 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -249,7 +249,7 @@ void init(void) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif - mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); + mixerInit(masterConfig.mixerConfig.mixerMode, masterConfig.customMotorMixer); #ifdef USE_SERVOS servoMixerInit(masterConfig.customServoMixer); #endif @@ -418,10 +418,8 @@ void init(void) #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, - masterConfig.acc_hardware, - masterConfig.mag_hardware, - masterConfig.baro_hardware, - masterConfig.mag_declination, + &masterConfig.sensorSelectionConfig, + masterConfig.compassConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom)) { // if gyro was not detected due to whatever reason, we give up now. @@ -550,7 +548,7 @@ void init(void) initBlackbox(); #endif - if (masterConfig.mixerMode == MIXER_GIMBAL) { + if (masterConfig.mixerConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(); diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 6e8e23779a..13abb7509c 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -30,6 +30,11 @@ typedef enum { MAG_MAX = MAG_AK8963 } magSensor_e; +typedef struct compassConfig_s { + int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ + // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. +} compassConfig_t; + void compassInit(void); union flightDynamicsTrims_u; void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index fe2645b8df..37bc9a20ec 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -623,9 +623,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, - uint8_t accHardwareToUse, - uint8_t magHardwareToUse, - uint8_t baroHardwareToUse, + sensorSelectionConfig_t *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator) @@ -651,7 +649,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, gyro.init(gyroLpf); // driver initialisation gyroInit(); // sensor initialisation - if (detectAcc(accHardwareToUse)) { + if (detectAcc(sensorSelectionConfig->acc_hardware)) { acc.acc_1G = 256; // set default acc.init(&acc); // driver initialisation accInit(gyro.targetLooptime); // sensor initialisation @@ -661,21 +659,18 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c - if (detectMag(magHardwareToUse)) { + if (detectMag(sensorSelectionConfig->mag_hardware)) { // calculate magnetic declination const int16_t deg = magDeclinationFromConfig / 100; const int16_t min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units } #else - UNUSED(magHardwareToUse); UNUSED(magDeclinationFromConfig); #endif #ifdef BARO - detectBaro(baroHardwareToUse); -#else - UNUSED(baroHardwareToUse); + detectBaro(sensorSelectionConfig->baro_hardware); #endif reconfigureAlignment(sensorAlignmentConfig); diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index 325979d548..95eec1f459 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -16,5 +16,5 @@ */ #pragma once - -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator); +struct sensorSelectionConfig_s; +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 9e6fdda92f..3507a2965b 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -69,3 +69,14 @@ typedef struct sensorAlignmentConfig_s { sensor_align_e acc_align; // acc alignment sensor_align_e mag_align; // mag alignment } sensorAlignmentConfig_t; + +typedef struct sensorSelectionConfig_s { + uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device + uint8_t baro_hardware; // Barometer hardware to use + uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device +} sensorSelectionConfig_t; + +typedef struct sensorTrims_s { + flightDynamicsTrims_t accZero; + flightDynamicsTrims_t magZero; +} sensorTrims_t; diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 0599007af1..cbb59814e9 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -48,7 +48,7 @@ void targetConfiguration(master_t *config) config->rxConfig.spektrum_sat_bind = 5; config->rxConfig.spektrum_sat_bind_autoreset = 1; config->gyro_sync_denom = 2; - config->mag_hardware = MAG_NONE; // disabled by default + config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default config->pid_process_denom = 1; if (hardwareMotorType == MOTOR_BRUSHED) { diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 57a1d62cbf..b34766b1e8 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -54,7 +54,7 @@ void targetConfiguration(master_t *config) config->batteryConfig.currentMeterOffset = 2500; config->batteryConfig.currentMeterScale = -667; config->gyro_sync_denom = 1; - config->mag_hardware = MAG_NONE; // disabled by default + config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default config->pid_process_denom = 1; if (hardwareMotorType == MOTOR_BRUSHED) { diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 1eaa69d6d5..ffcb7a3b76 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -51,8 +51,8 @@ void targetValidateConfiguration(master_t *config) if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) { intFeatureClear(FEATURE_SDCARD, &config->enabledFeatures); - if (config->blackbox_device == BLACKBOX_DEVICE_SDCARD) { - config->blackbox_device = BLACKBOX_DEVICE_FLASH; + if (config->blackboxConfig.device == BLACKBOX_DEVICE_SDCARD) { + config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH; } } } diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index 83f103e6ec..e01ae9d851 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -39,7 +39,7 @@ // alternative defaults settings for Colibri/Gemini targets void targetConfiguration(master_t *config) { - config->mixerMode = MIXER_HEX6X; + config->mixerConfig.mixerMode = MIXER_HEX6X; config->rxConfig.serialrx_provider = 2; config->motorConfig.minthrottle = 1070; @@ -48,7 +48,7 @@ void targetConfiguration(master_t *config) config->boardAlignment.pitchDegrees = 10; //config->rcControlsConfig.deadband = 10; //config->rcControlsConfig.yaw_deadband = 10; - config->mag_hardware = 1; + config->sensorSelectionConfig.mag_hardware = 1; config->profile[0].controlRateProfile[0].dynThrPID = 45; config->profile[0].controlRateProfile[0].tpa_breakpoint = 1700; diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 1981cfd144..36c6c7cddd 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -556,7 +556,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) // DEPRECATED - Use MSP_API_VERSION case BST_IDENT: bstWrite8(MW_VERSION); - bstWrite8(masterConfig.mixerMode); + bstWrite8(masterConfig.mixerConfig.mixerMode); bstWrite8(BST_PROTOCOL_VERSION); bstWrite32(CAP_DYNBALANCE); // "capability" break; @@ -683,8 +683,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A break; case BST_ARMING_CONFIG: - bstWrite8(masterConfig.auto_disarm_delay); - bstWrite8(masterConfig.disarm_kill_switch); + bstWrite8(masterConfig.armingConfig.auto_disarm_delay); + bstWrite8(masterConfig.armingConfig.disarm_kill_switch); break; case BST_LOOP_TIME: //bstWrite16(masterConfig.looptime); @@ -769,7 +769,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(masterConfig.rxConfig.rssi_channel); bstWrite8(0); - bstWrite16(masterConfig.mag_declination / 10); + bstWrite16(masterConfig.compassConfig.mag_declination / 10); bstWrite8(masterConfig.batteryConfig.vbatscale); bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); @@ -868,7 +868,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_MIXER: - bstWrite8(masterConfig.mixerMode); + bstWrite8(masterConfig.mixerConfig.mixerMode); break; case BST_RX_CONFIG: @@ -904,7 +904,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_BF_CONFIG: - bstWrite8(masterConfig.mixerMode); + bstWrite8(masterConfig.mixerConfig.mixerMode); bstWrite32(featureMask()); @@ -1033,8 +1033,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.accelerometerTrims.values.roll = bstRead16(); break; case BST_SET_ARMING_CONFIG: - masterConfig.auto_disarm_delay = bstRead8(); - masterConfig.disarm_kill_switch = bstRead8(); + masterConfig.armingConfig.auto_disarm_delay = bstRead8(); + masterConfig.armingConfig.disarm_kill_switch = bstRead8(); break; case BST_SET_LOOP_TIME: //masterConfig.looptime = bstRead16(); @@ -1132,7 +1132,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.rxConfig.rssi_channel = bstRead8(); bstRead8(); - masterConfig.mag_declination = bstRead16() * 10; + masterConfig.compassConfig.mag_declination = bstRead16() * 10; masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI @@ -1273,7 +1273,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) #ifndef USE_QUAD_MIXER_ONLY case BST_SET_MIXER: - masterConfig.mixerMode = bstRead8(); + masterConfig.mixerConfig.mixerMode = bstRead8(); break; #endif @@ -1319,7 +1319,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) #ifdef USE_QUAD_MIXER_ONLY bstRead8(); // mixerMode ignored #else - masterConfig.mixerMode = bstRead8(); // mixerMode + masterConfig.mixerConfig.mixerMode = bstRead8(); // mixerMode #endif featureClearAll(); diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 543db24752..5601a2c9eb 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -41,7 +41,7 @@ // alternative defaults settings for MULTIFLITEPICO targets void targetConfiguration(master_t *config) { - config->mag_hardware = MAG_NONE; // disabled by default + config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default config->batteryConfig.vbatscale = 100; config->batteryConfig.vbatresdivval = 15; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index f8663dcf7e..29dea7bd98 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -430,7 +430,7 @@ void mavlinkSendHUDAndHeartbeat(void) mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; uint8_t mavSystemType; - switch(masterConfig.mixerMode) + switch(masterConfig.mixerConfig.mixerMode) { case MIXER_TRI: mavSystemType = MAV_TYPE_TRICOPTER; From 7ec2031ce51fb71e8cc9e6eb868e630ddf951086 Mon Sep 17 00:00:00 2001 From: fishpepper Date: Thu, 24 Nov 2016 20:07:19 +0100 Subject: [PATCH 19/51] bugfix for sending when parity is enabled --- src/main/drivers/serial_uart.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index cd154bf9eb..9b083349d6 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -69,7 +69,14 @@ static void uartReconfigure(uartPort_t *uartPort) USART_Cmd(uartPort->USARTx, DISABLE); USART_InitStructure.USART_BaudRate = uartPort->port.baudRate; - USART_InitStructure.USART_WordLength = USART_WordLength_8b; + + // according to the stm32 documentation wordlen has to be 9 for parity bits + // this does not seem to matter for rx but will give bad data on tx! + if (uartPort->port.options & SERIAL_PARITY_EVEN) { + USART_InitStructure.USART_WordLength = USART_WordLength_9b; + } else { + USART_InitStructure.USART_WordLength = USART_WordLength_8b; + } USART_InitStructure.USART_StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_StopBits_2 : USART_StopBits_1; USART_InitStructure.USART_Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_Parity_Even : USART_Parity_No; From 0917b428cefc8e7a2c4acca38c02add27b2790f4 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 24 Nov 2016 22:33:46 +0100 Subject: [PATCH 20/51] AlienFlight F4 clean up - remove magic numbers - correct telemtry serial port configuration --- src/main/target/ALIENFLIGHTF4/config.c | 13 +++++++++---- src/main/target/ALIENFLIGHTF4/target.h | 2 ++ 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 57a1d62cbf..6470e5321e 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -48,18 +48,23 @@ #include "hardware_revision.h" +#define CURRENTOFFSET 2500 // ACS712/714-30A - 0A = 2.5V +#define CURRENTSCALE -667 // ACS712/714-30A - 66.666 mV/A inverted mode + +#define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz + // alternative defaults settings for AlienFlight targets void targetConfiguration(master_t *config) { - config->batteryConfig.currentMeterOffset = 2500; - config->batteryConfig.currentMeterScale = -667; + config->batteryConfig.currentMeterOffset = CURRENTOFFSET; + config->batteryConfig.currentMeterScale = CURRENTSCALE; config->gyro_sync_denom = 1; config->mag_hardware = MAG_NONE; // disabled by default config->pid_process_denom = 1; if (hardwareMotorType == MOTOR_BRUSHED) { config->motorConfig.minthrottle = 1000; - config->motorConfig.motorPwmRate = 32000; + config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED; config->motorConfig.useUnsyncedPwm = true; } @@ -71,7 +76,7 @@ void targetConfiguration(master_t *config) } else { config->rxConfig.serialrx_provider = SERIALRX_SBUS; config->rxConfig.sbus_inversion = 0; - config->serialConfig.portConfigs[SERIAL_PORT_USART2].functionMask = FUNCTION_TELEMETRY_FRSKY; + config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; config->telemetryConfig.telemetry_inversion = 0; intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->enabledFeatures); } diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index b0947c71ae..058759327e 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -168,6 +168,8 @@ #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER +#define TELEMETRY_UART SERIAL_PORT_USART1 + #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff From 54bab8b4183c1cf3029a07bb515a0c8492849e66 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 24 Nov 2016 22:37:11 +0000 Subject: [PATCH 21/51] Added CCM for all F3 and F4 targets --- src/main/target/link/stm32_flash_f303_128k.ld | 1 + src/main/target/link/stm32_flash_f303_256k.ld | 1 + src/main/target/link/stm32_flash_f405_opbl.ld | 2 +- src/main/target/link/stm32_flash_f411.ld | 1 + src/main/target/link/stm32_flash_f411_opbl.ld | 1 + 5 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/target/link/stm32_flash_f303_128k.ld b/src/main/target/link/stm32_flash_f303_128k.ld index 0050c64b5d..8d948e1d45 100644 --- a/src/main/target/link/stm32_flash_f303_128k.ld +++ b/src/main/target/link/stm32_flash_f303_128k.ld @@ -16,6 +16,7 @@ MEMORY FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K + CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } diff --git a/src/main/target/link/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld index 0d71ef1261..2f10b692d5 100644 --- a/src/main/target/link/stm32_flash_f303_256k.ld +++ b/src/main/target/link/stm32_flash_f303_256k.ld @@ -16,6 +16,7 @@ MEMORY FLASH_CONFIG (r) : ORIGIN = 0x0803F000, LENGTH = 4K RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K + CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld index 4fc707c55c..27d7544d89 100644 --- a/src/main/target/link/stm32_flash_f405_opbl.ld +++ b/src/main/target/link/stm32_flash_f405_opbl.ld @@ -25,8 +25,8 @@ MEMORY FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 992K FLASH_CONFIG (r): ORIGIN = 0x080FC000, LENGTH = 16K - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld index 789c757e9a..b7439b60d3 100644 --- a/src/main/target/link/stm32_flash_f411.ld +++ b/src/main/target/link/stm32_flash_f411.ld @@ -25,6 +25,7 @@ MEMORY FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 16K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld index 5658d5e797..e329397e61 100644 --- a/src/main/target/link/stm32_flash_f411_opbl.ld +++ b/src/main/target/link/stm32_flash_f411_opbl.ld @@ -26,6 +26,7 @@ MEMORY FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 16K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } From 0459775db99af572104fddf7eea9eeb782b87752 Mon Sep 17 00:00:00 2001 From: jamming Date: Fri, 25 Nov 2016 09:24:16 +0800 Subject: [PATCH 22/51] changes log --- src/main/target/KAKUTEF4/target.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index ee640c0f13..1013bda91a 100644 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -8,4 +8,6 @@ TARGET_SRC = \ drivers/compass_ak8963.c \ drivers/compass_hmc5883l.c \ drivers/max7456.c + + \ No newline at end of file From 2956df5ba1bb06154aace7681db6ff1744d59ec9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 25 Nov 2016 08:17:59 +0000 Subject: [PATCH 23/51] Update target.mk --- src/main/target/KAKUTEF4/target.mk | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index 1013bda91a..10665d1634 100644 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -7,7 +7,4 @@ TARGET_SRC = \ drivers/barometer_ms5611.c \ drivers/compass_ak8963.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c - - - \ No newline at end of file + drivers/max7456.c From b6583a1eb5a651da0d70eca2d6e70329cd874044 Mon Sep 17 00:00:00 2001 From: fishpepper Date: Fri, 25 Nov 2016 11:16:45 +0100 Subject: [PATCH 24/51] fixing serial passthrough on opened ports --- src/main/io/serial_cli.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 58697228a7..3e2d973737 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1441,6 +1441,12 @@ static void cliSerialPassthrough(char *cmdline) passThroughPort->mode, mode); serialSetMode(passThroughPort, mode); } + // If this port has a rx callback associated we need to remove it now. + // Otherwise no data will be pushed in the serial port buffer! + if (passThroughPort->rxCallback) { + printf("Removing rxCallback from port\r\n"); + passThroughPort->rxCallback = 0; + } } printf("Relaying data to device on port %d, Reset your board to exit " From f15bdcdce926d93ce2c23d62b6ecdc93d4d9cd1c Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 25 Nov 2016 22:24:30 +1100 Subject: [PATCH 25/51] Perform DMA check before writing to motors. --- src/main/drivers/pwm_output_stm32f3xx.c | 4 ++++ src/main/drivers/pwm_output_stm32f4xx.c | 4 ++++ src/main/drivers/pwm_output_stm32f7xx.c | 6 +++++- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 5adf8f63b0..b0db2103e7 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -71,6 +71,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value) motorDmaOutput_t * const motor = &dmaMotors[index]; + if (!motor->timerHardware->dmaChannel) { + return; + } + uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 0773efe08b..da5eb75440 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -69,6 +69,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value) motorDmaOutput_t * const motor = &dmaMotors[index]; + if (!motor->timerHardware->dmaStream) { + return; + } + uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 32ce620547..2a52652d1a 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -69,6 +69,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value) motorDmaOutput_t * const motor = &dmaMotors[index]; + if (!motor->timerHardware->dmaStream) { + return; + } + uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row @@ -88,7 +92,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value) packet <<= 1; } - if( HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) + if(HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ return; From 22c5b88fd0c6518ae692bc60d544778aefdd2824 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 25 Nov 2016 11:35:36 +0000 Subject: [PATCH 26/51] Updated CRSF telemetry --- src/main/telemetry/crsf.c | 68 ++++++------------------ src/main/telemetry/crsf.h | 1 - src/test/unit/telemetry_crsf_unittest.cc | 24 +++------ 3 files changed, 23 insertions(+), 70 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index dcf448edaf..79a40e2987 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -142,7 +142,7 @@ CRC: (uint8_t) Payload: int32_t Latitude ( degree / 10`000`000 ) int32_t Longitude (degree / 10`000`000 ) -uint16_t Groundspeed ( km/h / 100 ) +uint16_t Groundspeed ( km/h / 10 ) uint16_t GPS heading ( degree / 100 ) uint16 Altitude ( meter ­ 1000m offset ) uint8_t Satellites in use ( counter ) @@ -154,7 +154,7 @@ void crsfFrameGps(sbuf_t *dst) crsfSerialize8(dst, CRSF_FRAMETYPE_GPS); crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees crsfSerialize32(dst, GPS_coord[LON]); - crsfSerialize16(dst, GPS_speed * 36); // GPS_speed is in 0.1m/s + crsfSerialize16(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s crsfSerialize16(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10 //Send real GPS altitude only if it's reliable (there's a GPS fix) const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000; @@ -215,39 +215,6 @@ typedef enum { CRSF_RF_POWER_2000_mW = 6, } crsrRfPower_e; -/* -0x14 Link statistics -Uplink is the connection from the ground to the UAV and downlink the opposite direction. -Payload: -uint8_t UplinkRSSI Ant.1(dBm*­1) -uint8_t UplinkRSSI Ant.2(dBm*­1) -uint8_t Uplink Package success rate / Link quality ( % ) -int8_t Uplink SNR ( db ) -uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 ) -uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz) -uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW ) -uint8_t Downlink RSSI ( dBm * ­-1 ) -uint8_t Downlink package success rate / Link quality ( % ) -int8_t Downlink SNR ( db ) -*/ - -void crsfFrameLinkStatistics(sbuf_t *dst) -{ - // use sbufWrite since CRC does not include frame length - sbufWriteU8(dst, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - crsfSerialize8(dst, CRSF_FRAMETYPE_LINK_STATISTICS); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); -} - /* 0x1E Attitude Payload: @@ -301,14 +268,10 @@ void crsfFrameFlightMode(sbuf_t *dst) #define BV(x) (1 << (x)) // bit value // schedule array to decide how often each type of frame is sent -#define CRSF_SCHEDULE_COUNT 5 -static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT] = { - BV(CRSF_FRAME_ATTITUDE), - BV(CRSF_FRAME_BATTERY_SENSOR), - BV(CRSF_FRAME_LINK_STATISTICS), - BV(CRSF_FRAME_FLIGHT_MODE), - BV(CRSF_FRAME_GPS), -}; +#define CRSF_SCHEDULE_COUNT_MAX 5 +static uint8_t crsfScheduleCount; +static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; + static void processCrsf(void) { @@ -328,11 +291,6 @@ static void processCrsf(void) crsfFrameBatterySensor(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_LINK_STATISTICS)) { - crsfInitializeFrame(dst); - crsfFrameLinkStatistics(dst); - crsfFinalize(dst); - } if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) { crsfInitializeFrame(dst); crsfFrameFlightMode(dst); @@ -345,7 +303,7 @@ static void processCrsf(void) crsfFinalize(dst); } #endif - crsfScheduleIndex = (crsfScheduleIndex + 1) % CRSF_SCHEDULE_COUNT; + crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } void initCrsfTelemetry(void) @@ -353,6 +311,15 @@ void initCrsfTelemetry(void) // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) // and feature is enabled, if so, set CRSF telemetry enabled crsfTelemetryEnabled = crsfRxIsActive(); + int index = 0; + crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE); + crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR); + crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE); + if (feature(FEATURE_GPS)) { + crsfSchedule[index++] = BV(CRSF_FRAME_GPS); + } + crsfScheduleCount = (uint8_t)index; + } bool checkCrsfTelemetryState(void) @@ -396,9 +363,6 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) case CRSF_FRAME_BATTERY_SENSOR: crsfFrameBatterySensor(sbuf); break; - case CRSF_FRAME_LINK_STATISTICS: - crsfFrameLinkStatistics(sbuf); - break; case CRSF_FRAME_FLIGHT_MODE: crsfFrameFlightMode(sbuf); break; diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 3b6c5d4cca..d6a126a29e 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -23,7 +23,6 @@ typedef enum { CRSF_FRAME_START = 0, CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, CRSF_FRAME_BATTERY_SENSOR, - CRSF_FRAME_LINK_STATISTICS, CRSF_FRAME_FLIGHT_MODE, CRSF_FRAME_GPS, CRSF_FRAME_COUNT diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 028b0e75ac..6f1762c572 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -68,10 +68,11 @@ uint8_t crfsCrc(uint8_t *frame, int frameLen) } return crc; } + /* int32_t Latitude ( degree / 10`000`000 ) int32_t Longitude (degree / 10`000`000 ) -uint16_t Groundspeed ( km/h / 100 ) +uint16_t Groundspeed ( km/h / 10 ) uint16_t GPS heading ( degree / 100 ) uint16 Altitude ( meter ­ 1000m offset ) uint8_t Satellites in use ( counter ) @@ -81,8 +82,7 @@ uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_altitude; // altitude in m uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_ground_course = 0; // degrees * 10 - - */ +*/ #define FRAME_HEADER_FOOTER_LEN 4 TEST(TelemetryCrsfTest, TestGPS) @@ -112,7 +112,7 @@ TEST(TelemetryCrsfTest, TestGPS) GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER; ENABLE_STATE(GPS_FIX); GPS_altitude = 2345; // altitude in m - GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h + GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 GPS_numSat = 9; GPS_ground_course = 1479; // degrees * 10 frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); @@ -121,7 +121,7 @@ TEST(TelemetryCrsfTest, TestGPS) longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10]; EXPECT_EQ(1630000000, longitude); groundSpeed = frame[11] << 8 | frame[12]; - EXPECT_EQ(5868, groundSpeed); + EXPECT_EQ(587, groundSpeed); GPSheading = frame[13] << 8 | frame[14]; EXPECT_EQ(14790, GPSheading); altitude = frame[15] << 8 | frame[16]; @@ -169,18 +169,6 @@ TEST(TelemetryCrsfTest, TestBattery) EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]); } -TEST(TelemetryCrsfTest, TestLinkStatistics) -{ - uint8_t frame[CRSF_FRAME_SIZE_MAX]; - - int frameLen = getCrsfFrame(frame, CRSF_FRAME_LINK_STATISTICS); - EXPECT_EQ(CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address - EXPECT_EQ(12, frame[1]); // length - EXPECT_EQ(0x14, frame[2]); // type - EXPECT_EQ(crfsCrc(frame, frameLen), frame[13]); -} - TEST(TelemetryCrsfTest, TestAttitude) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; @@ -303,6 +291,8 @@ void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} uint32_t micros(void) {return 0;} +bool feature(uint32_t) {return true;} + uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} uint8_t serialRead(serialPort_t *) {return 0;} From d4b8ffe008569fc09bd92fa7bd1cb6c8ec10940b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 25 Nov 2016 11:55:55 +0000 Subject: [PATCH 27/51] Removed cli play_sound command for F1 targets --- src/main/io/serial_cli.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0871fd00bf..2fbd504378 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -127,7 +127,9 @@ static void cliExit(char *cmdline); static void cliFeature(char *cmdline); static void cliMotor(char *cmdline); static void cliName(char *cmdline); +#if (FLASH_SIZE > 128) static void cliPlaySound(char *cmdline); +#endif static void cliProfile(char *cmdline); static void cliRateProfile(char *cmdline); static void cliReboot(void); @@ -331,8 +333,10 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix), CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), +#if (FLASH_SIZE > 128) CLI_COMMAND_DEF("play_sound", NULL, "[]\r\n", cliPlaySound), +#endif CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), @@ -3144,11 +3148,9 @@ static void cliMotor(char *cmdline) cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); } +#if (FLASH_SIZE > 128) static void cliPlaySound(char *cmdline) { -#if (FLASH_SIZE <= 64) && !defined(CLI_MINIMAL_VERBOSITY) - UNUSED(cmdline); -#else int i; const char *name; static int lastSoundIdx = -1; @@ -3178,8 +3180,8 @@ static void cliPlaySound(char *cmdline) beeperSilence(); cliPrintf("Playing sound %d: %s\r\n", i, name); beeper(beeperModeForTableIndex(i)); -#endif } +#endif static void cliProfile(char *cmdline) { From 032d9354ed7cd44a693ad67849e2036b1757bac6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 12:11:08 +0000 Subject: [PATCH 28/51] Use function pointers to tidy PID filter handling --- src/main/flight/pid.c | 152 +++++++++++++++++++++++++----------------- 1 file changed, 90 insertions(+), 62 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cb116542ee..57adeacbf7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -21,6 +21,7 @@ #include +#include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" @@ -74,33 +75,77 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -pt1Filter_t deltaFilter[3]; -pt1Filter_t yawFilter; -biquadFilter_t dtermFilterLpf[3]; -biquadFilter_t dtermFilterNotch[3]; -bool dtermNotchInitialised; -bool dtermBiquadLpfInitialised; -firFilterDenoise_t dtermDenoisingState[3]; +static filterApplyFnPtr dtermNotchFilterApplyFn; +static void *dtermFilterNotch[2]; +static filterApplyFnPtr dtermLpfApplyFn; +static void *dtermFilterLpf[2]; +static filterApplyFnPtr ptermYawFilterApplyFn; +static void *ptermYawFilter; static void pidInitFilters(const pidProfile_t *pidProfile) { - static uint8_t lowpassFilterType; + static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed + static biquadFilter_t biquadFilterNotch[2]; + static pt1Filter_t pt1Filter[2]; + static biquadFilter_t biquadFilter[2]; + static firFilterDenoise_t denoisingFilter[2]; + static pt1Filter_t pt1FilterYaw; - if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { - float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); - for (int axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); - dtermNotchInitialised = true; + if (initialized) { + return; } - if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) { - if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { - for (int axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); - } + BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 + const float dT = (float)targetPidLooptime * 0.000001f; - if (pidProfile->dterm_filter_type == FILTER_FIR) { - for (int axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + if (pidProfile->dterm_notch_hz == 0) { + dtermNotchFilterApplyFn = nullFilterApply; + } else { + dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterNotch[axis] = &biquadFilterNotch[axis]; + biquadFilterInit(dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); } - lowpassFilterType = pidProfile->dterm_filter_type; + } + + if (pidProfile->dterm_lpf_hz == 0) { + dtermLpfApplyFn = nullFilterApply; + } else { + switch (pidProfile->dterm_filter_type) { + default: + dtermLpfApplyFn = nullFilterApply; + break; + case FILTER_PT1: + dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterLpf[axis] = &pt1Filter[axis]; + pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT); + } + break; + case FILTER_BIQUAD: + dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterLpf[axis] = &biquadFilter[axis]; + biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + } + break; + case FILTER_FIR: + dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + dtermFilterLpf[axis] = &denoisingFilter[axis]; + firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + } + break; + } + } + + if (pidProfile->yaw_lpf_hz == 0) { + ptermYawFilterApplyFn = nullFilterApply; + } else { + ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; + ptermYawFilter = &pt1FilterYaw; + pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT); } } @@ -156,7 +201,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // ----------PID controller---------- const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - for (int axis = 0; axis < 3; axis++) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { static uint8_t configP[3], configI[3], configD[3]; @@ -177,9 +222,9 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity; if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + const float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; if (ABS(currentVelocity) > maxVelocity) { setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } @@ -210,30 +255,28 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- + + // -----calculate error rate const float errorRate = setpointRate[axis] - PVRate; // r - y // -----calculate P component and add Dynamic Part based on stick input float PTerm = Kp[axis] * errorRate * tpaFactor; - // -----calculate I component. + // -----calculate I component // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); + const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); + const float itermScaler = setpointRateScaler * kiThrottleGain; - // Handle All windup Scenarios + float ITerm = errorGyroIf[axis]; + ITerm += Ki[axis] * errorRate * dT * itermScaler;; // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; + ITerm = constrainf(ITerm, -250.0f, 250.0f); + errorGyroIf[axis] = ITerm; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * dT * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - const float ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - float DTerm; - if (axis != YAW) { + // -----calculate D component (Yaw D not yet supported) + float DTerm = 0.0; + if (axis != FD_YAW) { static float previousSetpoint[3]; float dynC = c[axis]; if (pidProfile->setpointRelaxRatio < 100) { @@ -248,38 +291,23 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } previousSetpoint[axis] = setpointRate[axis]; const float rD = dynC * setpointRate[axis] - PVRate; // cr - y - float delta = rD - lastRateError[axis]; + // Divide rate change by dT to get differential (ie dr/dt) + const float delta = (rD - lastRateError[axis]) / dT; lastRateError[axis] = rD; - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / dT); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (pidProfile->dterm_filter_type == FILTER_BIQUAD) - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - else if (pidProfile->dterm_filter_type == FILTER_PT1) - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, dT); - else - delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta); - } - DTerm = Kd[axis] * delta * tpaFactor; + DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm); + + // apply filters + DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm); + DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm); - // -----calculate total PID output - axisPIDf[axis] = PTerm + ITerm + DTerm; } else { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT); - - axisPIDf[axis] = PTerm + ITerm; - - DTerm = 0.0f; // needed for blackbox + PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm); } + // -----calculate total PID output + axisPIDf[axis] = PTerm + ITerm + DTerm; // Disable PID control at zero throttle if (!pidStabilisationEnabled) axisPIDf[axis] = 0; From a8bf4855395cf5bd117fc589a44d9b3273a46c02 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 25 Nov 2016 12:54:41 +0000 Subject: [PATCH 29/51] Updated DECIDEGREES_TO_RADIANS10000 conversion --- src/main/telemetry/crsf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 79a40e2987..a5f619d6dd 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -223,7 +223,7 @@ int16_t Roll angle ( rad / 10000 ) int16_t Yaw angle ( rad / 10000 ) */ -#define DECIDEGREES_TO_RADIANS10000(angle) (1000.0f * (angle) * RAD) +#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD)) void crsfFrameAttitude(sbuf_t *dst) { From 79ad21c813836203e35bb4869abd7dd7bed056e4 Mon Sep 17 00:00:00 2001 From: francoisduf Date: Fri, 25 Nov 2016 20:02:56 +0100 Subject: [PATCH 30/51] Dshot support --- src/main/target/YUPIF4/target.c | 16 +++++++++------- src/main/target/YUPIF4/target.h | 8 ++++---- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 74a21fdb61..16f17c726d 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -20,15 +20,17 @@ #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" + const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // MS5 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // MS6 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1 ), // S4_OUT - DMA1_ST6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0 ), // S5_OUT - DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST2 }; diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index b25ea293c7..1a38664fc7 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -109,11 +109,11 @@ // SPI Ports #define USE_SPI -#define USE_SPI_DEVICE_1 //MPU6500 +#define USE_SPI_DEVICE_1 //Gyro #define SPI1_NSS_PIN PA4 #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 +#define SPI1_MOSI_PIN PA7 /* #define USE_SPI_DEVICE_2 //Free @@ -138,7 +138,7 @@ #define VBAT_ADC_PIN PC1 #define RSSI_ADC_GPIO_PIN PC0 - +#define USE_DSHOT #define LED_STRIP // Default configuration @@ -156,4 +156,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 7 -#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(8)) +#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8)) From 515cb55d3424e3f30c8518529334a47846301eea Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 26 Nov 2016 07:16:33 +1100 Subject: [PATCH 31/51] Enabling pull-ups for F4 serial --- src/main/drivers/serial_uart_stm32f4xx.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 2f72317eaf..79d3e18ad0 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -345,12 +345,12 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po else { if (mode & MODE_TX) { IOInit(tx, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(tx, IOCFG_AF_PP_UP, uart->af); } if (mode & MODE_RX) { IOInit(rx, OWNER_SERIAL_RX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(rx, IOCFG_AF_PP_UP, uart->af); } } From 519c3d243b0e30663342bc110b737f61fa1e291c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 25 Nov 2016 23:24:59 +0100 Subject: [PATCH 32/51] Fix findSerialPortIndexByIdentifier() function --- src/main/io/serial.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index ebc242cc21..4c9bd96678 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -110,8 +110,7 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate) int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier) { for (int index = 0; index < SERIAL_PORT_COUNT; index++) { - const serialPortUsage_t *candidate = &serialPortUsageList[index]; - if (candidate->identifier == identifier) { + if (serialPortIdentifiers[index] == identifier) { return index; } } From af8453c2246a8ffe27de129beffcc097cd72fb52 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 26 Nov 2016 00:08:18 +0100 Subject: [PATCH 33/51] Fix Filter Init Loop --- src/main/flight/pid.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 57adeacbf7..21dfbcdd95 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -147,6 +147,8 @@ static void pidInitFilters(const pidProfile_t *pidProfile) ptermYawFilter = &pt1FilterYaw; pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT); } + + initialized = true; } // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. From 12d3a39d84062708814b082940e6e9a4a0b8c3a0 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 25 Nov 2016 23:55:32 +0100 Subject: [PATCH 34/51] Fix PWM rate for Oneshot125 on AlienFlight F3 and F4 --- src/main/target/ALIENFLIGHTF3/config.c | 3 +-- src/main/target/ALIENFLIGHTF4/config.c | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index cbb59814e9..a7cfc3a91e 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -47,14 +47,13 @@ void targetConfiguration(master_t *config) { config->rxConfig.spektrum_sat_bind = 5; config->rxConfig.spektrum_sat_bind_autoreset = 1; - config->gyro_sync_denom = 2; config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default - config->pid_process_denom = 1; if (hardwareMotorType == MOTOR_BRUSHED) { config->motorConfig.minthrottle = 1000; config->motorConfig.motorPwmRate = 32000; config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED; + config->pid_process_denom = 2; config->motorConfig.useUnsyncedPwm = true; } diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 36975b2a2d..216d9e486f 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -58,14 +58,13 @@ void targetConfiguration(master_t *config) { config->batteryConfig.currentMeterOffset = CURRENTOFFSET; config->batteryConfig.currentMeterScale = CURRENTSCALE; - config->gyro_sync_denom = 1; config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default - config->pid_process_denom = 1; if (hardwareMotorType == MOTOR_BRUSHED) { config->motorConfig.minthrottle = 1000; config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED; + config->pid_process_denom = 1; config->motorConfig.useUnsyncedPwm = true; } From e9407f3065e499ba2b301ee07f04f6ed9c8667b1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 14:09:08 +0000 Subject: [PATCH 35/51] Tidied initialisation, especially PID filters --- src/main/drivers/accgyro.h | 9 +++++++++ src/main/drivers/gyro_sync.c | 9 --------- src/main/fc/config.c | 29 ++++++++++++++++++++--------- src/main/fc/config.h | 1 + src/main/fc/fc_msp.c | 12 ++++++++++++ src/main/flight/pid.c | 6 ++---- src/main/flight/pid.h | 3 ++- src/main/main.c | 23 +++++------------------ src/main/sensors/initialisation.c | 28 +++++++++++++++++++++++++++- src/main/sensors/initialisation.h | 4 +++- src/main/target/NAZE/config.c | 2 +- 11 files changed, 82 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 0020d770fa..7e43b73078 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -23,6 +23,15 @@ #define MPU_I2C_INSTANCE I2C_DEVICE #endif +#define GYRO_LPF_256HZ 0 +#define GYRO_LPF_188HZ 1 +#define GYRO_LPF_98HZ 2 +#define GYRO_LPF_42HZ 3 +#define GYRO_LPF_20HZ 4 +#define GYRO_LPF_10HZ 5 +#define GYRO_LPF_5HZ 6 +#define GYRO_LPF_NONE 7 + typedef struct gyro_s { sensorGyroInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index dbdc2244c9..4e9f02622b 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -23,15 +23,6 @@ bool gyroSyncCheckUpdate(const gyro_t *gyro) return gyro->intStatus(); } -#define GYRO_LPF_256HZ 0 -#define GYRO_LPF_188HZ 1 -#define GYRO_LPF_98HZ 2 -#define GYRO_LPF_42HZ 3 -#define GYRO_LPF_20HZ 4 -#define GYRO_LPF_10HZ 5 -#define GYRO_LPF_5HZ 6 -#define GYRO_LPF_NONE 7 - uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) { int gyroSamplePeriod; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 223fdfc9f6..4d002f16d8 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -564,7 +564,7 @@ void createDefaultConfig(master_t *config) config->current_profile_index = 0; // default profile config->dcm_kp = 2500; // 1.0 * 10000 config->dcm_ki = 0; // 0.003 * 10000 - config->gyro_lpf = 0; // 256HZ default + config->gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X config->gyro_sync_denom = 8; config->pid_process_denom = 1; @@ -830,13 +830,6 @@ void activateConfig(void) ¤tProfile->pidProfile ); - // Prevent invalid notch cutoff - if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1) - masterConfig.gyro_soft_notch_hz_1 = 0; - - if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) - masterConfig.gyro_soft_notch_hz_2 = 0; - gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz_1, @@ -998,12 +991,30 @@ void validateAndFixConfig(void) if (!isSerialConfigValid(serialConfig)) { resetSerialConfig(serialConfig); } - + + validateAndFixGyroConfig(); + #if defined(TARGET_VALIDATECONFIG) targetValidateConfiguration(&masterConfig); #endif } +void validateAndFixGyroConfig(void) +{ + // Prevent invalid notch cutoff + if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1) { + masterConfig.gyro_soft_notch_hz_1 = 0; + } + if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) { + masterConfig.gyro_soft_notch_hz_2 = 0; + } + + if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyro_lpf != GYRO_LPF_NONE) { + masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + masterConfig.gyro_sync_denom = 1; + } +} + void readEEPROMAndNotify(void) { // re-read written data diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 6f903e5d83..3a3c4334a6 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -75,6 +75,7 @@ void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); void validateAndFixConfig(void); +void validateAndFixGyroConfig(void); void activateConfig(void); uint8_t getCurrentProfile(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index df38554c72..adca0a3667 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1458,6 +1458,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.gyro_soft_notch_hz_2 = sbufReadU16(src); masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src); } + // reinitialize the gyro filters with the new values + validateAndFixGyroConfig(); + gyroUseConfig(&masterConfig.gyroConfig, + masterConfig.gyro_soft_lpf_hz, + masterConfig.gyro_soft_notch_hz_1, + masterConfig.gyro_soft_notch_cutoff_1, + masterConfig.gyro_soft_notch_hz_2, + masterConfig.gyro_soft_notch_cutoff_2, + masterConfig.gyro_soft_type); + gyroInit(); + // reinitialize the PID filters with the new values + pidInitFilters(¤tProfile->pidProfile); break; case MSP_SET_PID_ADVANCED: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 21dfbcdd95..cccfad9ab0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -56,7 +56,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; static float errorGyroIf[3]; -void setTargetPidLooptime(uint32_t pidLooptime) +void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; } @@ -82,7 +82,7 @@ static void *dtermFilterLpf[2]; static filterApplyFnPtr ptermYawFilterApplyFn; static void *ptermYawFilter; -static void pidInitFilters(const pidProfile_t *pidProfile) +void pidInitFilters(const pidProfile_t *pidProfile) { static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed static biquadFilter_t biquadFilterNotch[2]; @@ -166,8 +166,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio dT = (float)targetPidLooptime * 0.000001f; } - pidInitFilters(pidProfile); - float horizonLevelStrength = 1; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f96652c92d..e059ed84c7 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -104,5 +104,6 @@ extern uint8_t PIDweight[3]; void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); -void setTargetPidLooptime(uint32_t pidLooptime); +void pidSetTargetLooptime(uint32_t pidLooptime); +void pidInitFilters(const pidProfile_t *pidProfile); diff --git a/src/main/main.c b/src/main/main.c index 8cb9faf375..4737b62290 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -421,7 +421,8 @@ void init(void) &masterConfig.sensorSelectionConfig, masterConfig.compassConfig.mag_declination, masterConfig.gyro_lpf, - masterConfig.gyro_sync_denom)) { + masterConfig.gyro_sync_denom, + &masterConfig.sonarConfig)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } @@ -443,10 +444,9 @@ void init(void) LED0_OFF; LED1_OFF; -#ifdef MAG - if (sensors(SENSOR_MAG)) - compassInit(); -#endif + // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime() + pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime + pidInitFilters(¤tProfile->pidProfile); imuInit(); @@ -478,12 +478,6 @@ void init(void) } #endif -#ifdef SONAR - if (feature(FEATURE_SONAR)) { - sonarInit(&masterConfig.sonarConfig); - } -#endif - #ifdef LED_STRIP ledStripInit(&masterConfig.ledStripConfig); @@ -537,13 +531,6 @@ void init(void) } #endif - if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) { - masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - masterConfig.gyro_sync_denom = 1; - } - - setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime - #ifdef BLACKBOX initBlackbox(); #endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 37bc9a20ec..8d1e20ecbc 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,8 @@ #include "common/axis.h" +#include "config/feature.h" + #include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" @@ -61,6 +63,7 @@ #include "drivers/sonar_hcsr04.h" +#include "fc/config.h" #include "fc/runtime_config.h" #include "sensors/sensors.h" @@ -609,6 +612,19 @@ retry: } #endif +#ifdef SONAR +static bool detectSonar(void) +{ + if (feature(FEATURE_SONAR)) { + // the user has set the sonar feature, so assume they have an HC-SR04 plugged in, + // since there is no way to detect it + sensorsSet(SENSOR_SONAR); + return true; + } + return false; +} +#endif + void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) { if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { @@ -626,7 +642,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, sensorSelectionConfig_t *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, - uint8_t gyroSyncDenominator) + uint8_t gyroSyncDenominator, + const sonarConfig_t *sonarConfig) { memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); @@ -664,6 +681,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, const int16_t deg = magDeclinationFromConfig / 100; const int16_t min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units + compassInit(); } #else UNUSED(magDeclinationFromConfig); @@ -673,6 +691,14 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, detectBaro(sensorSelectionConfig->baro_hardware); #endif +#ifdef SONAR + if (detectSonar()) { + sonarInit(sonarConfig); + } +#else + UNUSED(sonarConfig); +#endif + reconfigureAlignment(sensorAlignmentConfig); return true; diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index 95eec1f459..29ce3035e9 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -16,5 +16,7 @@ */ #pragma once + struct sensorSelectionConfig_s; -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator); +struct sonarConfig_s; +bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, const struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator, const struct sonarConfig_s *sonarConfig); diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 9fdbca437c..e279937540 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -43,7 +43,7 @@ void targetConfiguration(master_t *config) config->motorConfig.minthrottle = 1049; - config->gyro_lpf = 1; + config->gyro_lpf = GYRO_LPF_188HZ; config->gyro_soft_lpf_hz = 100; config->gyro_soft_notch_hz_1 = 0; config->gyro_soft_notch_hz_2 = 0; From 747649ce3af3c605407536e126434517c01787e0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 15:04:19 +0000 Subject: [PATCH 36/51] Fixed build when SONAR undefined --- src/main/main.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 4737b62290..5fa43d3876 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -417,12 +417,17 @@ void init(void) } #endif +#ifdef SONAR + const sonarConfig_t *sonarConfig = &masterConfig.sonarConfig; +#else + const void *sonarConfig = NULL; +#endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, &masterConfig.sensorSelectionConfig, masterConfig.compassConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom, - &masterConfig.sonarConfig)) { + sonarConfig)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } From fa4d04b5a7aadf3ad144e77025583050ddcc6f43 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 15:12:43 +0000 Subject: [PATCH 37/51] Fixed gyro Q factor calculation --- src/main/sensors/gyro.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d27c2eb17c..f5b51ea1bf 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -68,10 +68,14 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; gyroSoftNotchHz1 = gyro_soft_notch_hz_1; + if (gyro_soft_notch_hz_1) { + gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); + } gyroSoftNotchHz2 = gyro_soft_notch_hz_2; + if (gyro_soft_notch_hz_2) { + gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); + } gyroSoftLpfType = gyro_soft_lpf_type; - gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); - gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); } void gyroInit(void) From 2c7d06a94e9343ed105a82cc7711bf03ed3553f9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 19:04:14 +0000 Subject: [PATCH 38/51] Moved gyro filter settings into gyroConfig --- src/main/blackbox/blackbox.c | 12 ++++++------ src/main/cms/cms_menu_imu.c | 10 +++++----- src/main/config/config_master.h | 8 ++------ src/main/fc/config.c | 28 +++++++++++----------------- src/main/fc/fc_msp.c | 28 +++++++++++----------------- src/main/fc/mw.c | 2 +- src/main/io/serial_cli.c | 12 ++++++------ src/main/sensors/gyro.c | 24 +++++++++--------------- src/main/sensors/gyro.h | 16 ++++++++-------- 9 files changed, 59 insertions(+), 81 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4d10e90778..ed4ba0137d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1263,12 +1263,12 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); - BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyro_soft_type); - BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyro_soft_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyro_soft_notch_hz_1, - masterConfig.gyro_soft_notch_hz_2); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyro_soft_notch_cutoff_1, - masterConfig.gyro_soft_notch_cutoff_2); + BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1, + masterConfig.gyroConfig.gyro_soft_notch_hz_2); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, + masterConfig.gyroConfig.gyro_soft_notch_cutoff_2); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.sensorSelectionConfig.baro_hardware); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 24b988cb07..57ecc67ba9 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -282,11 +282,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, - { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, - { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, - { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, - { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, - { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, + { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyroConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, + { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, + { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, + { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, + { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 827be582b0..e74ed5cbcd 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -91,14 +91,10 @@ typedef struct master_s { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. + + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_sync_denom; // Gyro sample divider - uint8_t gyro_soft_type; // Gyro Filter Type - uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz - uint16_t gyro_soft_notch_hz_1; // Biquad gyro notch hz - uint16_t gyro_soft_notch_cutoff_1; // Biquad gyro notch low cutoff - uint16_t gyro_soft_notch_hz_2; // Biquad gyro notch hz - uint16_t gyro_soft_notch_cutoff_2; // Biquad gyro notch low cutoff uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 4d002f16d8..794bd669f1 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -575,12 +575,12 @@ void createDefaultConfig(master_t *config) config->gyro_sync_denom = 4; config->pid_process_denom = 2; #endif - config->gyro_soft_type = FILTER_PT1; - config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz_1 = 400; - config->gyro_soft_notch_cutoff_1 = 300; - config->gyro_soft_notch_hz_2 = 200; - config->gyro_soft_notch_cutoff_2 = 100; + config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1; + config->gyroConfig.gyro_soft_lpf_hz = 90; + config->gyroConfig.gyro_soft_notch_hz_1 = 400; + config->gyroConfig.gyro_soft_notch_cutoff_1 = 300; + config->gyroConfig.gyro_soft_notch_hz_2 = 200; + config->gyroConfig.gyro_soft_notch_cutoff_2 = 100; config->debug_mode = DEBUG_NONE; @@ -830,13 +830,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, - masterConfig.gyro_soft_lpf_hz, - masterConfig.gyro_soft_notch_hz_1, - masterConfig.gyro_soft_notch_cutoff_1, - masterConfig.gyro_soft_notch_hz_2, - masterConfig.gyro_soft_notch_cutoff_2, - masterConfig.gyro_soft_type); + gyroUseConfig(&masterConfig.gyroConfig); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); @@ -1002,11 +996,11 @@ void validateAndFixConfig(void) void validateAndFixGyroConfig(void) { // Prevent invalid notch cutoff - if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1) { - masterConfig.gyro_soft_notch_hz_1 = 0; + if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) { + masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0; } - if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) { - masterConfig.gyro_soft_notch_hz_2 = 0; + if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) { + masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0; } if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyro_lpf != GYRO_LPF_NONE) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index adca0a3667..664dc628e4 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1094,15 +1094,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_FILTER_CONFIG : - sbufWriteU8(dst, masterConfig.gyro_soft_lpf_hz); + sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz); sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz); sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_1); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_1); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1); sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz); sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_2); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_2); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2); break; case MSP_PID_ADVANCED: @@ -1445,28 +1445,22 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_FILTER_CONFIG: - masterConfig.gyro_soft_lpf_hz = sbufReadU8(src); + masterConfig.gyroConfig.gyro_soft_lpf_hz = sbufReadU8(src); currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src); currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src); if (dataSize > 5) { - masterConfig.gyro_soft_notch_hz_1 = sbufReadU16(src); - masterConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src); } if (dataSize > 13) { - masterConfig.gyro_soft_notch_hz_2 = sbufReadU16(src); - masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src); } // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); - gyroUseConfig(&masterConfig.gyroConfig, - masterConfig.gyro_soft_lpf_hz, - masterConfig.gyro_soft_notch_hz_1, - masterConfig.gyro_soft_notch_cutoff_1, - masterConfig.gyro_soft_notch_hz_2, - masterConfig.gyro_soft_notch_cutoff_2, - masterConfig.gyro_soft_type); + gyroUseConfig(&masterConfig.gyroConfig); gyroInit(); // reinitialize the PID filters with the new values pidInitFilters(¤tProfile->pidProfile); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 5f5d4dc63b..aab4497f79 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -803,7 +803,7 @@ void subTaskMotorUpdate(void) uint8_t setPidUpdateCountDown(void) { - if (masterConfig.gyro_soft_lpf_hz) { + if (masterConfig.gyroConfig.gyro_soft_lpf_hz) { return masterConfig.pid_process_denom - 1; } else { return 1; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a23c0b15d2..b3e6b8f52e 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -809,12 +809,12 @@ const clivalue_t valueTable[] = { { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, - { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, - { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, - { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, - { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, - { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, + { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, + { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, + { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, + { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f5b51ea1bf..37855041e8 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -57,25 +57,19 @@ static void *notchFilter1[3]; static filterApplyFnPtr notchFilter2ApplyFn; static void *notchFilter2[3]; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, - uint8_t gyro_soft_lpf_hz, - uint16_t gyro_soft_notch_hz_1, - uint16_t gyro_soft_notch_cutoff_1, - uint16_t gyro_soft_notch_hz_2, - uint16_t gyro_soft_notch_cutoff_2, - uint8_t gyro_soft_lpf_type) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse) { gyroConfig = gyroConfigToUse; - gyroSoftLpfHz = gyro_soft_lpf_hz; - gyroSoftNotchHz1 = gyro_soft_notch_hz_1; - if (gyro_soft_notch_hz_1) { - gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); + gyroSoftLpfHz = gyroConfig->gyro_soft_lpf_hz; + gyroSoftNotchHz1 = gyroConfig->gyro_soft_notch_hz_1; + if (gyroConfig->gyro_soft_notch_hz_1) { + gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); } - gyroSoftNotchHz2 = gyro_soft_notch_hz_2; - if (gyro_soft_notch_hz_2) { - gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); + gyroSoftNotchHz2 = gyroConfig->gyro_soft_notch_hz_2; + if (gyroConfig->gyro_soft_notch_hz_2) { + gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); } - gyroSoftLpfType = gyro_soft_lpf_type; + gyroSoftLpfType = gyroConfig->gyro_soft_lpf_type; } void gyroInit(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2946a4602f..7f60d7deec 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -41,16 +41,16 @@ extern int32_t gyroADC[XYZ_AXIS_COUNT]; extern float gyroADCf[XYZ_AXIS_COUNT]; typedef struct gyroConfig_s { - uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. + uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. + uint8_t gyro_soft_lpf_hz; + uint16_t gyro_soft_notch_hz_1; + uint16_t gyro_soft_notch_cutoff_1; + uint16_t gyro_soft_notch_hz_2; + uint16_t gyro_soft_notch_cutoff_2; + uint8_t gyro_soft_lpf_type; } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, - uint8_t gyro_soft_lpf_hz, - uint16_t gyro_soft_notch_hz_1, - uint16_t gyro_soft_notch_cutoff_1, - uint16_t gyro_soft_notch_hz_2, - uint16_t gyro_soft_notch_cutoff_2, - uint8_t gyro_soft_lpf_type); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); From 0f082201f4d3cdcb0b1e3dc6bd62ee546215ca0c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 21:09:52 +0000 Subject: [PATCH 39/51] Removed unrequired static data from sensors/gyro.c --- src/main/sensors/gyro.c | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 37855041e8..7848d1b8db 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,10 +44,7 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static uint8_t gyroSoftLpfType; -static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; static float gyroSoftNotchQ1, gyroSoftNotchQ2; -static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -60,16 +57,12 @@ static void *notchFilter2[3]; void gyroUseConfig(const gyroConfig_t *gyroConfigToUse) { gyroConfig = gyroConfigToUse; - gyroSoftLpfHz = gyroConfig->gyro_soft_lpf_hz; - gyroSoftNotchHz1 = gyroConfig->gyro_soft_notch_hz_1; if (gyroConfig->gyro_soft_notch_hz_1) { gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); } - gyroSoftNotchHz2 = gyroConfig->gyro_soft_notch_hz_2; if (gyroConfig->gyro_soft_notch_hz_2) { gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); } - gyroSoftLpfType = gyroConfig->gyro_soft_lpf_type; } void gyroInit(void) @@ -84,14 +77,14 @@ void gyroInit(void) notchFilter1ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply; - if (gyroSoftLpfHz) { // Initialisation needs to happen once samplingrate is known - if (gyroSoftLpfType == FILTER_BIQUAD) { + if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known + if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; biquadFilterInitLPF(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); } - } else if (gyroSoftLpfType == FILTER_PT1) { + } else if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { @@ -107,14 +100,14 @@ void gyroInit(void) } } - if (gyroSoftNotchHz1) { + if (gyroConfig->gyro_soft_notch_hz_1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; biquadFilterInit(notchFilter1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } - if (gyroSoftNotchHz1) { + if (gyroConfig->gyro_soft_notch_hz_2) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; From 78e6130aabea2ce55609c36c7a586579c0f29049 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 24 Nov 2016 07:24:17 +0000 Subject: [PATCH 40/51] Moved further gyro parameters into gyroConfig_t --- src/main/blackbox/blackbox.c | 4 ++-- src/main/config/config_master.h | 3 --- src/main/fc/config.c | 14 ++++++-------- src/main/fc/fc_msp.c | 9 ++++----- src/main/io/serial_cli.c | 4 ++-- src/main/main.c | 3 +-- src/main/sensors/gyro.c | 28 ++++++++++------------------ src/main/sensors/gyro.h | 7 ++++--- src/main/sensors/initialisation.c | 11 +++++------ src/main/sensors/initialisation.h | 8 +++++++- 10 files changed, 41 insertions(+), 50 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ed4ba0137d..e8df1a8ea4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo() ); BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); - BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyro_sync_denom); + BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyroConfig.gyro_sync_denom); BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); @@ -1262,7 +1262,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); - BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); + BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyroConfig.gyro_lpf); BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index e74ed5cbcd..a0aec3753a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -92,9 +92,6 @@ typedef struct master_s { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. - - uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. - uint8_t gyro_sync_denom; // Gyro sample divider uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 794bd669f1..ad3c3a5b89 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -564,15 +564,15 @@ void createDefaultConfig(master_t *config) config->current_profile_index = 0; // default profile config->dcm_kp = 2500; // 1.0 * 10000 config->dcm_ki = 0; // 0.003 * 10000 - config->gyro_lpf = GYRO_LPF_256HZ; // 256HZ default + config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X - config->gyro_sync_denom = 8; + config->gyroConfig.gyro_sync_denom = 8; config->pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) - config->gyro_sync_denom = 1; + config->gyroConfig.gyro_sync_denom = 1; config->pid_process_denom = 4; #else - config->gyro_sync_denom = 4; + config->gyroConfig.gyro_sync_denom = 4; config->pid_process_denom = 2; #endif config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1; @@ -830,8 +830,6 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig); - #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); #endif @@ -1003,9 +1001,9 @@ void validateAndFixGyroConfig(void) masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0; } - if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyro_lpf != GYRO_LPF_NONE) { + if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) { masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - masterConfig.gyro_sync_denom = 1; + masterConfig.gyroConfig.gyro_sync_denom = 1; } } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 664dc628e4..808859860a 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1081,11 +1081,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_ADVANCED_CONFIG: - if (masterConfig.gyro_lpf) { + if (masterConfig.gyroConfig.gyro_lpf) { sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000 sbufWriteU8(dst, 1); } else { - sbufWriteU8(dst, masterConfig.gyro_sync_denom); + sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom); sbufWriteU8(dst, masterConfig.pid_process_denom); } sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm); @@ -1433,7 +1433,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_ADVANCED_CONFIG: - masterConfig.gyro_sync_denom = sbufReadU8(src); + masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src); masterConfig.pid_process_denom = sbufReadU8(src); masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src); #ifdef USE_DSHOT @@ -1460,8 +1460,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); - gyroUseConfig(&masterConfig.gyroConfig); - gyroInit(); + gyroInit(&masterConfig.gyroConfig); // reinitialize the PID filters with the new values pidInitFilters(¤tProfile->pidProfile); break; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b3e6b8f52e..b7039530af 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -807,8 +807,8 @@ const clivalue_t valueTable[] = { { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } }, { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, - { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, - { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, + { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, + { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, diff --git a/src/main/main.c b/src/main/main.c index 5fa43d3876..48849f012d 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -425,8 +425,7 @@ void init(void) if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, &masterConfig.sensorSelectionConfig, masterConfig.compassConfig.mag_declination, - masterConfig.gyro_lpf, - masterConfig.gyro_sync_denom, + &masterConfig.gyroConfig, sonarConfig)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 7848d1b8db..56c29bd83d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,7 +44,6 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static float gyroSoftNotchQ1, gyroSoftNotchQ2; static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -54,18 +53,7 @@ static void *notchFilter1[3]; static filterApplyFnPtr notchFilter2ApplyFn; static void *notchFilter2[3]; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse) -{ - gyroConfig = gyroConfigToUse; - if (gyroConfig->gyro_soft_notch_hz_1) { - gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); - } - if (gyroConfig->gyro_soft_notch_hz_2) { - gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); - } -} - -void gyroInit(void) +void gyroInit(const gyroConfig_t *gyroConfigToUse) { static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; @@ -73,6 +61,8 @@ void gyroInit(void) static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; + gyroConfig = gyroConfigToUse; + softLpfFilterApplyFn = nullFilterApply; notchFilter1ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply; @@ -82,36 +72,38 @@ void gyroInit(void) softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); } } else if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterPt1[axis]; - pt1FilterInit(softLpfFilter[axis], gyroSoftLpfHz, gyroDt); + pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt); } } else { softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroDenoiseState[axis]; - firFilterDenoiseInit(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); + firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); } } } if (gyroConfig->gyro_soft_notch_hz_1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; + const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInit(notchFilter1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } if (gyroConfig->gyro_soft_notch_hz_2) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInit(notchFilter2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); + biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7f60d7deec..bea1268e57 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -42,17 +42,18 @@ extern float gyroADCf[XYZ_AXIS_COUNT]; typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. + uint8_t gyro_sync_denom; // Gyro sample divider + uint8_t gyro_soft_lpf_type; uint8_t gyro_soft_lpf_hz; + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint16_t gyro_soft_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2; uint16_t gyro_soft_notch_cutoff_2; - uint8_t gyro_soft_lpf_type; } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse); void gyroSetCalibrationCycles(void); -void gyroInit(void); +void gyroInit(const gyroConfig_t *gyroConfigToUse); void gyroUpdate(void); bool isGyroCalibrationComplete(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 8d1e20ecbc..5a00dc3213 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -638,11 +638,10 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } } -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, +bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, sensorSelectionConfig_t *sensorSelectionConfig, int16_t magDeclinationFromConfig, - uint8_t gyroLpf, - uint8_t gyroSyncDenominator, + const gyroConfig_t *gyroConfig, const sonarConfig_t *sonarConfig) { memset(&acc, 0, sizeof(acc)); @@ -662,9 +661,9 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, // Now time to init things // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation - gyro.init(gyroLpf); // driver initialisation - gyroInit(); // sensor initialisation + gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation + gyro.init(gyroConfig->gyro_lpf); // driver initialisation + gyroInit(gyroConfig); // sensor initialisation if (detectAcc(sensorSelectionConfig->acc_hardware)) { acc.acc_1G = 256; // set default diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index 29ce3035e9..402d33cbc5 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,6 +17,12 @@ #pragma once +struct sensorAlignmentConfig_s; struct sensorSelectionConfig_s; +struct gyroConfig_s; struct sonarConfig_s; -bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, const struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator, const struct sonarConfig_s *sonarConfig); +bool sensorsAutodetect(const sensorAlignmentConfig_s *sensorAlignmentConfig, + const struct sensorSelectionConfig_s *sensorSelectionConfig, + int16_t magDeclinationFromConfig, + const struct gyroConfig_s *gyroConfig, + const struct sonarConfig_s *sonarConfig); From 7781ae1e046aa6beac4b8aa74a6e7b21caa839e2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 24 Nov 2016 07:46:46 +0000 Subject: [PATCH 41/51] Updated target config.c files --- src/main/target/NAZE/config.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index e279937540..b4142093eb 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -43,10 +43,10 @@ void targetConfiguration(master_t *config) config->motorConfig.minthrottle = 1049; - config->gyro_lpf = GYRO_LPF_188HZ; - config->gyro_soft_lpf_hz = 100; - config->gyro_soft_notch_hz_1 = 0; - config->gyro_soft_notch_hz_2 = 0; + config->gyroConfig.gyro_lpf = GYRO_LPF_188HZ; + config->gyroConfig.gyro_soft_lpf_hz = 100; + config->gyroConfig.gyro_soft_notch_hz_1 = 0; + config->gyroConfig.gyro_soft_notch_hz_2 = 0; /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { config->rxConfig.channelRanges[channel].min = 1180; From 6c6a5d88dc0e21e72158df29e5dcf8c86158c15c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 24 Nov 2016 07:58:31 +0000 Subject: [PATCH 42/51] Fixed COLIBRI_RACE build --- src/main/target/COLIBRI_RACE/i2c_bst.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 36c6c7cddd..5774973293 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -977,7 +977,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(masterConfig.rcControlsConfig.yaw_deadband); break; case BST_FC_FILTERS: - bstWrite16(constrain(masterConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values + bstWrite16(constrain(masterConfig.gyroConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values break; default: // we do not know how to handle the (valid) message, indicate error BST @@ -1404,7 +1404,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.rcControlsConfig.yaw_deadband = bstRead8(); break; case BST_SET_FC_FILTERS: - masterConfig.gyro_lpf = bstRead16(); + masterConfig.gyroConfig.gyro_lpf = bstRead16(); break; default: From 137b323577ff4657d2046937eccb2aab9b860499 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 24 Nov 2016 17:27:52 +0000 Subject: [PATCH 43/51] Fixed MOTOLAB and MULTIFLITEPICO targets --- src/main/target/MOTOLAB/config.c | 2 +- src/main/target/MULTIFLITEPICO/config.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c index 4cd38f98f2..94a217a364 100755 --- a/src/main/target/MOTOLAB/config.c +++ b/src/main/target/MOTOLAB/config.c @@ -26,6 +26,6 @@ // Motolab target supports 2 different type of boards Tornado / Cyclone. void targetConfiguration(master_t *config) { - config->gyro_sync_denom = 4; + config->gyroConfig.gyro_sync_denom = 4; config->pid_process_denom = 1; } diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 5601a2c9eb..ddc186ccc7 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -70,7 +70,7 @@ void targetConfiguration(master_t *config) { config->motorConfig.motorPwmRate = 17000; - config->gyro_sync_denom = 4; + config->gyroConfig.gyro_sync_denom = 4; config->pid_process_denom = 1; config->profile[0].pidProfile.P8[ROLL] = 70; From 2d8d03e5075297600d20753497906f59d3d30235 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 25 Nov 2016 12:30:59 +0000 Subject: [PATCH 44/51] Fixup after rebase --- src/main/sensors/initialisation.c | 4 ++-- src/main/sensors/initialisation.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 5a00dc3213..d1812a9b15 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -625,7 +625,7 @@ static bool detectSonar(void) } #endif -void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) +static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig) { if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { gyroAlign = sensorAlignmentConfig->gyro_align; @@ -639,7 +639,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, - sensorSelectionConfig_t *sensorSelectionConfig, + const sensorSelectionConfig_t *sensorSelectionConfig, int16_t magDeclinationFromConfig, const gyroConfig_t *gyroConfig, const sonarConfig_t *sonarConfig) diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index 402d33cbc5..c1b3204f91 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -21,7 +21,7 @@ struct sensorAlignmentConfig_s; struct sensorSelectionConfig_s; struct gyroConfig_s; struct sonarConfig_s; -bool sensorsAutodetect(const sensorAlignmentConfig_s *sensorAlignmentConfig, +bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig, const struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, const struct gyroConfig_s *gyroConfig, From 4c924ef69b42401faccf7d0739cba7175fda372f Mon Sep 17 00:00:00 2001 From: J Blackman Date: Sat, 26 Nov 2016 12:15:07 +1100 Subject: [PATCH 45/51] Bump EEPROM version number Updated version number following config parameter changes (moved to structs) --- src/main/config/config_eeprom.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index e8d59235dc..f1f9facba6 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,7 +17,7 @@ #pragma once -#define EEPROM_CONF_VERSION 145 +#define EEPROM_CONF_VERSION 146 void initEEPROM(void); void writeEEPROM(); From ccd759d93a500225ad13d4a7b5dc8590c1fd7a27 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 26 Nov 2016 21:30:21 +0100 Subject: [PATCH 46/51] Remove unnecessary lazy initialisation flag checks --- src/main/flight/pid.c | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cccfad9ab0..20839a2775 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -84,17 +84,12 @@ static void *ptermYawFilter; void pidInitFilters(const pidProfile_t *pidProfile) { - static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed static biquadFilter_t biquadFilterNotch[2]; static pt1Filter_t pt1Filter[2]; static biquadFilter_t biquadFilter[2]; static firFilterDenoise_t denoisingFilter[2]; static pt1Filter_t pt1FilterYaw; - if (initialized) { - return; - } - BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 const float dT = (float)targetPidLooptime * 0.000001f; @@ -147,8 +142,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) ptermYawFilter = &pt1FilterYaw; pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT); } - - initialized = true; } // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. From 28f2cf051092037fb2df37bb708cb0d63dc4d92e Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 21 Nov 2016 01:53:47 +1300 Subject: [PATCH 47/51] Made MAVLink telemetry shareable with serial Rx. --- src/main/io/serial.c | 4 ++-- src/main/telemetry/mavlink.c | 23 +++++++++++++++-------- src/main/telemetry/telemetry.h | 2 +- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 4c9bd96678..8dcc4b14a2 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -208,7 +208,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction } #ifdef TELEMETRY -#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_MAVLINK) +#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT) #else #define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) #endif @@ -221,7 +221,7 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) * rules: * - 1 MSP port minimum, max MSP ports is defined and must be adhered to. * - MSP is allowed to be shared with EITHER any telemetry OR blackbox. - * - serial RX and FrSky / LTM telemetry can be shared + * - serial RX and FrSky / LTM / MAVLink telemetry can be shared * - No other sharing combinations are valid. */ uint8_t mspPortCount = 0; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 29dea7bd98..411089db02 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -170,16 +170,23 @@ void configureMAVLinkTelemetryPort(void) void checkMAVLinkTelemetryState(void) { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing); + if (portConfig && telemetryCheckRxPortShared(portConfig)) { + if (!mavlinkTelemetryEnabled && telemetrySharedPort != NULL) { + mavlinkPort = telemetrySharedPort; + mavlinkTelemetryEnabled = true; + } + } else { + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing); - if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) { - return; + if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) { + return; + } + + if (newTelemetryEnabledValue) + configureMAVLinkTelemetryPort(); + else + freeMAVLinkTelemetryPort(); } - - if (newTelemetryEnabledValue) - configureMAVLinkTelemetryPort(); - else - freeMAVLinkTelemetryPort(); } void mavlinkSendSystemStatus(void) diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 214722f12e..9355c703db 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -59,4 +59,4 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing); void telemetryUseConfig(telemetryConfig_t *telemetryConfig); -#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM) +#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) From 1c2bbf9c1ac066d41d0886a7d25bd5400aa0cfe8 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 25 Nov 2016 21:31:39 +1300 Subject: [PATCH 48/51] Added battery warnings based on consumption. --- src/main/fc/config.c | 3 + src/main/io/serial_cli.c | 3 + src/main/sensors/battery.c | 166 ++++++++++++++++++++++------------ src/main/sensors/battery.h | 3 + src/main/target/NAZE/target.h | 2 + 5 files changed, 119 insertions(+), 58 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 93c8832107..57bf748ef5 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -430,6 +430,9 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->batteryCapacity = 0; batteryConfig->currentMeterType = CURRENT_SENSOR_ADC; batteryConfig->batterynotpresentlevel = 55; // VBAT below 5.5 V will be igonored + batteryConfig->useVBatAlerts = true; + batteryConfig->useConsumptionAlerts = false; + batteryConfig->consumptionWarningPercentage = 10; } #ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 58697228a7..8809d9a077 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -794,6 +794,9 @@ const clivalue_t valueTable[] = { { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, { "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.batterynotpresentlevel, .config.minmax = { 0, 200 } }, + { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.consumptionWarningPercentage, .config.minmax = { 0, 100 } }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 2250460a31..f1330bf415 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -58,12 +58,14 @@ uint16_t batteryCriticalVoltage; uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) uint16_t vbatLatest = 0; // most recent unsmoothed value -uint16_t amperageLatest = 0; // most recent value int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) +uint16_t amperageLatest = 0; // most recent value + int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start -static batteryState_e batteryState; +static batteryState_e vBatState; +static batteryState_e consumptionState; static uint16_t batteryAdcToVoltage(uint16_t src) { @@ -74,12 +76,12 @@ static uint16_t batteryAdcToVoltage(uint16_t src) static void updateBatteryVoltage(void) { - static biquadFilter_t vbatFilter; - static bool vbatFilterIsInitialised; + static biquadFilter_t vBatFilter; + static bool vBatFilterIsInitialised; - if (!vbatFilterIsInitialised) { - biquadFilterInitLPF(&vbatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update - vbatFilterIsInitialised = true; + if (!vBatFilterIsInitialised) { + biquadFilterInitLPF(&vBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update + vBatFilterIsInitialised = true; } #ifdef USE_ESC_TELEMETRY @@ -88,17 +90,17 @@ static void updateBatteryVoltage(void) if (debugMode == DEBUG_BATTERY) { debug[0] = -1; } - vbat = biquadFilterApply(&vbatFilter, vbatLatest); + vbat = biquadFilterApply(&vBatFilter, vbatLatest); } else #endif { - uint16_t vbatSample = adcGetChannel(ADC_BATTERY); + uint16_t vBatSample = adcGetChannel(ADC_BATTERY); if (debugMode == DEBUG_BATTERY) { - debug[0] = vbatSample; + debug[0] = vBatSample; } - vbat = batteryAdcToVoltage(biquadFilterApply(&vbatFilter, vbatSample)); - vbatLatest = batteryAdcToVoltage(vbatSample); + vbat = batteryAdcToVoltage(biquadFilterApply(&vBatFilter, vBatSample)); + vbatLatest = batteryAdcToVoltage(vBatSample); } if (debugMode == DEBUG_BATTERY) { @@ -106,18 +108,35 @@ static void updateBatteryVoltage(void) } } +static void updateBatteryAlert(void) +{ + switch(getBatteryState()) { + case BATTERY_WARNING: + beeper(BEEPER_BAT_LOW); + + break; + case BATTERY_CRITICAL: + beeper(BEEPER_BAT_CRIT_LOW); + + break; + case BATTERY_OK: + case BATTERY_NOT_PRESENT: + break; + } +} + void updateBattery(void) { - uint16_t vbatPrevious = vbatLatest; + uint16_t vBatPrevious = vbatLatest; updateBatteryVoltage(); - uint16_t vbatMeasured = vbatLatest; + uint16_t vBatMeasured = vbatLatest; /* battery has just been connected*/ - if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA))) { + if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ - batteryState = BATTERY_OK; + vBatState = BATTERY_OK; - unsigned cells = (vbatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; + unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; @@ -126,50 +145,55 @@ void updateBattery(void) batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */ - } else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA) { - batteryState = BATTERY_NOT_PRESENT; + } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { + vBatState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; } if (debugMode == DEBUG_BATTERY) { - debug[2] = batteryState; + debug[2] = vBatState; debug[3] = batteryCellCount; } - switch(batteryState) { - case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { - batteryState = BATTERY_WARNING; - beeper(BEEPER_BAT_LOW); - } - break; - case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { - batteryState = BATTERY_CRITICAL; - beeper(BEEPER_BAT_CRIT_LOW); - } else if (vbat > batteryWarningVoltage) { - batteryState = BATTERY_OK; - } else { - beeper(BEEPER_BAT_LOW); - } - break; - case BATTERY_CRITICAL: - if (vbat > batteryCriticalVoltage) { - batteryState = BATTERY_WARNING; - beeper(BEEPER_BAT_LOW); - } else { - beeper(BEEPER_BAT_CRIT_LOW); - } - break; - case BATTERY_NOT_PRESENT: - break; + if (batteryConfig->useVBatAlerts) { + switch(vBatState) { + case BATTERY_OK: + if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { + vBatState = BATTERY_WARNING; + } + + break; + case BATTERY_WARNING: + if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { + vBatState = BATTERY_CRITICAL; + } else if (vbat > batteryWarningVoltage) { + vBatState = BATTERY_OK; + } + + break; + case BATTERY_CRITICAL: + if (vbat > batteryCriticalVoltage) { + vBatState = BATTERY_WARNING; + } + + break; + case BATTERY_NOT_PRESENT: + break; + } + + updateBatteryAlert(); } } batteryState_e getBatteryState(void) { + batteryState_e batteryState = BATTERY_NOT_PRESENT; + if (vBatState != BATTERY_NOT_PRESENT) { + batteryState = MAX(vBatState, consumptionState); + } + return batteryState; } @@ -177,13 +201,14 @@ const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PR const char * getBatteryStateString(void) { - return batteryStateStrings[batteryState]; + return batteryStateStrings[getBatteryState()]; } void batteryInit(batteryConfig_t *initialBatteryConfig) { batteryConfig = initialBatteryConfig; - batteryState = BATTERY_NOT_PRESENT; + vBatState = BATTERY_NOT_PRESENT; + consumptionState = BATTERY_OK; batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; @@ -201,22 +226,25 @@ static int32_t currentSensorToCentiamps(uint16_t src) static void updateBatteryCurrent(void) { - static biquadFilter_t ibatFilter; - static bool ibatFilterIsInitialised; + static biquadFilter_t iBatFilter; + static bool iBatFilterIsInitialised; - if (!ibatFilterIsInitialised) { - biquadFilterInitLPF(&ibatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update - ibatFilterIsInitialised = true; + if (!iBatFilterIsInitialised) { + biquadFilterInitLPF(&iBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update + iBatFilterIsInitialised = true; } - uint16_t ibatSample = adcGetChannel(ADC_CURRENT); - amperage = currentSensorToCentiamps(biquadFilterApply(&ibatFilter, ibatSample)); + uint16_t iBatSample = adcGetChannel(ADC_CURRENT); + amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample)); + amperageLatest = currentSensorToCentiamps(iBatSample); } static void updateCurrentDrawn(int32_t lastUpdateAt) { - int mAhDrawnRaw = (amperage * lastUpdateAt) / 1000; - mAhDrawn = mAhDrawn + mAhDrawnRaw / (3600 * 100); + static float mAhDrawnF = 0.0f; // used to get good enough resolution + + mAhDrawnF = mAhDrawnF + (amperage * lastUpdateAt / (100.0f * 1000 * 3600)); + mAhDrawn = mAhDrawnF; } void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -257,6 +285,28 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea break; } + + if (batteryConfig->useConsumptionAlerts) { + switch(consumptionState) { + case BATTERY_OK: + if (calculateBatteryCapacityRemainingPercentage() <= batteryConfig->consumptionWarningPercentage) { + consumptionState = BATTERY_WARNING; + } + + break; + case BATTERY_WARNING: + if (calculateBatteryCapacityRemainingPercentage() == 0) { + vBatState = BATTERY_CRITICAL; + } + + break; + case BATTERY_CRITICAL: + case BATTERY_NOT_PRESENT: + break; + } + + updateBatteryAlert(); + } } float calculateVbatPidCompensation(void) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index b2dafca270..872e017000 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -60,6 +60,9 @@ typedef struct batteryConfig_s { uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp uint16_t batteryCapacity; // mAh uint8_t batterynotpresentlevel; // Below this level battery is considered as not present + bool useVBatAlerts; // Issue alerts based on VBat readings + bool useConsumptionAlerts; // Issue alerts based on total power consumption + uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning } batteryConfig_t; typedef enum { diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 82bc69ba70..184ea76ee5 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -20,6 +20,8 @@ #define TARGET_CONFIG #define USE_HARDWARE_REVISION_DETECTION +#define CLI_MINIMAL_VERBOSITY + #define BOARD_HAS_VOLTAGE_DIVIDER #define LED0 PB3 From 04ea404f8290577ff1332e1b98ade12f3338d16b Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sun, 27 Nov 2016 17:10:30 +0100 Subject: [PATCH 49/51] VCP could only handle max 256 byte frames, too small data type uint8_t. Now uint32_t. --- src/main/vcpf4/usbd_cdc_vcp.c | 2 +- src/main/vcpf4/usbd_cdc_vcp.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 78455c00d8..7c8a1abe8b 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -156,7 +156,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) * Output : None. * Return : None. *******************************************************************************/ -uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength) +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) { VCP_DataTx(ptrBuffer, sendLength); return sendLength; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index c6544ab797..7ef4535883 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -36,7 +36,7 @@ __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; -uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength); // HJI +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength); uint32_t CDC_Send_FreeBytes(void); uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI uint32_t CDC_Receive_BytesAvailable(void); From 77e425260be18f27fa8234922bbc3210af3a9644 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 28 Nov 2016 21:06:06 +1100 Subject: [PATCH 50/51] Minor clean up of port / pin output in Resource command. --- src/main/drivers/io_impl.h | 2 ++ src/main/io/serial_cli.c | 32 ++++++++++++++------------------ 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h index 470cf8163d..5796bbfc1d 100644 --- a/src/main/drivers/io_impl.h +++ b/src/main/drivers/io_impl.h @@ -50,6 +50,8 @@ uint16_t IO_Pin(IO_t io); #define IO_GPIOBYTAG(tag) IO_GPIO(IOGetByTag(tag)) #define IO_PINBYTAG(tag) IO_Pin(IOGetByTag(tag)) +#define IO_GPIOPortIdxByTag(tag) DEFIO_TAG_GPIOID(tag) +#define IO_GPIOPinIdxByTag(tag) DEFIO_TAG_PIN(tag) uint32_t IO_EXTI_Line(IO_t io); ioRec_t *IO_Rec(IO_t io); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5ee05f6cd6..4fedfec5e7 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3811,47 +3811,43 @@ static void printResource(uint8_t dumpMask, master_t *defaultConfig) if (resourceTable[i].maxIndex > 0) { for (int index = 0; index < resourceTable[i].maxIndex; index++) { - ioTag_t ioPtr = *(resourceTable[i].ptr + index); - ioTag_t ioPtrDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + ioTag_t ioTag = *(resourceTable[i].ptr + index); + ioTag_t ioTagDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); - IO_t io = IOGetByTag(ioPtr); - IO_t ioDefault = IOGetByTag(ioPtrDefault); - bool equalsDefault = io == ioDefault; + bool equalsDefault = ioTag == ioTagDefault; const char *format = "resource %s %d %c%02d\r\n"; const char *formatUnassigned = "resource %s %d NONE\r\n"; - if (DEFIO_TAG_ISEMPTY(ioDefault)) { + if (!ioTagDefault) { cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); } else { - cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault)); } - if (DEFIO_TAG_ISEMPTY(io)) { + if (!ioTag) { if (!(dumpMask & HIDE_UNUSED)) { cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); } } else { - cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); } } } else { - ioTag_t ioPtr = *resourceTable[i].ptr; - ioTag_t ioPtrDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + ioTag_t ioTag = *resourceTable[i].ptr; + ioTag_t ioTagDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig); - IO_t io = IOGetByTag(ioPtr); - IO_t ioDefault = IOGetByTag(ioPtrDefault); - bool equalsDefault = io == ioDefault; + bool equalsDefault = ioTag == ioTagDefault; const char *format = "resource %s %c%02d\r\n"; const char *formatUnassigned = "resource %s NONE\r\n"; - if (DEFIO_TAG_ISEMPTY(ioDefault)) { + if (!ioTagDefault) { cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner); } else { - cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault)); } - if (DEFIO_TAG_ISEMPTY(io)) { + if (!ioTag) { if (!(dumpMask & HIDE_UNUSED)) { cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner); } } else { - cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); } } } From a39df1cb86d21c652b76cb60cd3fbf2e33e0fd6a Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 29 Nov 2016 08:06:51 +1300 Subject: [PATCH 51/51] Fixed setting of 3D deadband throttle to 1000. --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 808859860a..e6f999722c 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1413,13 +1413,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src); masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src); masterConfig.flight3DConfig.neutral3d = sbufReadU16(src); - masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RC_DEADBAND: masterConfig.rcControlsConfig.deadband = sbufReadU8(src); masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src); masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src); + masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RESET_CURR_PID: