rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
atbetaflight/src/main/flight/imu.c

711 lines
23 KiB
C
Raw Blame History

/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
// Inertial Measurement Unit (IMU)
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/time.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "io/gps.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
#include <stdio.h>
#include <pthread.h>
static pthread_mutex_t imuUpdateLock;
#if defined(SIMULATOR_IMU_SYNC)
static uint32_t imuDeltaT = 0;
static bool imuUpdated = false;
#endif
#define IMU_LOCK pthread_mutex_lock(&imuUpdateLock)
#define IMU_UNLOCK pthread_mutex_unlock(&imuUpdateLock)
#else
#define IMU_LOCK
#define IMU_UNLOCK
#endif
// 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
#define ATTITUDE_RESET_QUIET_TIME 250000 // 250ms - gyro quiet period after disarm before attitude reset
#define ATTITUDE_RESET_GYRO_LIMIT 15 // 15 deg/sec - gyro limit for quiet period
#define ATTITUDE_RESET_KP_GAIN 25.0 // dcmKpGain value to use during attitude reset
#define ATTITUDE_RESET_ACTIVE_TIME 500000 // 500ms - Time to wait for attitude to converge at high gain
#define GPS_COG_MIN_GROUNDSPEED 500 // 500cm/s minimum groundspeed for a gps heading to be considered valid
int32_t accSum[XYZ_AXIS_COUNT];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
bool canUseGPSHeading = true;
static float throttleAngleScale;
static float fc_acc;
static float smallAngleCosZ = 0;
static imuRuntimeConfig_t imuRuntimeConfig;
STATIC_UNIT_TESTED float rMat[3][3];
// quaternion of sensor frame relative to earth frame
STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;
STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE;
// headfree quaternions
quaternion headfree = QUATERNION_INITIALIZE;
quaternion offset = QUATERNION_INITIALIZE;
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
attitudeEulerAngles_t attitude = EULER_INITIALIZE;
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.dcm_kp = 2500, // 1.0 * 10000
.dcm_ki = 0, // 0.003 * 10000
.small_angle = 25,
.accDeadband = {.xy = 40, .z= 40},
.acc_unarmedcal = 1
);
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
imuQuaternionComputeProducts(&q, &qP);
rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
rMat[0][1] = 2.0f * (qP.xy + -qP.wz);
rMat[0][2] = 2.0f * (qP.xz - -qP.wy);
rMat[1][0] = 2.0f * (qP.xy - -qP.wz);
rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
rMat[1][2] = 2.0f * (qP.yz + -qP.wx);
rMat[2][0] = 2.0f * (qP.xz + -qP.wy);
rMat[2][1] = 2.0f * (qP.yz - -qP.wx);
rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy;
#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) && !defined(SET_IMU_FROM_EULER)
rMat[1][0] = -2.0f * (qP.xy - -qP.wz);
rMat[2][0] = -2.0f * (qP.xz + -qP.wy);
#endif
}
/*
* 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(uint16_t throttle_correction_angle)
{
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;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
}
void imuInit(void)
{
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
#ifdef USE_GPS
canUseGPSHeading = true;
#else
canUseGPSHeading = false;
#endif
imuComputeRotationMatrix();
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
if (pthread_mutex_init(&imuUpdateLock, NULL) != 0) {
printf("Create imuUpdateLock error!\n");
}
#endif
}
void imuResetAccelerationSum(void)
{
accSum[0] = 0;
accSum[1] = 0;
accSum[2] = 0;
accSumCount = 0;
accTimeSum = 0;
}
static float invSqrt(float x)
{
return 1.0f / sqrtf(x);
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useCOG, float courseOverGround, const float dcmKpGain)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
// Calculate general spin rate (rad/s)
const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
float ex = 0, ey = 0, ez = 0;
if (useCOG) {
while (courseOverGround > M_PIf) {
courseOverGround -= (2.0f * M_PIf);
}
while (courseOverGround < -M_PIf) {
courseOverGround += (2.0f * M_PIf);
}
const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
}
#ifdef USE_MAG
// Use measured magnetic field vector
float recipMagNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipMagNorm > 0.01f) {
// Normalise magnetometer measurement
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
// (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)
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)
const float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
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
float recipAccNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipAccNorm > 0.01f) {
// Normalise accelerometer measurement
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]);
ey += (az * rMat[2][0] - ax * rMat[2][2]);
ez += (ax * rMat[2][1] - ay * rMat[2][0]);
}
// Compute and apply integral feedback if enabled
if (imuRuntimeConfig.dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
const float dcmKiGain = imuRuntimeConfig.dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
}
} else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
gy += dcmKpGain * ey + integralFBy;
gz += dcmKpGain * ez + integralFBz;
// Integrate rate of change of quaternion
gx *= (0.5f * dt);
gy *= (0.5f * dt);
gz *= (0.5f * dt);
quaternion buffer;
buffer.w = q.w;
buffer.x = q.x;
buffer.y = q.y;
buffer.z = q.z;
q.w += (-buffer.x * gx - buffer.y * gy - buffer.z * gz);
q.x += (+buffer.w * gx + buffer.y * gz - buffer.z * gy);
q.y += (+buffer.w * gy - buffer.x * gz + buffer.z * gx);
q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx);
// Normalise quaternion
float recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z));
q.w *= recipNorm;
q.x *= recipNorm;
q.y *= recipNorm;
q.z *= recipNorm;
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
}
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
quaternionProducts buffer;
if (FLIGHT_MODE(HEADFREE_MODE)) {
imuQuaternionComputeProducts(&headfree, &buffer);
attitude.values.roll = lrintf(atan2_approx((+2.0f * (buffer.wx + buffer.yz)), (+1.0f - 2.0f * (buffer.xx + buffer.yy))) * (1800.0f / M_PIf));
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (1800.0f / M_PIf));
attitude.values.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf)));
} else {
attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
}
if (attitude.values.yaw < 0)
attitude.values.yaw += 3600;
// Update small angle state
if (rMat[2][2] > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE);
} else {
DISABLE_STATE(SMALL_ANGLE);
}
}
static bool imuIsAccelerometerHealthy(float *accAverage)
{
float accMagnitude = 0;
for (int axis = 0; axis < 3; axis++) {
const float a = accAverage[axis];
accMagnitude += a * a;
}
accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G));
// Accept accel readings only in range 0.90g - 1.10g
return (81 < accMagnitude) && (accMagnitude < 121);
}
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
// When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
// After disarming we want to quickly reestablish convergence to deal with the attitude estimation being incorrect due to a crash.
// - wait for a 250ms period of low gyro activity to ensure the craft is not moving
// - use a large dcmKpGain value for 500ms to allow the attitude estimate to quickly converge
// - reset the gain back to the standard setting
float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
{
static bool lastArmState = false;
static timeUs_t gyroQuietPeriodTimeEnd = 0;
static timeUs_t attitudeResetTimeEnd = 0;
static bool attitudeResetCompleted = false;
float ret;
bool attitudeResetActive = false;
const bool armState = ARMING_FLAG(ARMED);
if (!armState) {
if (lastArmState) { // Just disarmed; start the gyro quiet period
gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
attitudeResetTimeEnd = 0;
attitudeResetCompleted = false;
}
// If gyro activity exceeds the threshold then restart the quiet period.
// Also, if the attitude reset has been complete and there is subsequent gyro activity then
// start the reset cycle again. This addresses the case where the pilot rights the craft after a crash.
if ((attitudeResetTimeEnd > 0) || (gyroQuietPeriodTimeEnd > 0) || attitudeResetCompleted) {
if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (!useAcc)) {
gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
attitudeResetTimeEnd = 0;
}
}
if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation
if (currentTimeUs >= attitudeResetTimeEnd) {
gyroQuietPeriodTimeEnd = 0;
attitudeResetTimeEnd = 0;
attitudeResetCompleted = true;
} else {
attitudeResetActive = true;
}
} else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) {
// Start the high gain period to bring the estimation into convergence
attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME;
gyroQuietPeriodTimeEnd = 0;
}
}
lastArmState = armState;
if (attitudeResetActive) {
ret = ATTITUDE_RESET_KP_GAIN;
} else {
ret = imuRuntimeConfig.dcm_kp;
if (!armState) {
ret = ret * 10.0f; // Scale the kP to generally converge faster when disarmed.
}
}
return ret;
}
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{
static timeUs_t previousIMUUpdateTime;
bool useAcc = false;
bool useMag = false;
bool useCOG = false; // Whether or not correct yaw via imuMahonyAHRSupdate from our ground course
float courseOverGround = 0; // To be used when useCOG is true. Stored in Radians
const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
previousIMUUpdateTime = currentTimeUs;
#ifdef USE_MAG
if (sensors(SENSOR_MAG) && compassIsHealthy()) {
useMag = true;
}
#endif
#if defined(USE_GPS)
if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
// Use GPS course over ground to correct attitude.values.yaw
if (STATE(FIXED_WING)) {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
} else {
// If GPS rescue mode is active and we can use it, go for it. When we're close to home we will
// probably stop re calculating GPS heading data. Other future modes can also use this extern
if (canUseGPSHeading) {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
}
}
if (useCOG && shouldInitializeGPSHeading()) {
// Reset our reference and reinitialize quaternion. This will likely ideally happen more than once per flight, but for now,
// shouldInitializeGPSHeading() returns true only once.
imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
useCOG = false; // Don't use the COG when we first reinitialize. Next time around though, yes.
}
}
#endif
#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC)
UNUSED(imuMahonyAHRSupdate);
UNUSED(imuIsAccelerometerHealthy);
UNUSED(useAcc);
UNUSED(useMag);
UNUSED(useCOG);
UNUSED(canUseGPSHeading);
UNUSED(courseOverGround);
UNUSED(deltaT);
#else
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
// printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real());
deltaT = imuDeltaT;
#endif
float gyroAverage[XYZ_AXIS_COUNT];
gyroGetAccumulationAverage(gyroAverage);
float accAverage[XYZ_AXIS_COUNT];
if (accGetAccumulationAverage(accAverage)) {
useAcc = imuIsAccelerometerHealthy(accAverage);
}
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, accAverage[X], accAverage[Y], accAverage[Z],
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
useCOG, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
imuUpdateEulerAngles();
#endif
}
void imuUpdateAttitude(timeUs_t currentTimeUs)
{
if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
IMU_LOCK;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
if (imuUpdated == false) {
IMU_UNLOCK;
return;
}
imuUpdated = false;
#endif
imuCalculateEstimatedAttitude(currentTimeUs);
IMU_UNLOCK;
} else {
acc.accADC[X] = 0;
acc.accADC[Y] = 0;
acc.accADC[Z] = 0;
}
}
bool shouldInitializeGPSHeading()
{
static bool initialized = false;
if (!initialized) {
initialized = true;
return true;
}
return false;
}
float getCosTiltAngle(void)
{
return rMat[2][2];
}
void getQuaternion(quaternion *quat)
{
quat->w = q.w;
quat->x = q.x;
quat->y = q.y;
quat->z = q.z;
}
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (rMat[2][2] <= 0.015f) {
return 0;
}
int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
}
void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
if (initialRoll > 1800) {
initialRoll -= 3600;
}
if (initialPitch > 1800) {
initialPitch -= 3600;
}
if (initialYaw > 1800) {
initialYaw -= 3600;
}
const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
quatProd->xx = sq(q1);
quatProd->yy = sq(q2);
quatProd->zz = sq(q3);
quatProd->xy = q1 * q2;
quatProd->xz = q1 * q3;
quatProd->yz = q2 * q3;
quatProd->wx = q0 * q1;
quatProd->wy = q0 * q2;
quatProd->wz = q0 * q3;
imuComputeRotationMatrix();
}
#ifdef SIMULATOR_BUILD
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
{
IMU_LOCK;
attitude.values.roll = roll * 10;
attitude.values.pitch = pitch * 10;
attitude.values.yaw = yaw * 10;
IMU_UNLOCK;
}
void imuSetAttitudeQuat(float w, float x, float y, float z)
{
IMU_LOCK;
q.w = w;
q.x = x;
q.y = y;
q.z = z;
imuComputeRotationMatrix();
imuUpdateEulerAngles();
IMU_UNLOCK;
}
#endif
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
void imuSetHasNewData(uint32_t dt)
{
IMU_LOCK;
imuUpdated = true;
imuDeltaT = dt;
IMU_UNLOCK;
}
#endif
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
{
quatProd->ww = quat->w * quat->w;
quatProd->wx = quat->w * quat->x;
quatProd->wy = quat->w * quat->y;
quatProd->wz = quat->w * quat->z;
quatProd->xx = quat->x * quat->x;
quatProd->xy = quat->x * quat->y;
quatProd->xz = quat->x * quat->z;
quatProd->yy = quat->y * quat->y;
quatProd->yz = quat->y * quat->z;
quatProd->zz = quat->z * quat->z;
}
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)));
offset.w = cos_approx(yaw/2);
offset.x = 0;
offset.y = 0;
offset.z = sin_approx(yaw/2);
return true;
} else {
return false;
}
}
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);
const float D = (q1->y + q1->z) * (q2->w - q2->x);
const float E = (q1->x + q1->z) * (q2->x + q2->y);
const float F = (q1->x - q1->z) * (q2->x - q2->y);
const float G = (q1->w + q1->y) * (q2->w - q2->z);
const float H = (q1->w - q1->y) * (q2->w + q2->z);
result->w = B + (- E - F + G + H) / 2.0f;
result->x = A - (+ E + F + G + H) / 2.0f;
result->y = C + (+ E - F + G - H) / 2.0f;
result->z = D + (+ E - F - G + H) / 2.0f;
}
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v)
{
quaternionProducts buffer;
imuQuaternionMultiplication(&offset, &q, &headfree);
imuQuaternionComputeProducts(&headfree, &buffer);
const float x = (buffer.ww + buffer.xx - buffer.yy - buffer.zz) * v->X + 2.0f * (buffer.xy + buffer.wz) * v->Y + 2.0f * (buffer.xz - buffer.wy) * v->Z;
const float y = 2.0f * (buffer.xy - buffer.wz) * v->X + (buffer.ww - buffer.xx + buffer.yy - buffer.zz) * v->Y + 2.0f * (buffer.yz + buffer.wx) * v->Z;
const float z = 2.0f * (buffer.xz + buffer.wy) * v->X + 2.0f * (buffer.yz - buffer.wx) * v->Y + (buffer.ww - buffer.xx - buffer.yy + buffer.zz) * v->Z;
v->X = x;
v->Y = y;
v->Z = z;
}
Reference in New Issue View Git Blame Copy Permalink