From e9407f3065e499ba2b301ee07f04f6ed9c8667b1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 14:09:08 +0000 Subject: [PATCH 01/10] 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 02/10] 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 03/10] 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 04/10] 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 05/10] 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 06/10] 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 07/10] 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 08/10] 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 09/10] 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 10/10] 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,