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

Avoid compass calculation in IMU when USE_MAG not defined

Browse source
This commit is contained in:
Martin Budden 2018-02-01 10:48:05 +00:00
parent 67d33b0a52
commit d7ccd2488c
1 changed files with 39 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -180,7 +180,7 @@ void imuResetAccelerationSum(void)
#if defined(USE_ALT_HOLD)
static void imuTransformVectorBodyToEarth(t_fp_vector * v)
{
/* From body frame to earth frame */
// From body frame to earth frame
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;
@ -191,7 +191,7 @@ static void imuTransformVectorBodyToEarth(t_fp_vector * v)
}
// rotate acc into Earth frame and calculate acceleration in it
static void imuCalculateAcceleration(uint32_t deltaT)
static void imuCalculateAcceleration(timeDelta_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
@ -260,23 +260,22 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
float ez = 0;
float ex = 0, ey = 0, ez = 0;
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
ez += sin_approx(yawError / 2.0f);
}
#ifdef USE_MAG
// Use measured magnetic field vector
float ex = 0, ey = 0;
float recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
float recipMagNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipMagNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
recipMagNorm = invSqrt(recipMagNorm);
mx *= recipMagNorm;
my *= recipMagNorm;
mz *= recipMagNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles
@ -295,15 +294,21 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
ey += rMat[2][1] * ez_ef;
ez += rMat[2][2] * ez_ef;
}
#else
UNUSED(useMag);
UNUSED(mx);
UNUSED(my);
UNUSED(mz);
#endif
// Use measured acceleration vector
recipNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipNorm > 0.01f) {
float recipAccNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipAccNorm > 0.01f) {
// Normalise accelerometer measurement
recipNorm = invSqrt(recipNorm);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
recipAccNorm = invSqrt(recipAccNorm);
ax *= recipAccNorm;
ay *= recipAccNorm;
az *= recipAccNorm;
// Error is sum of cross product between estimated direction and measured direction of gravity
ex += (ay * rMat[2][2] - az * rMat[2][1]);
@ -320,8 +325,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
}
}
else {
} else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
@ -352,7 +356,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx);
// Normalise quaternion
recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z));
float recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z));
q.w *= recipNorm;
q.x *= recipNorm;
q.y *= recipNorm;
@ -362,7 +366,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
imuComputeRotationMatrix();
}
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void){
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
quaternionProducts buffer;
if (FLIGHT_MODE(HEADFREE_MODE)) {
@ -380,7 +385,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void){
if (attitude.values.yaw < 0)
attitude.values.yaw += 3600;
/* Update small angle state */
// Update small angle state
if (rMat[2][2] > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE);
} else {
@ -404,13 +409,13 @@ static bool imuIsAccelerometerHealthy(void)
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{
static uint32_t previousIMUUpdateTime;
static timeUs_t previousIMUUpdateTime;
float rawYawError = 0;
bool useAcc = false;
bool useMag = false;
bool useYaw = false;
uint32_t deltaT = currentTimeUs - previousIMUUpdateTime;
const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
previousIMUUpdateTime = currentTimeUs;
if (imuIsAccelerometerHealthy()) {
@ -540,7 +545,8 @@ void imuSetHasNewData(uint32_t dt)
}
#endif
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd) {
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
{
quatProd->ww = quat->w * quat->w;
quatProd->wx = quat->w * quat->x;
quatProd->wy = quat->w * quat->y;
@ -553,7 +559,8 @@ void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd
quatProd->zz = quat->z * quat->z;
}
bool imuQuaternionHeadfreeOffsetSet(void) {
bool imuQuaternionHeadfreeOffsetSet(void)
{
if ((ABS(attitude.values.roll) < 450) && (ABS(attitude.values.pitch) < 450)) {
const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz)));
@ -562,13 +569,14 @@ bool imuQuaternionHeadfreeOffsetSet(void) {
offset.y = 0;
offset.z = sin_approx(yaw/2);
return(true);
return true;
} else {
return(false);
return false;
}
}
void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result) {
void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result)
{
const float A = (q1->w + q1->x) * (q2->w + q2->x);
const float B = (q1->z - q1->y) * (q2->y - q2->z);
const float C = (q1->w - q1->x) * (q2->y + q2->z);
@ -584,7 +592,8 @@ void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *res
result->z = D + (+ E - F - G + H) / 2.0f;
}
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v) {
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v)
{
quaternionProducts buffer;
imuQuaternionMultiplication(&offset, &q, &headfree);