Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +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

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

@ -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;