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;