diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 9e2e3ef932..4336ee4767 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -90,11 +90,9 @@ typedef struct master_s { sensorTrims_t sensorTrims; boardAlignment_t boardAlignment; - uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) - uint16_t dcm_ki; // DCM filter integral gain ( x 10000) + imuConfig_t imuConfig; + rollAndPitchTrims_t accelerometerTrims; // accelerometer trim - accDeadband_t accDeadband; - uint8_t acc_unarmedcal; // turn automatic acc compensation on/off uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d1d7c601ac..9e93c15ec1 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -565,8 +565,8 @@ void createDefaultConfig(master_t *config) // global settings config->current_profile_index = 0; // default profile - config->dcm_kp = 2500; // 1.0 * 10000 - config->dcm_ki = 0; // 0.003 * 10000 + config->imuConfig.dcm_kp = 2500; // 1.0 * 10000 + config->imuConfig.dcm_ki = 0; // 0.003 * 10000 config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X config->gyroConfig.gyro_sync_denom = 8; @@ -669,7 +669,7 @@ void createDefaultConfig(master_t *config) 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->imuConfig.small_angle = 25; config->airplaneConfig.fixedwing_althold_dir = 1; @@ -703,9 +703,9 @@ void createDefaultConfig(master_t *config) config->compassConfig.mag_declination = 0; config->accelerometerConfig.acc_lpf_hz = 10.0f; - config->accDeadband.xy = 40; - config->accDeadband.z = 40; - config->acc_unarmedcal = 1; + config->imuConfig.accDeadband.xy = 40; + config->imuConfig.accDeadband.z = 40; + config->imuConfig.acc_unarmedcal = 1; #ifdef BARO resetBarometerConfig(&config->barometerConfig); @@ -822,8 +822,6 @@ void activateControlRateConfig(void) void activateConfig(void) { - static imuRuntimeConfig_t imuRuntimeConfig; - activateControlRateConfig(); resetAdjustmentStates(); @@ -859,15 +857,10 @@ void activateConfig(void) servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig); #endif - 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.armingConfig.small_angle; imuConfigure( - &imuRuntimeConfig, + &masterConfig.imuConfig, ¤tProfile->pidProfile, - &masterConfig.accDeadband, masterConfig.throttleCorrectionConfig.throttle_correction_angle ); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 752fb963df..a69c321df8 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -169,7 +169,6 @@ 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); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b745e5fd2f..514e956aef 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -67,9 +67,8 @@ float smallAngleCosZ = 0; float magneticDeclination = 0.0f; // calculated at startup from config static bool isAccelUpdatedAtLeastOnce = false; -static imuRuntimeConfig_t *imuRuntimeConfig; +static imuRuntimeConfig_t imuRuntimeConfig; static pidProfile_t *pidProfile; -static accDeadband_t *accDeadband; STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame static float rMat[3][3]; @@ -105,22 +104,24 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) } void imuConfigure( - imuRuntimeConfig_t *initialImuRuntimeConfig, + imuConfig_t *imuConfig, pidProfile_t *initialPidProfile, - accDeadband_t *initialAccDeadband, uint16_t throttle_correction_angle ) { - imuRuntimeConfig = initialImuRuntimeConfig; + imuRuntimeConfig.dcm_kp = imuConfig->dcm_kp / 10000.0f; + imuRuntimeConfig.dcm_ki = imuConfig->dcm_ki / 10000.0f; + imuRuntimeConfig.acc_unarmedcal = imuConfig->acc_unarmedcal; + imuRuntimeConfig.small_angle = imuConfig->small_angle; + pidProfile = initialPidProfile; - accDeadband = initialAccDeadband; fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } void imuInit(void) { - smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)); + smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle)); gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second accVelScale = 9.80665f / acc.acc_1G / 10000.0f; @@ -180,7 +181,7 @@ void imuCalculateAcceleration(uint32_t deltaT) imuTransformVectorBodyToEarth(&accel_ned); - if (imuRuntimeConfig->acc_unarmedcal == 1) { + if (imuRuntimeConfig.acc_unarmedcal == 1) { if (!ARMING_FLAG(ARMED)) { accZoffset -= accZoffset / 64; accZoffset += accel_ned.V.Z; @@ -192,9 +193,9 @@ void imuCalculateAcceleration(uint32_t deltaT) accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter // apply Deadband to reduce integration drift and vibration influence - accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy); - accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy); - accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z); + accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy); + accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy); + accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z); // sum up Values for later integration to get velocity and distance accTimeSum += deltaT; @@ -286,10 +287,10 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, } // Compute and apply integral feedback if enabled - if(imuRuntimeConfig->dcm_ki > 0.0f) { + if(imuRuntimeConfig.dcm_ki > 0.0f) { // Stop integrating if spinning beyond the certain limit if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) { - float dcmKiGain = imuRuntimeConfig->dcm_ki; + float dcmKiGain = imuRuntimeConfig.dcm_ki; integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki integralFBy += dcmKiGain * ey * dt; integralFBz += dcmKiGain * ez * dt; @@ -302,7 +303,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, } // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster - float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor(); + float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor(); // Apply proportional and integral feedback gx += dcmKpGain * ex + integralFBx; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 4b388eee78..08b18a7d09 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -56,11 +56,20 @@ typedef struct throttleCorrectionConfig_s { uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. } throttleCorrectionConfig_t; +typedef struct imuConfig_s { + uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) + uint16_t dcm_ki; // DCM filter integral gain ( x 10000) + uint8_t small_angle; + uint8_t acc_unarmedcal; // turn automatic acc compensation on/off + accDeadband_t accDeadband; +} imuConfig_t; + typedef struct imuRuntimeConfig_s { - uint8_t acc_unarmedcal; float dcm_ki; float dcm_kp; + uint8_t acc_unarmedcal; uint8_t small_angle; + accDeadband_t accDeadband; } imuRuntimeConfig_t; typedef enum { @@ -81,9 +90,8 @@ typedef struct accProcessor_s { struct pidProfile_s; void imuConfigure( - imuRuntimeConfig_t *initialImuRuntimeConfig, + imuConfig_t *imuConfig, struct pidProfile_s *initialPidProfile, - accDeadband_t *initialAccDeadband, uint16_t throttle_correction_angle ); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3ff7ab56e9..ed54a283df 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -719,7 +719,7 @@ const clivalue_t valueTable[] = { { "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 } }, + { "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.small_angle, .config.minmax = { 0, 180 } }, { "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, .config.minmax = { -1, 1 } }, @@ -820,8 +820,8 @@ const clivalue_t valueTable[] = { { "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 } }, + { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.imuConfig.dcm_kp, .config.minmax = { 0, 50000 } }, + { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.imuConfig.dcm_ki, .config.minmax = { 0, 50000 } }, { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.alt_hold_deadband, .config.minmax = { 1, 250 } }, { "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rcControlsConfig.alt_hold_fast_change, .config.lookup = { TABLE_OFF_ON } }, @@ -870,9 +870,9 @@ const clivalue_t valueTable[] = { { "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.accelerometerConfig.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 } }, - { "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, + { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.accDeadband.xy, .config.minmax = { 0, 100 } }, + { "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.accDeadband.z, .config.minmax = { 0, 100 } }, + { "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.imuConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } },