Improvement vector library

mk/source.mk

@@ -71,6 +71,7 @@ COMMON_SRC = \
             common/time.c \
             common/typeconversion.c \
             common/uvarint.c \
+            common/vector.c \
             config/config.c \
             config/config_eeprom.c \
             config/config_streamer.c \

src/main/blackbox/blackbox.c

@@ -1143,10 +1143,10 @@ static void loadMainState(timeUs_t currentTimeUs)
         blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i] * blackboxHighResolutionScale);
         blackboxCurrent->gyroUnfilt[i] = lrintf(gyro.gyroADC[i] * blackboxHighResolutionScale);
 #if defined(USE_ACC)
-        blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
+        blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]);
 #endif
 #ifdef USE_MAG
-        blackboxCurrent->magADC[i] = lrintf(mag.magADC[i]);
+        blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]);
 #endif
     }
 

src/main/common/maths.c

@@ -26,7 +26,8 @@
 
 #include "build/build_config.h"
 
-#include "axis.h"
+#include "common/axis.h"
+
 #include "maths.h"
 
 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
@@ -188,44 +189,6 @@ float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float des
     return (a / b) + destFrom;
 }
 
-void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
-{
-    float cosx, sinx, cosy, siny, cosz, sinz;
-    float coszcosx, sinzcosx, coszsinx, sinzsinx;
-
-    cosx = cos_approx(delta->angles.roll);
-    sinx = sin_approx(delta->angles.roll);
-    cosy = cos_approx(delta->angles.pitch);
-    siny = sin_approx(delta->angles.pitch);
-    cosz = cos_approx(delta->angles.yaw);
-    sinz = sin_approx(delta->angles.yaw);
-
-    coszcosx = cosz * cosx;
-    sinzcosx = sinz * cosx;
-    coszsinx = sinx * cosz;
-    sinzsinx = sinx * sinz;
-
-    rotation->m[0][X] = cosz * cosy;
-    rotation->m[0][Y] = -cosy * sinz;
-    rotation->m[0][Z] = siny;
-    rotation->m[1][X] = sinzcosx + (coszsinx * siny);
-    rotation->m[1][Y] = coszcosx - (sinzsinx * siny);
-    rotation->m[1][Z] = -sinx * cosy;
-    rotation->m[2][X] = (sinzsinx) - (coszcosx * siny);
-    rotation->m[2][Y] = (coszsinx) + (sinzcosx * siny);
-    rotation->m[2][Z] = cosy * cosx;
-}
-
-void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix)
-{
-    struct fp_vector *vDest = (struct fp_vector *)v;
-    struct fp_vector vTmp = *vDest;
-
-    vDest->X = (rotationMatrix->m[0][X] * vTmp.X + rotationMatrix->m[1][X] * vTmp.Y + rotationMatrix->m[2][X] * vTmp.Z);
-    vDest->Y = (rotationMatrix->m[0][Y] * vTmp.X + rotationMatrix->m[1][Y] * vTmp.Y + rotationMatrix->m[2][Y] * vTmp.Z);
-    vDest->Z = (rotationMatrix->m[0][Z] * vTmp.X + rotationMatrix->m[1][Z] * vTmp.Y + rotationMatrix->m[2][Z] * vTmp.Z);
-}
-
 // Quick median filter implementation
 // (c) N. Devillard - 1998
 // http://ndevilla.free.fr/median/median.pdf

src/main/common/maths.h

@@ -75,18 +75,6 @@ typedef struct stdev_s
     int m_n;
 } stdev_t;
 
-// Floating point 3 vector.
-typedef struct fp_vector {
-    float X;
-    float Y;
-    float Z;
-} t_fp_vector_def;
-
-typedef union u_fp_vector {
-    float A[3];
-    t_fp_vector_def V;
-} t_fp_vector;
-
 // Floating point Euler angles.
 // Be carefull, could be either of degrees or radians.
 typedef struct fp_angles {
@@ -100,10 +88,6 @@ typedef union {
     fp_angles_def angles;
 } fp_angles_t;
 
-typedef struct fp_rotationMatrix_s {
-    float m[3][3];              // matrix
-} fp_rotationMatrix_t;
-
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);
 float fapplyDeadband(float value, float deadband);
@@ -117,9 +101,6 @@ float degreesToRadians(int16_t degrees);
 int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
 float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo);
 
-void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation);
-void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix);
-
 int32_t quickMedianFilter3(const int32_t * v);
 int32_t quickMedianFilter5(const int32_t * v);
 int32_t quickMedianFilter7(const int32_t * v);

src/main/common/sensor_alignment.c

@@ -27,29 +27,30 @@
 
 #include "common/sensor_alignment.h"
 #include "common/sensor_alignment_impl.h"
+#include "common/vector.h"
 
-void buildRotationMatrixFromAlignment(const sensorAlignment_t* sensorAlignment, fp_rotationMatrix_t* rm)
+void buildRotationMatrixFromAngles(matrix33_t *rm, const sensorAlignment_t *rpy)
 {
     fp_angles_t rotationAngles;
-    rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(sensorAlignment->roll);
-    rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(sensorAlignment->pitch);
-    rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(sensorAlignment->yaw);
+    rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(rpy->roll);
+    rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(rpy->pitch);
+    rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(rpy->yaw);
 
-    buildRotationMatrix(&rotationAngles, rm);
+    buildRotationMatrix(rm, &rotationAngles);
 }
 
 
-void buildAlignmentFromStandardAlignment(sensorAlignment_t* sensorAlignment, sensor_align_e alignment)
+void buildAlignmentFromStandardAlignment(sensorAlignment_t* rpy, sensor_align_e stdAlignment)
 {
-    if (alignment == ALIGN_CUSTOM || alignment == ALIGN_DEFAULT) {
+    if (stdAlignment == ALIGN_CUSTOM || stdAlignment == ALIGN_DEFAULT) {
         return;
     }
 
-    uint8_t alignmentBits = ALIGNMENT_TO_BITMASK(alignment);
+    uint8_t alignmentBits = ALIGNMENT_TO_BITMASK(stdAlignment);
 
-    memset(sensorAlignment, 0x00, sizeof(sensorAlignment_t));
+    memset(rpy, 0x00, sizeof(sensorAlignment_t));
 
     for (int axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
-        sensorAlignment->raw[axis] = DEGREES_TO_DECIDEGREES(90) * ALIGNMENT_AXIS_ROTATIONS(alignmentBits, axis);
+        rpy->raw[axis] = DEGREES_TO_DECIDEGREES(90) * ALIGNMENT_AXIS_ROTATIONS(alignmentBits, axis);
     }
 }

src/main/common/sensor_alignment.h

@@ -20,8 +20,9 @@
 
 #pragma once
 
-#include "axis.h"
-#include "maths.h"
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/vector.h"
 
 typedef enum {
     ALIGN_DEFAULT = 0, // driver-provided alignment
@@ -69,5 +70,5 @@ typedef union sensorAlignment_u {
 #define CUSTOM_ALIGN_CW180_DEG_FLIP  SENSOR_ALIGNMENT(  0, 180, 180)
 #define CUSTOM_ALIGN_CW270_DEG_FLIP  SENSOR_ALIGNMENT(  0, 180, 270)
 
-void buildRotationMatrixFromAlignment(const sensorAlignment_t* alignment, fp_rotationMatrix_t* rm);
-void buildAlignmentFromStandardAlignment(sensorAlignment_t* sensorAlignment, sensor_align_e alignment);
+void buildRotationMatrixFromAngles(matrix33_t *rm, const sensorAlignment_t *rpy);
+void buildAlignmentFromStandardAlignment(sensorAlignment_t *rpy, sensor_align_e stdAlignment);

src/main/common/vector.c

@@ -0,0 +1,212 @@
+/*
+ * 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/>.
+ */
+
+#include "platform.h"
+
+#include <math.h>
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "vector.h"
+
+bool vector2Equal(const vector2_t *a, const vector2_t *b)
+{
+    return (a->x == b->x) && (a->y == b->y);
+}
+
+void vector2Zero(vector2_t *v)
+{
+    v->x = 0.0f;
+    v->y = 0.0f;
+}
+
+void vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b)
+{
+    result->x = a->x + b->x;
+    result->y = a->y + b->y;
+}
+
+void vector2Scale(vector2_t *result, const vector2_t *v, const float k)
+{
+    result->x = v->x * k;
+    result->y = v->y * k;
+}
+
+float vector2Dot(const vector2_t *a, const vector2_t *b)
+{
+    return a->x * b->x + a->y * b->y;
+}
+
+float vector2Cross(const vector2_t *a, const vector2_t *b)
+{
+    return a->x * b->y - a->y * b->x;
+}
+
+float vector2NormSq(const vector2_t *v)
+{
+    return vector2Dot(v, v);
+}
+
+float vector2Norm(const vector2_t *v)
+{
+    return sqrtf(vector2NormSq(v));
+}
+
+void vector2Normalize(vector2_t *result, const vector2_t *v)
+{
+    const float normSq = vector2NormSq(v);
+
+    if (normSq > 0.0f) {
+        vector2Scale(result, v, 1.0f / sqrtf(normSq));
+    } else {
+        vector2Zero(result);
+    }
+}
+
+bool vector3Equal(const vector3_t *a, const vector3_t *b)
+{
+    return (a->x == b->x) && (a->y == b->y) && (a->z == b->z);
+}
+
+void vector3Zero(vector3_t *v)
+{
+    v->x = 0.0f;
+    v->y = 0.0f;
+    v->z = 0.0f;
+}
+
+void vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b)
+{
+    result->x = a->x + b->x;
+    result->y = a->y + b->y;
+    result->z = a->z + b->z;
+}
+
+void vector3Scale(vector3_t *result, const vector3_t *v, const float k)
+{
+    result->x = v->x * k;
+    result->y = v->y * k;
+    result->z = v->z * k;
+}
+
+float vector3Dot(const vector3_t *a, const vector3_t *b)
+{
+    return a->x * b->x + a->y * b->y + a->z * b->z;
+}
+
+void vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b)
+{
+    const vector3_t tmpA = *a;
+    const vector3_t tmpB = *b;
+
+    result->x = tmpA.y * tmpB.z - tmpA.z * tmpB.y;
+    result->y = tmpA.z * tmpB.x - tmpA.x * tmpB.z;
+    result->z = tmpA.x * tmpB.y - tmpA.y * tmpB.x;
+}
+
+float vector3NormSq(const vector3_t *v)
+{
+    return vector3Dot(v, v);
+}
+
+float vector3Norm(const vector3_t *v)
+{
+    return sqrtf(vector3NormSq(v));
+}
+
+void vector3Normalize(vector3_t *result, const vector3_t *v)
+{
+    const float normSq = vector3NormSq(v);
+
+    if (normSq > 0) {  // Hopefully sqrt(nonzero) is quite large
+        vector3Scale(result, v, 1.0f / sqrtf(normSq));
+    } else {
+        vector3Zero(result);
+    }
+}
+
+void matrixVectorMul(vector3_t * result, const matrix33_t *mat, const vector3_t *v)
+{
+    const vector3_t tmp = *v;
+
+    result->x = mat->m[0][0] * tmp.x + mat->m[0][1] * tmp.y + mat->m[0][2] * tmp.z;
+    result->y = mat->m[1][0] * tmp.x + mat->m[1][1] * tmp.y + mat->m[1][2] * tmp.z;
+    result->z = mat->m[2][0] * tmp.x + mat->m[2][1] * tmp.y + mat->m[2][2] * tmp.z;
+}
+
+void matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v)
+{
+    const vector3_t tmp = *v;
+
+    result->x = mat->m[0][0] * tmp.x + mat->m[1][0] * tmp.y + mat->m[2][0] * tmp.z;
+    result->y = mat->m[0][1] * tmp.x + mat->m[1][1] * tmp.y + mat->m[2][1] * tmp.z;
+    result->z = mat->m[0][2] * tmp.x + mat->m[1][2] * tmp.y + mat->m[2][2] * tmp.z;
+}
+
+void buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy)
+{
+    const float cosx = cos_approx(rpy->angles.roll);
+    const float sinx = sin_approx(rpy->angles.roll);
+    const float cosy = cos_approx(rpy->angles.pitch);
+    const float siny = sin_approx(rpy->angles.pitch);
+    const float cosz = cos_approx(rpy->angles.yaw);
+    const float sinz = sin_approx(rpy->angles.yaw);
+
+    const float coszcosx = cosz * cosx;
+    const float sinzcosx = sinz * cosx;
+    const float coszsinx = sinx * cosz;
+    const float sinzsinx = sinx * sinz;
+
+    result->m[0][X] = cosz * cosy;
+    result->m[0][Y] = -cosy * sinz;
+    result->m[0][Z] = siny;
+    result->m[1][X] = sinzcosx + (coszsinx * siny);
+    result->m[1][Y] = coszcosx - (sinzsinx * siny);
+    result->m[1][Z] = -sinx * cosy;
+    result->m[2][X] = (sinzsinx) - (coszcosx * siny);
+    result->m[2][Y] = (coszsinx) + (sinzcosx * siny);
+    result->m[2][Z] = cosy * cosx;
+}
+
+void applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix)
+{
+    matrixTrnVectorMul(v, rotationMatrix, v);
+}
+
+matrix33_t * yawToRotationMatrixZ(matrix33_t * result, const float yaw)
+{
+    matrix33_t r;
+    const float sinYaw = sin_approx(yaw);
+    const float cosYaw = cos_approx(yaw);
+
+    r.m[0][0] = cosYaw;
+    r.m[1][0] = sinYaw;
+    r.m[2][0] = 0.0f;
+    r.m[0][1] = -sinYaw;
+    r.m[1][1] = cosYaw;
+    r.m[2][1] = 0.0f;
+    r.m[0][2] = 0.0f;
+    r.m[1][2] = 0.0f;
+    r.m[2][2] = 1.0f;
+
+    *result = r;
+    return result;
+}