Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Moved masterConfig imu items into a struct

Browse source
This commit is contained in:
Martin Budden 2016-11-29 22:28:11 +00:00
parent c0761eb619
commit 6ff265ec62
6 changed files with 41 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/config/config_master.h
View file

@ -90,11 +90,9 @@ typedef struct master_s {
sensorTrims_t sensorTrims; sensorTrims_t sensorTrims;
boardAlignment_t boardAlignment; boardAlignment_t boardAlignment;
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) imuConfig_t imuConfig;
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim 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). 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 uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate

21
src/main/fc/config.c
View file

@ -565,8 +565,8 @@ void createDefaultConfig(master_t *config)
// global settings // global settings
config->current_profile_index = 0; // default profile config->current_profile_index = 0; // default profile
config->dcm_kp = 2500; // 1.0 * 10000 config->imuConfig.dcm_kp = 2500; // 1.0 * 10000
config->dcm_ki = 0; // 0.003 * 10000 config->imuConfig.dcm_ki = 0; // 0.003 * 10000
config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default
#ifdef STM32F10X #ifdef STM32F10X
config->gyroConfig.gyro_sync_denom = 8; 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.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support
config->armingConfig.disarm_kill_switch = 1; config->armingConfig.disarm_kill_switch = 1;
config->armingConfig.auto_disarm_delay = 5; config->armingConfig.auto_disarm_delay = 5;
config->armingConfig.small_angle = 25; config->imuConfig.small_angle = 25;
config->airplaneConfig.fixedwing_althold_dir = 1; config->airplaneConfig.fixedwing_althold_dir = 1;
@ -703,9 +703,9 @@ void createDefaultConfig(master_t *config)
config->compassConfig.mag_declination = 0; config->compassConfig.mag_declination = 0;
config->accelerometerConfig.acc_lpf_hz = 10.0f; config->accelerometerConfig.acc_lpf_hz = 10.0f;
config->accDeadband.xy = 40; config->imuConfig.accDeadband.xy = 40;
config->accDeadband.z = 40; config->imuConfig.accDeadband.z = 40;
config->acc_unarmedcal = 1; config->imuConfig.acc_unarmedcal = 1;
#ifdef BARO #ifdef BARO
resetBarometerConfig(&config->barometerConfig); resetBarometerConfig(&config->barometerConfig);
@ -822,8 +822,6 @@ void activateControlRateConfig(void)
void activateConfig(void) void activateConfig(void)
{ {
static imuRuntimeConfig_t imuRuntimeConfig;
activateControlRateConfig(); activateControlRateConfig();
resetAdjustmentStates(); resetAdjustmentStates();
@ -859,15 +857,10 @@ void activateConfig(void)
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig); servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig);
#endif #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( imuConfigure(
&imuRuntimeConfig, &masterConfig.imuConfig,
&currentProfile->pidProfile, &currentProfile->pidProfile,
&masterConfig.accDeadband,
masterConfig.throttleCorrectionConfig.throttle_correction_angle masterConfig.throttleCorrectionConfig.throttle_correction_angle
); );

1
src/main/fc/rc_controls.h
View file

@ -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 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 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 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_t;
bool areUsingSticksToArm(void); bool areUsingSticksToArm(void);

29
src/main/flight/imu.c
View file

@ -67,9 +67,8 @@ float smallAngleCosZ = 0;
float magneticDeclination = 0.0f; // calculated at startup from config float magneticDeclination = 0.0f; // calculated at startup from config
static bool isAccelUpdatedAtLeastOnce = false; static bool isAccelUpdatedAtLeastOnce = false;
static imuRuntimeConfig_t *imuRuntimeConfig; static imuRuntimeConfig_t imuRuntimeConfig;
static pidProfile_t *pidProfile; 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_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]; static float rMat[3][3];
@ -105,22 +104,24 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
} }
void imuConfigure( void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig, imuConfig_t *imuConfig,
pidProfile_t *initialPidProfile, pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
uint16_t throttle_correction_angle 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; pidProfile = initialPidProfile;
accDeadband = initialAccDeadband;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
} }
void imuInit(void) 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 gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
accVelScale = 9.80665f / acc.acc_1G / 10000.0f; accVelScale = 9.80665f / acc.acc_1G / 10000.0f;
@ -180,7 +181,7 @@ void imuCalculateAcceleration(uint32_t deltaT)
imuTransformVectorBodyToEarth(&accel_ned); imuTransformVectorBodyToEarth(&accel_ned);
if (imuRuntimeConfig->acc_unarmedcal == 1) { if (imuRuntimeConfig.acc_unarmedcal == 1) {
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64; accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z; 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 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 // apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy); accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy); accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z); accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z);
// sum up Values for later integration to get velocity and distance // sum up Values for later integration to get velocity and distance
accTimeSum += deltaT; accTimeSum += deltaT;
@ -286,10 +287,10 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
} }
// Compute and apply integral feedback if enabled // 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 // Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_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 integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt; integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * 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 // 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 // Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx; gx += dcmKpGain * ex + integralFBx;

14
src/main/flight/imu.h
View file

@ -56,11 +56,20 @@ typedef struct throttleCorrectionConfig_s {
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
} throttleCorrectionConfig_t; } 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 { typedef struct imuRuntimeConfig_s {
uint8_t acc_unarmedcal;
float dcm_ki; float dcm_ki;
float dcm_kp; float dcm_kp;
uint8_t acc_unarmedcal;
uint8_t small_angle; uint8_t small_angle;
accDeadband_t accDeadband;
} imuRuntimeConfig_t; } imuRuntimeConfig_t;
typedef enum { typedef enum {
@ -81,9 +90,8 @@ typedef struct accProcessor_s {
struct pidProfile_s; struct pidProfile_s;
void imuConfigure( void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig, imuConfig_t *imuConfig,
struct pidProfile_s *initialPidProfile, struct pidProfile_s *initialPidProfile,
accDeadband_t *initialAccDeadband,
uint16_t throttle_correction_angle uint16_t throttle_correction_angle
); );

12
src/main/io/serial_cli.c
View file

@ -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 } }, { "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 } }, { "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 } }, { "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 } }, { "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_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 } }, { "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 } }, { "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_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.imuConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .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_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 } }, { "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_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 } }, { "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 } }, { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.accDeadband.xy, .config.minmax = { 0, 100 } },
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.z, .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.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, { "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_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 } }, { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } },