HEADFREE true 3D (second edition)...

	reduced memory footprint ...
	rebased squashed cleanup

lib/main/MAVLink/mavlink_conversions.h

@@ -117,12 +117,12 @@ MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float
  */
 MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
 {
-    float cosPhi_2 = cosf(roll / 2);
-    float sinPhi_2 = sinf(roll / 2);
-    float cosTheta_2 = cosf(pitch / 2);
-    float sinTheta_2 = sinf(pitch / 2);
-    float cosPsi_2 = cosf(yaw / 2);
-    float sinPsi_2 = sinf(yaw / 2);
+    float cosPhi_2 = cos_approx(roll / 2);
+    float sinPhi_2 = sin_approx(roll / 2);
+    float cosTheta_2 = cos_approx(pitch / 2);
+    float sinTheta_2 = sin_approx(pitch / 2);
+    float cosPsi_2 = cos_approx(yaw / 2);
+    float sinPsi_2 = sin_approx(yaw / 2);
     quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
             sinPhi_2 * sinTheta_2 * sinPsi_2);
     quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -188,12 +188,12 @@ MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quate
  */
 MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
 {
-    float cosPhi = cosf(roll);
-    float sinPhi = sinf(roll);
-    float cosThe = cosf(pitch);
-    float sinThe = sinf(pitch);
-    float cosPsi = cosf(yaw);
-    float sinPsi = sinf(yaw);
+    float cosPhi = cos_approx(roll);
+    float sinPhi = sin_approx(roll);
+    float cosThe = cos_approx(pitch);
+    float sinThe = sin_approx(pitch);
+    float cosPsi = cos_approx(yaw);
+    float sinPsi = sin_approx(yaw);
 
     dcm[0][0] = cosThe * cosPsi;
     dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;

src/main/common/filter.c

@@ -108,8 +108,8 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
 {
     // setup variables
     const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
-    const float sn = sinf(omega);
-    const float cs = cosf(omega);
+    const float sn = sin_approx(omega);
+    const float cs = cos_approx(omega);
     const float alpha = sn / (2.0f * Q);
 
     float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0;

src/main/fc/fc_core.c

@@ -106,8 +106,6 @@ enum {
 int16_t magHold;
 #endif
 
-int16_t headFreeModeHold;
-
 static bool reverseMotors = false;
 static bool flipOverAfterCrashMode = false;
 
@@ -281,10 +279,11 @@ void tryArm(void)
 
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
+
         if (isModeActivationConditionPresent(BOXPREARM)) {
             ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
         }
-        headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
+        imuQuaternionHeadfreeOffsetSet();
 
         disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 
@@ -524,7 +523,9 @@ void processRx(timeUs_t currentTimeUs)
             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
         }
         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
-            headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
+            if (imuQuaternionHeadfreeOffsetSet()){
+               beeper(BEEPER_RX_SET);
+            }
         }
     }
 #endif

src/main/fc/fc_core.h

@@ -25,7 +25,6 @@ extern int16_t magHold;
 #endif
 
 extern bool isRXDataNew;
-extern int16_t headFreeModeHold;
 
 typedef struct throttleCorrectionConfig_s {
     uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg

src/main/fc/fc_rc.c

@@ -328,12 +328,21 @@ void updateRcCommands(void)
     }
 
     if (FLIGHT_MODE(HEADFREE_MODE)) {
-        const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
-        const float cosDiff = cos_approx(radDiff);
-        const float sinDiff = sin_approx(radDiff);
-        const float rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
-        rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
-        rcCommand[PITCH] = rcCommand_PITCH;
+        static t_fp_vector_def  rcCommandBuff;
+
+        rcCommandBuff.X = rcCommand[ROLL];
+        rcCommandBuff.Y = rcCommand[PITCH];
+        if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)))) {
+            rcCommandBuff.Z = rcCommand[YAW];
+        } else {
+            rcCommandBuff.Z = 0;
+        }
+        imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
+        rcCommand[ROLL] = rcCommandBuff.X;
+        rcCommand[PITCH] = rcCommandBuff.Y;
+        if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)))) {
+            rcCommand[YAW] = rcCommandBuff.Z;
+        }
     }
 }
 

src/main/flight/imu.c

@@ -27,7 +27,6 @@
 #include "build/debug.h"
 
 #include "common/axis.h"
-#include "common/maths.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -87,14 +86,19 @@ static float throttleAngleScale;
 static float fc_acc;
 static float smallAngleCosZ = 0;
 
-static float magneticDeclination = 0.0f;       // calculated at startup from config
-
 static imuRuntimeConfig_t imuRuntimeConfig;
 
-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 rMat[3][3];
 
-attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
+// 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);
 
@@ -106,34 +110,24 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .acc_unarmedcal = 1
 );
 
-STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
-{
-    float q1q1 = sq(q1);
-    float q2q2 = sq(q2);
-    float q3q3 = sq(q3);
-
-    float q0q1 = q0 * q1;
-    float q0q2 = q0 * q2;
-    float q0q3 = q0 * q3;
-    float q1q2 = q1 * q2;
-    float q1q3 = q1 * q3;
-    float q2q3 = q2 * q3;
+STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
+    imuQuaternionComputeProducts(&q, &qP);
 
-    rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
-    rMat[0][1] = 2.0f * (q1q2 + -q0q3);
-    rMat[0][2] = 2.0f * (q1q3 - -q0q2);
+    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 * (q1q2 - -q0q3);
-    rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
-    rMat[1][2] = 2.0f * (q2q3 + -q0q1);
+    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 * (q1q3 + -q0q2);
-    rMat[2][1] = 2.0f * (q2q3 - -q0q1);
-    rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
+    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 * (q1q2 - -q0q3);
-    rMat[2][0] = -2.0f * (q1q3 + -q0q2);
+    rMat[1][0] = -2.0f * (qP.xy - -qP.wz);
+    rMat[2][0] = -2.0f * (qP.xz + -qP.wy);
 #endif
 }
 
@@ -345,31 +339,42 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     gy *= (0.5f * dt);
     gz *= (0.5f * dt);
 
-    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);
-    q3 += (qa * gz + qb * gy - qc * gx);
+    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
-    recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
-    q0 *= recipNorm;
-    q1 *= recipNorm;
-    q2 *= recipNorm;
-    q3 *= recipNorm;
+    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)
-{
-    /* Compute pitch/roll angles */
-    attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
-    attitude.values.pitch = lrintf(((0.5f * M_PIf) - acosf(-rMat[2][0])) * (1800.0f / M_PIf));
-    attitude.values.yaw = lrintf((-atan2f(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination));
+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;
@@ -485,7 +490,7 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
     if (rMat[2][2] <= 0.015f) {
         return 0;
     }
-    int angle = lrintf(acosf(rMat[2][2]) * throttleAngleScale);
+    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)));
@@ -506,10 +511,10 @@ void imuSetAttitudeQuat(float w, float x, float y, float z)
 {
     IMU_LOCK;
 
-    q0 = w;
-    q1 = x;
-    q2 = y;
-    q3 = z;
+    q.w = w;
+    q.x = x;
+    q.y = y;
+    q.z = z;
 
     imuComputeRotationMatrix();
     imuUpdateEulerAngles();
@@ -528,3 +533,62 @@ void imuSetHasNewData(uint32_t 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;
+}

src/main/flight/imu.h

@@ -19,7 +19,7 @@
 
 #include "common/axis.h"
 #include "common/time.h"
-
+#include "common/maths.h"
 #include "config/parameter_group.h"
 
 // Exported symbols
@@ -28,6 +28,15 @@ extern int accSumCount;
 extern float accVelScale;
 extern int32_t accSum[XYZ_AXIS_COUNT];
 
+typedef struct {
+    float w,x,y,z;
+} quaternion;
+#define QUATERNION_INITIALIZE  {.w=1, .x=0, .y=0,.z=0}
+
+typedef struct {
+    float ww,wx,wy,wz,xx,xy,xz,yy,yz,zz;
+} quaternionProducts;
+#define QUATERNION_PRODUCTS_INITIALIZE  {.ww=1, .wx=0, .wy=0, .wz=0, .xx=0, .xy=0, .xz=0, .yy=0, .yz=0, .zz=0}
 
 typedef union {
     int16_t raw[XYZ_AXIS_COUNT];
@@ -38,6 +47,7 @@ typedef union {
         int16_t yaw;
     } values;
 } attitudeEulerAngles_t;
+#define EULER_INITIALIZE  { { 0, 0, 0 } }
 
 extern attitudeEulerAngles_t attitude;
 
@@ -80,3 +90,7 @@ void imuSetAttitudeQuat(float w, float x, float y, float z);
 void imuSetHasNewData(uint32_t dt);
 #endif
 #endif
+
+void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd);
+bool imuQuaternionHeadfreeOffsetSet(void);
+void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v);

src/main/sensors/gyroanalyse.c

@@ -89,7 +89,7 @@ static float hanningWindow[FFT_WINDOW_SIZE];
 void initHanning()
 {
     for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
-        hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
+        hanningWindow[i] = (0.5 - 0.5 * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
     }
 }