diff --git a/src/main/config/config.c b/src/main/config/config.c index e08b4d464..665d3fd3c 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -400,6 +400,7 @@ static void resetConf(void) masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.gyro_lpf = 1; // 188HZ + masterConfig.gyro_sync_denom = 1; masterConfig.gyro_soft_lpf_hz = 60; resetAccelerometerTrims(&masterConfig.accZero); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 7e679da08..f74c41845 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -57,7 +57,8 @@ typedef struct master_t { 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. - uint8_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_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 float gyro_soft_lpf_hz; // Biqyad gyro lpf hz 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/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 84b2ed165..7d8873997 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -41,27 +41,14 @@ bool gyroSyncCheckUpdate(void) { return getMpuDataStatus(&gyro); } -void gyroUpdateSampleRate(uint8_t lpf) { - int gyroSamplePeriod, gyroSyncDenominator; +void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) { + int gyroSamplePeriod; if (!lpf) { gyroSamplePeriod = 125; -#ifdef STM32F303xC -#ifdef COLIBRI_RACE // Leave out LUX target for now. Need to test 2.6Khz - gyroSyncDenominator = 3; // Sample every 3d gyro measurement 2,6khz -#else - gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz -#endif -#else - if (!sensors(SENSOR_ACC) && !sensors(SENSOR_BARO) && !sensors(SENSOR_MAG)) { - gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz - } else { - gyroSyncDenominator = 8; // Sample every 8th gyro measurement 1khz - } -#endif } else { gyroSamplePeriod = 1000; - gyroSyncDenominator = 1; // Full Sampling 1khz + gyroSyncDenominator = 1; // Always full Sampling 1khz } // calculate gyro divider and targetLooptime (expected cycleTime) diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 57d8b4485..698640cf4 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -11,4 +11,4 @@ extern uint32_t targetLooptime; bool gyroSyncCheckUpdate(void); uint8_t gyroMPU6xxxGetDividerDrops(void); -void gyroUpdateSampleRate(uint8_t lpf); +void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a5f2235ad..6bd9f4c24 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -597,6 +597,7 @@ 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_soft_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, { "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 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5cd0fadf6..4422c266b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -101,6 +101,39 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c extern uint16_t rssi; // FIXME dependency on mw.c extern void resetPidProfile(pidProfile_t *pidProfile); + +void setGyroSamplingSpeed(uint16_t looptime) { + uint16_t gyroSampleRate = 1000; + uint8_t maxDivider = 1; + + if (looptime != targetLooptime) { +#ifdef STM32F303xC + if (looptime < 1000) { + masterConfig.gyro_lpf = 1; + gyroSampleRate = 125; + maxDivider = 8; + } else { + masterConfig.gyro_lpf = 1; + } +#else + if (looptime < 1000) { + masterConfig.gyro_lpf = 0; + masterConfig.acc_hardware = 1; + masterConfig.baro_hardware = 1; + masterConfig.mag_hardware = 1; + gyroSampleRate = 125; + maxDivider = 8; + } else { + masterConfig.gyro_lpf = 1; + masterConfig.acc_hardware = 0; + masterConfig.baro_hardware = 0; + masterConfig.mag_hardware = 0; + } +#endif + } + masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); +} + void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. @@ -1203,7 +1236,6 @@ static bool processInCommand(void) uint32_t i; uint16_t tmp; uint8_t rate; - uint16_t gyroRefreshRate; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; @@ -1248,34 +1280,7 @@ static bool processInCommand(void) masterConfig.disarm_kill_switch = read8(); break; case MSP_SET_LOOP_TIME: - gyroRefreshRate = read16(); - if (gyroRefreshRate != targetLooptime) { - switch (gyroRefreshRate) { -#ifdef STM32F303xC - default: - case(1000): - masterConfig.gyro_lpf = 1; - break; - case(500): - masterConfig.gyro_lpf = 0; - break; -#else - default: - case(1000): - masterConfig.gyro_lpf = 1; - masterConfig.acc_hardware = 0; - masterConfig.baro_hardware = 0; - masterConfig.mag_hardware = 0; - break; - case(500): - masterConfig.gyro_lpf = 0; - masterConfig.acc_hardware = 1; - masterConfig.baro_hardware = 1; - masterConfig.mag_hardware = 1; - break; -#endif - } - } + setGyroSamplingSpeed(read16()); break; case MSP_SET_PID_CONTROLLER: currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); diff --git a/src/main/main.c b/src/main/main.c index b9a482e3e..b4e02e837 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -455,7 +455,7 @@ void init(void) } #endif - if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf)) { + if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index cd34d71c8..cad9b5fe6 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -689,7 +689,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } } -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf) +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator) { int16_t deg, min; @@ -715,7 +715,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a if (sensors(SENSOR_ACC)) acc.init(); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyroUpdateSampleRate(gyroLpf); // Set gyro refresh rate before initialisation + gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation gyro.init(gyroLpf); detectMag(magHardwareToUse); diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index 93de770a8..325979d54 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,4 +17,4 @@ #pragma once -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf); +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator);