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

Tidy of IMU header

Browse source
This commit is contained in:
Martin Budden 2017-02-05 20:37:20 +00:00
parent 9a8124ffc4
commit 3d1e42d1aa
14 changed files with 78 additions and 97 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -21,40 +21,40 @@
#include <stdint.h>
#include <math.h>
#include "common/maths.h"
#include "platform.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "io/gps.h"
#include "fc/runtime_config.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
// the limit (in degrees/second) beyond which we stop integrating
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
// which results in false gyro drift. See
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT 20
int32_t accSum[XYZ_AXIS_COUNT];
@ -63,11 +63,11 @@ uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
float throttleAngleScale;
float fc_acc;
float smallAngleCosZ = 0;
static float throttleAngleScale;
static float fc_acc;
static float smallAngleCosZ = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
static float magneticDeclination = 0.0f; // calculated at startup from config
static imuRuntimeConfig_t imuRuntimeConfig;
static pidProfile_t *pidProfile;
@ -103,6 +103,19 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
/*
* Calculate RC time constant used in the accZ lpf.
*/
static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
return 0.5f / (M_PIf * accz_lpf_cutoff);
}
static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
void imuConfigure(
imuConfig_t *imuConfig,
pidProfile_t *initialPidProfile,
@ -127,19 +140,6 @@ void imuInit(void)
imuComputeRotationMatrix();
}
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
/*
* Calculate RC time constant used in the accZ lpf.
*/
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
return 0.5f / (M_PIf * accz_lpf_cutoff);
}
void imuResetAccelerationSum(void)
{
accSum[0] = 0;
@ -149,14 +149,12 @@ void imuResetAccelerationSum(void)
accTimeSum = 0;
}
void imuTransformVectorBodyToEarth(t_fp_vector * v)
static void imuTransformVectorBodyToEarth(t_fp_vector * v)
{
float x,y,z;
/* From body frame to earth frame */
x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
v->V.X = x;
v->V.Y = -y;
@ -164,16 +162,15 @@ void imuTransformVectorBodyToEarth(t_fp_vector * v)
}
// rotate acc into Earth frame and calculate acceleration in it
void imuCalculateAcceleration(uint32_t deltaT)
static void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
float dT;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
const float dT = (float)deltaT * 1e-6f;
t_fp_vector accel_ned;
accel_ned.V.X = acc.accSmooth[X];
accel_ned.V.Y = acc.accSmooth[Y];
accel_ned.V.Z = acc.accSmooth[Z];
@ -186,8 +183,9 @@ void imuCalculateAcceleration(uint32_t deltaT)
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
} else {
accel_ned.V.Z -= acc.dev.acc_1G;
}
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
@ -227,15 +225,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useYaw, float yawError)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float hx, hy, bx;
float ex = 0, ey = 0, ez = 0;
float qa, qb, qc;
// Calculate general spin rate (rad/s)
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
float ez = 0;
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
@ -244,7 +239,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Use measured magnetic field vector
recipNorm = sq(mx) + sq(my) + sq(mz);
float ex = 0, ey = 0;
float recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
@ -257,12 +253,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
bx = sqrtf(hx * hx + hy * hy);
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
const float bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
const float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
@ -289,7 +285,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
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;
const float dcmKiGain = imuRuntimeConfig.dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
@ -302,7 +298,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();
const float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
@ -314,9 +310,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
gy *= (0.5f * dt);
gz *= (0.5f * dt);
qa = q0;
qb = q1;
qc = q2;
const float qa = q0;
const float qb = q1;
const float qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
@ -353,10 +349,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
static bool imuIsAccelerometerHealthy(void)
{
int32_t axis;
int32_t accMagnitude = 0;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
}