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

Vector library expansion (#12968)

Browse source
This commit is contained in:
Jan Post 2024-09-15 23:03:32 +02:00 • committed by GitHub
parent fc4b7a0008
commit f71170db1b
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
37 changed files with 630 additions and 587 deletions
Download patch file Download diff file Expand all files Collapse all files

2
mk/source.mk
View file

@ -72,6 +72,7 @@ COMMON_SRC = \
common/time.c \ common/time.c \
common/typeconversion.c \ common/typeconversion.c \
common/uvarint.c \ common/uvarint.c \
common/vector.c \
config/config.c \ config/config.c \
config/config_eeprom.c \ config/config_eeprom.c \
config/config_streamer.c \ config/config_streamer.c \
@ -408,6 +409,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
common/sdft.c \ common/sdft.c \
common/stopwatch.c \ common/stopwatch.c \
common/typeconversion.c \ common/typeconversion.c \
common/vector.c \
drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu.c \
drivers/accgyro/accgyro_mpu3050.c \ drivers/accgyro/accgyro_mpu3050.c \
drivers/accgyro/accgyro_spi_bmi160.c \ drivers/accgyro/accgyro_spi_bmi160.c \

4
src/main/blackbox/blackbox.c
View file

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

41
src/main/common/maths.c
View file

@ -26,7 +26,8 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "axis.h" #include "common/axis.h"
#include "maths.h" #include "maths.h"
#if defined(FAST_MATH) || defined(VERY_FAST_MATH) #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; 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 // Quick median filter implementation
// (c) N. Devillard - 1998 // (c) N. Devillard - 1998
// http://ndevilla.free.fr/median/median.pdf // http://ndevilla.free.fr/median/median.pdf

19
src/main/common/maths.h
View file

@ -75,18 +75,6 @@ typedef struct stdev_s
int m_n; int m_n;
} stdev_t; } 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. // Floating point Euler angles.
// Be carefull, could be either of degrees or radians. // Be carefull, could be either of degrees or radians.
typedef struct fp_angles { typedef struct fp_angles {
@ -100,10 +88,6 @@ typedef union {
fp_angles_def angles; fp_angles_def angles;
} fp_angles_t; } fp_angles_t;
typedef struct fp_rotationMatrix_s {
float m[3][3]; // matrix
} fp_rotationMatrix_t;
int gcd(int num, int denom); int gcd(int num, int denom);
int32_t applyDeadband(int32_t value, int32_t deadband); int32_t applyDeadband(int32_t value, int32_t deadband);
float fapplyDeadband(float value, float 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); int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float 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 quickMedianFilter3(const int32_t * v);
int32_t quickMedianFilter5(const int32_t * v); int32_t quickMedianFilter5(const int32_t * v);
int32_t quickMedianFilter7(const int32_t * v); int32_t quickMedianFilter7(const int32_t * v);

21
src/main/common/sensor_alignment.c
View file

@ -27,29 +27,30 @@
#include "common/sensor_alignment.h" #include "common/sensor_alignment.h"
#include "common/sensor_alignment_impl.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; fp_angles_t rotationAngles;
rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(sensorAlignment->roll); rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(rpy->roll);
rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(sensorAlignment->pitch); rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(rpy->pitch);
rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(sensorAlignment->yaw); 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; 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++) { 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);
} }
} }

9
src/main/common/sensor_alignment.h
View file

@ -20,8 +20,9 @@
#pragma once #pragma once
#include "axis.h" #include "common/axis.h"
#include "maths.h" #include "common/maths.h"
#include "common/vector.h"
typedef enum { typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment 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_CW180_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 180)
#define CUSTOM_ALIGN_CW270_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 270) #define CUSTOM_ALIGN_CW270_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 270)
void buildRotationMatrixFromAlignment(const sensorAlignment_t* alignment, fp_rotationMatrix_t* rm); void buildRotationMatrixFromAngles(matrix33_t *rm, const sensorAlignment_t *rpy);
void buildAlignmentFromStandardAlignment(sensorAlignment_t* sensorAlignment, sensor_align_e alignment); void buildAlignmentFromStandardAlignment(sensorAlignment_t *rpy, sensor_align_e stdAlignment);

209
src/main/common/vector.c Normal file
View file

@ -0,0 +1,209 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight is distributed in the hope that it 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);
}
void yawToRotationMatrixZ(matrix33_t *result, const float yaw)
{
const float sinYaw = sin_approx(yaw);
const float cosYaw = cos_approx(yaw);
result->m[0][0] = cosYaw;
result->m[1][0] = sinYaw;
result->m[2][0] = 0.0f;
result->m[0][1] = -sinYaw;
result->m[1][1] = cosYaw;
result->m[2][1] = 0.0f;
result->m[0][2] = 0.0f;
result->m[1][2] = 0.0f;
result->m[2][2] = 1.0f;
}

211
src/main/common/vector.h
View file

@ -1,19 +1,20 @@
/* /*
* This file is part of Cleanflight and Betaflight. * This file is part of Betaflight.
* *
* Cleanflight and Betaflight are free software. You can redistribute * Betaflight is free software. You can redistribute this software
* this software and/or modify this software under the terms of the * and/or modify this software under the terms of the GNU General
* GNU General Public License as published by the Free Software * Public License as published by the Free Software Foundation,
* Foundation, either version 3 of the License, or (at your option) * either version 3 of the License, or (at your option) any later
* any later version. * version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* *
* 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. * See the GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public
* along with this software. * License along with this software.
* *
* If not, see <http://www.gnu.org/licenses/>. * If not, see <http://www.gnu.org/licenses/>.
*/ */
@ -24,176 +25,52 @@
#pragma once #pragma once
#include <stdbool.h>
#include "common/maths.h" #include "common/maths.h"
typedef union { typedef union vector2_u {
float v[2]; float v[2];
struct { struct {
float x,y; float x, y;
}; };
} fpVector2_t; } vector2_t;
typedef union vector3_u {
typedef union {
float v[3]; float v[3];
struct { struct {
float x, y, z; float x, y, z;
}; };
} fpVector3_t; } vector3_t;
typedef struct { typedef struct matrix33_s {
float m[3][3]; float m[3][3];
} fpMat33_t; } matrix33_t;
static inline fpVector3_t * vectorZero(fpVector3_t *v) bool vector2Equal(const vector2_t *a, const vector2_t *b);
{ void vector2Zero(vector2_t *v);
v->x = 0.0f; void vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b);
v->y = 0.0f; void vector2Scale(vector2_t *result, const vector2_t *v, const float k);
v->z = 0.0f; float vector2Dot(const vector2_t *a, const vector2_t *b);
return v; float vector2Cross(const vector2_t *a, const vector2_t *b);
} float vector2NormSq(const vector2_t *v);
float vector2Norm(const vector2_t *v);
void vector2Normalize(vector2_t *result, const vector2_t *v);
static inline float vectorNormSquared(const fpVector3_t * v) bool vector3Equal(const vector3_t *a, const vector3_t *b);
{ void vector3Zero(vector3_t *v);
return sq(v->x) + sq(v->y) + sq(v->z); void vector3Add(vector3_t *result, const vector3_t *a, const vector3_t *b);
} void vector3Scale(vector3_t *result, const vector3_t *v, const float k);
float vector3Dot(const vector3_t *a, const vector3_t *b);
void vector3Cross(vector3_t *result, const vector3_t *a, const vector3_t *b);
float vector3NormSq(const vector3_t *v);
float vector3Norm(const vector3_t *v);
void vector3Normalize(vector3_t *result, const vector3_t *v);
static inline float vectorNorm(const fpVector3_t * v) void matrixVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
{ void matrixTrnVectorMul(vector3_t *result, const matrix33_t *mat, const vector3_t *v);
return sqrtf(vectorNormSquared(v));
}
static inline fpVector3_t * vectorCrossProduct(fpVector3_t *result, const fpVector3_t *a, const fpVector3_t *b) void buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy);
{ void applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix);
fpVector3_t ab;
ab.x = a->y * b->z - a->z * b->y;
ab.y = a->z * b->x - a->x * b->z;
ab.z = a->x * b->y - a->y * b->x;
*result = ab;
return result;
}
static inline fpVector3_t * vectorAdd(fpVector3_t *result, const fpVector3_t *a, const fpVector3_t *b)
{
fpVector3_t ab;
ab.x = a->x + b->x;
ab.y = a->y + b->y;
ab.z = a->z + b->z;
*result = ab;
return result;
}
static inline fpVector3_t * vectorScale(fpVector3_t *result, const fpVector3_t *a, const float b)
{
fpVector3_t ab;
ab.x = a->x * b;
ab.y = a->y * b;
ab.z = a->z * b;
*result = ab;
return result;
}
static inline fpVector3_t * vectorNormalize(fpVector3_t *result, const fpVector3_t *v)
{
float normSq = vectorNormSquared(v);
if (normSq > 0) { // Hopefully sqrt(nonzero) is quite large
return vectorScale(result, v, 1.0f / sqrtf(normSq));
} else {
return vectorZero(result);
}
}
static inline float vector2NormSquared(const fpVector2_t *a)
{
return sq(a->x) + sq(a->y);
}
static inline float vector2Norm(const fpVector2_t *a)
{
return sqrtf(vector2NormSquared(a));
}
static inline fpVector2_t * vector2Scale(fpVector2_t *result, const fpVector2_t *a, const float b)
{
fpVector2_t ab;
ab.x = a->x * b;
ab.y = a->y * b;
*result = ab;
return result;
}
static inline fpVector2_t * vector2Normalize(fpVector2_t *result, const fpVector2_t *v)
{
float normSq = vector2NormSquared(v);
if (normSq > 0.0f) {
return vector2Scale(result, v, 1.0f / sqrtf(normSq));
} else {
*result = (fpVector2_t){.x = 0.0f, .y = 0.0f};
return result;
}
}
static inline fpVector3_t * matrixVectorMul(fpVector3_t * result, const fpMat33_t * mat, const fpVector3_t * a)
{
fpVector3_t r;
r.x = mat->m[0][0] * a->x + mat->m[0][1] * a->y + mat->m[0][2] * a->z;
r.y = mat->m[1][0] * a->x + mat->m[1][1] * a->y + mat->m[1][2] * a->z;
r.z = mat->m[2][0] * a->x + mat->m[2][1] * a->y + mat->m[2][2] * a->z;
*result = r;
return result;
}
static inline fpMat33_t * yawToRotationMatrixZ(fpMat33_t * result, const float yaw)
{
fpMat33_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;
}
static inline fpVector3_t * matrixTrnVectorMul(fpVector3_t * result, const fpMat33_t * mat, const fpVector3_t * a)
{
fpVector3_t r;
r.x = mat->m[0][0] * a->x + mat->m[1][0] * a->y + mat->m[2][0] * a->z;
r.y = mat->m[0][1] * a->x + mat->m[1][1] * a->y + mat->m[2][1] * a->z;
r.z = mat->m[0][2] * a->x + mat->m[1][2] * a->y + mat->m[2][2] * a->z;
*result = r;
return result;
}
static inline float vector2Cross(const fpVector2_t *a, const fpVector2_t *b)
{
return a->x * b->y - a->y * b->x;
}
static inline float vector2Dot(const fpVector2_t *a, const fpVector2_t *b)
{
return a->x * b->x + a->y * b->y;
}
void yawToRotationMatrixZ(matrix33_t *result, const float yaw);

7
src/main/drivers/accgyro/accgyro.h
View file

@ -26,6 +26,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/sensor_alignment.h" #include "common/sensor_alignment.h"
#include "common/time.h" #include "common/time.h"
#include "common/vector.h"
#include "drivers/accgyro/accgyro_mpu.h" #include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/bus.h" #include "drivers/bus.h"
@ -103,7 +104,7 @@ typedef struct gyroDev_s {
extDevice_t dev; extDevice_t dev;
float scale; // scalefactor float scale; // scalefactor
float gyroZero[XYZ_AXIS_COUNT]; float gyroZero[XYZ_AXIS_COUNT];
float gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment vector3_t gyroADC; // gyro data after calibration and alignment
int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT];
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; // raw data from sensor int16_t gyroADCRaw[XYZ_AXIS_COUNT]; // raw data from sensor
int16_t temperature; int16_t temperature;
@ -125,7 +126,7 @@ typedef struct gyroDev_s {
ioTag_t mpuIntExtiTag; ioTag_t mpuIntExtiTag;
uint8_t gyroHasOverflowProtection; uint8_t gyroHasOverflowProtection;
gyroHardware_e gyroHardware; gyroHardware_e gyroHardware;
fp_rotationMatrix_t rotationMatrix; matrix33_t rotationMatrix;
uint16_t gyroSampleRateHz; uint16_t gyroSampleRateHz;
uint16_t accSampleRateHz; uint16_t accSampleRateHz;
uint8_t accDataReg; uint8_t accDataReg;
@ -148,7 +149,7 @@ typedef struct accDev_s {
bool acc_high_fsr; bool acc_high_fsr;
char revisionCode; // a revision code for the sensor, if known char revisionCode; // a revision code for the sensor, if known
uint8_t filler[2]; uint8_t filler[2];
fp_rotationMatrix_t rotationMatrix; matrix33_t rotationMatrix;
} accDev_t; } accDev_t;
static inline void accDevLock(accDev_t *acc) static inline void accDevLock(accDev_t *acc)

3
src/main/drivers/compass/compass.h
View file

@ -21,6 +21,7 @@
#pragma once #pragma once
#include "common/sensor_alignment.h" #include "common/sensor_alignment.h"
#include "common/vector.h"
#include "drivers/bus.h" #include "drivers/bus.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
@ -33,7 +34,7 @@ typedef struct magDev_s {
extDevice_t dev; extDevice_t dev;
busDevice_t bus; // For MPU slave bus instance busDevice_t bus; // For MPU slave bus instance
sensor_align_e magAlignment; sensor_align_e magAlignment;
fp_rotationMatrix_t rotationMatrix; matrix33_t rotationMatrix;
ioTag_t magIntExtiTag; ioTag_t magIntExtiTag;
int16_t magGain[3]; int16_t magGain[3];
uint16_t magOdrHz; uint16_t magOdrHz;

17
src/main/fc/rc.c
View file

@ -29,6 +29,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/utils.h" #include "common/utils.h"
#include "common/vector.h"
#include "config/config.h" #include "config/config.h"
#include "config/feature.h" #include "config/feature.h"
@ -781,20 +782,20 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
} }
} }
if (FLIGHT_MODE(HEADFREE_MODE)) { if (FLIGHT_MODE(HEADFREE_MODE)) {
static t_fp_vector_def rcCommandBuff; static vector3_t rcCommandBuff;
rcCommandBuff.X = rcCommand[ROLL]; rcCommandBuff.x = rcCommand[ROLL];
rcCommandBuff.Y = rcCommand[PITCH]; rcCommandBuff.y = rcCommand[PITCH];
if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) { if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
rcCommandBuff.Z = rcCommand[YAW]; rcCommandBuff.z = rcCommand[YAW];
} else { } else {
rcCommandBuff.Z = 0; rcCommandBuff.z = 0;
} }
imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff); imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
rcCommand[ROLL] = rcCommandBuff.X; rcCommand[ROLL] = rcCommandBuff.x;
rcCommand[PITCH] = rcCommandBuff.Y; rcCommand[PITCH] = rcCommandBuff.y;
if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) { if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
rcCommand[YAW] = rcCommandBuff.Z; rcCommand[YAW] = rcCommandBuff.z;
} }
} }
} }

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

@ -30,9 +30,6 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#include "common/axis.h"
#include "common/vector.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
@ -96,8 +93,8 @@ static float smallAngleCosZ = 0;
static imuRuntimeConfig_t imuRuntimeConfig; static imuRuntimeConfig_t imuRuntimeConfig;
float rMat[3][3]; matrix33_t rMat;
static fpVector2_t north_ef; static vector2_t north_ef;
#if defined(USE_ACC) #if defined(USE_ACC)
STATIC_UNIT_TESTED bool attitudeIsEstablished = false; STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
@ -147,21 +144,21 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
{ {
imuQuaternionComputeProducts(&q, &qP); imuQuaternionComputeProducts(&q, &qP);
rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz; rMat.m[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
rMat[0][1] = 2.0f * (qP.xy + -qP.wz); rMat.m[0][1] = 2.0f * (qP.xy + -qP.wz);
rMat[0][2] = 2.0f * (qP.xz - -qP.wy); rMat.m[0][2] = 2.0f * (qP.xz - -qP.wy);
rMat[1][0] = 2.0f * (qP.xy - -qP.wz); rMat.m[1][0] = 2.0f * (qP.xy - -qP.wz);
rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz; rMat.m[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
rMat[1][2] = 2.0f * (qP.yz + -qP.wx); rMat.m[1][2] = 2.0f * (qP.yz + -qP.wx);
rMat[2][0] = 2.0f * (qP.xz + -qP.wy); rMat.m[2][0] = 2.0f * (qP.xz + -qP.wy);
rMat[2][1] = 2.0f * (qP.yz - -qP.wx); rMat.m[2][1] = 2.0f * (qP.yz - -qP.wx);
rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy; rMat.m[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy;
#if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) && !defined(SET_IMU_FROM_EULER) #if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) && !defined(SET_IMU_FROM_EULER)
rMat[1][0] = -2.0f * (qP.xy - -qP.wz); rMat.m[1][0] = -2.0f * (qP.xy - -qP.wz);
rMat[2][0] = -2.0f * (qP.xz + -qP.wy); rMat.m[2][0] = -2.0f * (qP.xz + -qP.wy);
#endif #endif
} }
@ -230,9 +227,9 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt,
// Add error from magnetometer and Cog // Add error from magnetometer and Cog
// just rotate input value to body frame // just rotate input value to body frame
ex += rMat[Z][X] * (headingErrCog + headingErrMag); ex += rMat.m[Z][X] * (headingErrCog + headingErrMag);
ey += rMat[Z][Y] * (headingErrCog + headingErrMag); ey += rMat.m[Z][Y] * (headingErrCog + headingErrMag);
ez += rMat[Z][Z] * (headingErrCog + headingErrMag); ez += rMat.m[Z][Z] * (headingErrCog + headingErrMag);
DEBUG_SET(DEBUG_ATTITUDE, 3, (headingErrCog * 100)); DEBUG_SET(DEBUG_ATTITUDE, 3, (headingErrCog * 100));
DEBUG_SET(DEBUG_ATTITUDE, 7, lrintf(dcmKpGain * 100.0f)); DEBUG_SET(DEBUG_ATTITUDE, 7, lrintf(dcmKpGain * 100.0f));
@ -248,9 +245,9 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt,
az *= recipAccNorm; az *= recipAccNorm;
// Error is sum of cross product between estimated direction and measured direction of gravity // Error is sum of cross product between estimated direction and measured direction of gravity
ex += (ay * rMat[2][2] - az * rMat[2][1]); ex += (ay * rMat.m[2][2] - az * rMat.m[2][1]);
ey += (az * rMat[2][0] - ax * rMat[2][2]); ey += (az * rMat.m[2][0] - ax * rMat.m[2][2]);
ez += (ax * rMat[2][1] - ay * rMat[2][0]); ez += (ax * rMat.m[2][1] - ay * rMat.m[2][0]);
} }
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
@ -313,9 +310,9 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (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))); 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 { } else {
attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf)); attitude.values.roll = lrintf(atan2_approx(rMat.m[2][1], rMat.m[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.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat.m[2][0])) * (1800.0f / M_PIf));
attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf))); attitude.values.yaw = lrintf((-atan2_approx(rMat.m[1][0], rMat.m[0][0]) * (1800.0f / M_PIf)));
} }
if (attitude.values.yaw < 0) { if (attitude.values.yaw < 0) {
@ -455,10 +452,10 @@ STATIC_UNIT_TESTED float imuCalcCourseErr(float courseOverGround)
{ {
// Compute COG heading unit vector in earth frame (ef) from scalar GPS CourseOverGround // Compute COG heading unit vector in earth frame (ef) from scalar GPS CourseOverGround
// Earth frame X is pointing north and sin/cos argument is anticlockwise. (|cog_ef| == 1.0) // Earth frame X is pointing north and sin/cos argument is anticlockwise. (|cog_ef| == 1.0)
const fpVector2_t cog_ef = {.x = cos_approx(-courseOverGround), .y = sin_approx(-courseOverGround)}; const vector2_t cog_ef = {.x = cos_approx(-courseOverGround), .y = sin_approx(-courseOverGround)};
// Compute and normalise craft Earth frame heading vector from body X axis // Compute and normalise craft Earth frame heading vector from body X axis
fpVector2_t heading_ef = {.x = rMat[X][X], .y = rMat[Y][X]}; vector2_t heading_ef = {.x = rMat.m[X][X], .y = rMat.m[Y][X]};
vector2Normalize(&heading_ef, &heading_ef); // XY only, normalised to magnitude 1.0 vector2Normalize(&heading_ef, &heading_ef); // XY only, normalised to magnitude 1.0
// cross (vector product) = |heading| * |cog| * sin(angle) = 1 * 1 * sin(angle) // cross (vector product) = |heading| * |cog| * sin(angle) = 1 * 1 * sin(angle)
@ -491,14 +488,17 @@ static void imuDebug_GPS_RESCUE_HEADING(void)
// Encapsulate additional operations in a block so that it is only executed when the according debug mode is used // Encapsulate additional operations in a block so that it is only executed when the according debug mode is used
// Only re-calculate magYaw when there is a new Mag data reading, to avoid spikes // Only re-calculate magYaw when there is a new Mag data reading, to avoid spikes
if (debugMode == DEBUG_GPS_RESCUE_HEADING && mag.isNewMagADCFlag) { if (debugMode == DEBUG_GPS_RESCUE_HEADING && mag.isNewMagADCFlag) {
fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
fpVector3_t mag_ef;
matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF true north
fpMat33_t rMatZTrans; vector3_t mag_bf = mag.magADC;
yawToRotationMatrixZ(&rMatZTrans, -atan2_approx(rMat[1][0], rMat[0][0])); vector3_t mag_ef;
fpVector3_t mag_ef_yawed; matrixVectorMul(&mag_ef, &rMat, &mag_bf); // BF->EF true north
matrix33_t rMatZTrans;
yawToRotationMatrixZ(&rMatZTrans, -atan2_approx(rMat.m[1][0], rMat.m[0][0]));
vector3_t mag_ef_yawed;
matrixVectorMul(&mag_ef_yawed, &rMatZTrans, &mag_ef); // EF->EF yawed matrixVectorMul(&mag_ef_yawed, &rMatZTrans, &mag_ef); // EF->EF yawed
// Magnetic yaw is the angle between true north and the X axis of the body frame // Magnetic yaw is the angle between true north and the X axis of the body frame
int16_t magYaw = lrintf((atan2_approx(mag_ef_yawed.y, mag_ef_yawed.x) * (1800.0f / M_PIf))); int16_t magYaw = lrintf((atan2_approx(mag_ef_yawed.y, mag_ef_yawed.x) * (1800.0f / M_PIf)));
if (magYaw < 0) { if (magYaw < 0) {
@ -518,19 +518,19 @@ static void imuDebug_GPS_RESCUE_HEADING(void)
STATIC_UNIT_TESTED float imuCalcMagErr(void) STATIC_UNIT_TESTED float imuCalcMagErr(void)
{ {
// Use measured magnetic field vector // Use measured magnetic field vector
fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}}; vector3_t mag_bf = mag.magADC;
float magNormSquared = vectorNormSquared(&mag_bf); float magNormSquared = vector3NormSq(&mag_bf);
if (magNormSquared > 0.01f) { if (magNormSquared > 0.01f) {
// project magnetometer reading into Earth frame // project magnetometer reading into Earth frame
fpVector3_t mag_ef; vector3_t mag_ef;
matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF true north matrixVectorMul(&mag_ef, &rMat, &mag_bf); // BF->EF true north
// Normalise magnetometer measurement // Normalise magnetometer measurement
vectorScale(&mag_ef, &mag_ef, 1.0f / sqrtf(magNormSquared)); vector3Scale(&mag_ef, &mag_ef, 1.0f / sqrtf(magNormSquared));
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). // 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 // This way magnetic field will only affect heading and wont mess roll/pitch angles
fpVector2_t mag2d_ef = {.x = mag_ef.x, .y = mag_ef.y}; vector2_t mag2d_ef = {.x = mag_ef.x, .y = mag_ef.y};
// mag2d_ef - measured mag field vector in EF (2D ground plane projection) // mag2d_ef - measured mag field vector in EF (2D ground plane projection)
// north_ef - reference mag field vector heading due North in EF (2D ground plane projection). // north_ef - reference mag field vector heading due North in EF (2D ground plane projection).
// Adjusted for magnetic declination (in imuConfigure) // Adjusted for magnetic declination (in imuConfigure)
@ -682,7 +682,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
const bool useAcc = imuIsAccelerometerHealthy(); // all smoothed accADC values are within 10% of 1G const bool useAcc = imuIsAccelerometerHealthy(); // all smoothed accADC values are within 10% of 1G
imuMahonyAHRSupdate(dt, imuMahonyAHRSupdate(dt,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z], useAcc, acc.accADC.x, acc.accADC.y, acc.accADC.z,
magErr, cogErr, magErr, cogErr,
imuCalcKpGain(currentTimeUs, useAcc, gyroAverage)); imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
@ -731,9 +731,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
mixerSetThrottleAngleCorrection(throttleAngleCorrection); mixerSetThrottleAngleCorrection(throttleAngleCorrection);
} else { } else {
acc.accADC[X] = 0; acc.accADC.x = 0;
acc.accADC[Y] = 0; acc.accADC.y = 0;
acc.accADC[Z] = 0; acc.accADC.z = 0;
schedulerIgnoreTaskStateTime(); schedulerIgnoreTaskStateTime();
} }
@ -746,12 +746,12 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
// Positive angle - nose down, negative angle - nose up. // Positive angle - nose down, negative angle - nose up.
float getSinPitchAngle(void) float getSinPitchAngle(void)
{ {
return -rMat[2][0]; return -rMat.m[2][0];
} }
float getCosTiltAngle(void) float getCosTiltAngle(void)
{ {
return rMat[2][2]; return rMat.m[2][2];
} }
void getQuaternion(quaternion *quat) void getQuaternion(quaternion *quat)
@ -837,20 +837,20 @@ void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *res
result->z = D + (+ E - F - G + H) / 2.0f; result->z = D + (+ E - F - G + H) / 2.0f;
} }
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v) void imuQuaternionHeadfreeTransformVectorEarthToBody(vector3_t *v)
{ {
quaternionProducts buffer; quaternionProducts buffer;
imuQuaternionMultiplication(&offset, &q, &headfree); imuQuaternionMultiplication(&offset, &q, &headfree);
imuQuaternionComputeProducts(&headfree, &buffer); 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 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 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; 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->x = x;
v->Y = y; v->y = y;
v->Z = z; v->z = z;
} }
bool isUpright(void) bool isUpright(void)

6
src/main/flight/imu.h
View file

@ -23,6 +23,8 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/time.h" #include "common/time.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "pg/pg.h" #include "pg/pg.h"
// Exported symbols // Exported symbols
@ -50,7 +52,7 @@ typedef union {
#define EULER_INITIALIZE { { 0, 0, 0 } } #define EULER_INITIALIZE { { 0, 0, 0 } }
extern attitudeEulerAngles_t attitude; extern attitudeEulerAngles_t attitude;
extern float rMat[3][3]; extern matrix33_t rMat;
typedef struct imuConfig_s { typedef struct imuConfig_s {
uint16_t imu_dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t imu_dcm_kp; // DCM filter proportional gain ( x 10000)
@ -85,6 +87,6 @@ void imuSetHasNewData(uint32_t dt);
#endif #endif
bool imuQuaternionHeadfreeOffsetSet(void); bool imuQuaternionHeadfreeOffsetSet(void);
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v); void imuQuaternionHeadfreeTransformVectorEarthToBody(vector3_t *v);
bool shouldInitializeGPSHeading(void); bool shouldInitializeGPSHeading(void);
bool isUpright(void); bool isUpright(void);

4
src/main/io/dashboard.c
View file

@ -487,7 +487,7 @@ static void showSensorsPage(void)
#if defined(USE_ACC) #if defined(USE_ACC)
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC[X]), lrintf(acc.accADC[Y]), lrintf(acc.accADC[Z])); tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC.x), lrintf(acc.accADC.y), lrintf(acc.accADC.z));
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(dev, rowIndex++); i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer); i2c_OLED_send_string(dev, lineBuffer);
@ -503,7 +503,7 @@ static void showSensorsPage(void)
#ifdef USE_MAG #ifdef USE_MAG
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC[X]), lrintf(mag.magADC[Y]), lrintf(mag.magADC[Z])); tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC.x), lrintf(mag.magADC.y), lrintf(mag.magADC.z));
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(dev, rowIndex++); i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer); i2c_OLED_send_string(dev, lineBuffer);

4
src/main/msp/msp.c
View file

@ -1136,7 +1136,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
#if defined(USE_ACC) #if defined(USE_ACC)
sbufWriteU16(dst, lrintf(acc.accADC[i])); sbufWriteU16(dst, lrintf(acc.accADC.v[i]));
#else #else
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif
@ -1146,7 +1146,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
#if defined(USE_MAG) #if defined(USE_MAG)
sbufWriteU16(dst, lrintf(mag.magADC[i])); sbufWriteU16(dst, lrintf(mag.magADC.v[i]));
#else #else
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif

6
src/main/osd/osd.c
View file

@ -1316,11 +1316,7 @@ void osdProcessStats3(void)
if (sensors(SENSOR_ACC) if (sensors(SENSOR_ACC)
&& (VISIBLE(osdElementConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) { && (VISIBLE(osdElementConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) {
// only calculate the G force if the element is visible or the stat is enabled // only calculate the G force if the element is visible or the stat is enabled
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { osdGForce = vector3Norm(&acc.accADC) * acc.dev.acc_1G_rec;
const float a = acc.accADC[axis];
osdGForce += a * a;
}
osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
} }
#endif #endif
} }

2
src/main/osd/osd_elements.c
View file

@ -761,7 +761,7 @@ static void osdElementArtificialHorizon(osdElementParms_t *element)
static void osdElementUpDownReference(osdElementParms_t *element) static void osdElementUpDownReference(osdElementParms_t *element)
{ {
// Up/Down reference feature displays reference points on the OSD at Zenith and Nadir // Up/Down reference feature displays reference points on the OSD at Zenith and Nadir
const float earthUpinBodyFrame[3] = {-rMat[2][0], -rMat[2][1], -rMat[2][2]}; //transforum the up vector to the body frame const float earthUpinBodyFrame[3] = {-rMat.m[2][0], -rMat.m[2][1], -rMat.m[2][2]}; //transforum the up vector to the body frame
if (fabsf(earthUpinBodyFrame[2]) < SINE_25_DEG && fabsf(earthUpinBodyFrame[1]) < SINE_25_DEG) { if (fabsf(earthUpinBodyFrame[2]) < SINE_25_DEG && fabsf(earthUpinBodyFrame[1]) < SINE_25_DEG) {
float thetaB; // pitch from body frame to zenith/nadir float thetaB; // pitch from body frame to zenith/nadir

18
src/main/sensors/acceleration.c
View file

@ -43,9 +43,9 @@ FAST_DATA_ZERO_INIT acc_t acc; // acc access functions
static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims) static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
{ {
acc.accADC[X] -= accelerationTrims->raw[X]; acc.accADC.x -= accelerationTrims->raw[X];
acc.accADC[Y] -= accelerationTrims->raw[Y]; acc.accADC.y -= accelerationTrims->raw[Y];
acc.accADC[Z] -= accelerationTrims->raw[Z]; acc.accADC.z -= accelerationTrims->raw[Z];
} }
void accUpdate(timeUs_t currentTimeUs) void accUpdate(timeUs_t currentTimeUs)
@ -60,13 +60,13 @@ void accUpdate(timeUs_t currentTimeUs)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
const int16_t val = acc.dev.ADCRaw[axis]; const int16_t val = acc.dev.ADCRaw[axis];
acc.accADC[axis] = val; acc.accADC.v[axis] = val;
} }
if (acc.dev.accAlign == ALIGN_CUSTOM) { if (acc.dev.accAlign == ALIGN_CUSTOM) {
alignSensorViaMatrix(acc.accADC, &acc.dev.rotationMatrix); alignSensorViaMatrix(&acc.accADC, &acc.dev.rotationMatrix);
} else { } else {
alignSensorViaRotation(acc.accADC, acc.dev.accAlign); alignSensorViaRotation(&acc.accADC, acc.dev.accAlign);
} }
if (!accIsCalibrationComplete()) { if (!accIsCalibrationComplete()) {
@ -81,9 +81,9 @@ void accUpdate(timeUs_t currentTimeUs)
float accAdcSquaredSum = 0.0f; float accAdcSquaredSum = 0.0f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
const float val = acc.accADC[axis]; const float val = acc.accADC.v[axis];
acc.accADC[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val; acc.accADC.v[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val;
accAdcSquaredSum += sq(acc.accADC[axis]); accAdcSquaredSum += sq(acc.accADC.v[axis]);
} }
acc.accMagnitude = sqrtf(accAdcSquaredSum) * acc.dev.acc_1G_rec; // normally 1.0; used for disarm on impact detection acc.accMagnitude = sqrtf(accAdcSquaredSum) * acc.dev.acc_1G_rec; // normally 1.0; used for disarm on impact detection
acc.accDelta = (acc.accMagnitude - previousAccMagnitude) * acc.sampleRateHz; acc.accDelta = (acc.accMagnitude - previousAccMagnitude) * acc.sampleRateHz;

2
src/main/sensors/acceleration.h
View file

@ -54,7 +54,7 @@ typedef enum {
typedef struct acc_s { typedef struct acc_s {
accDev_t dev; accDev_t dev;
uint16_t sampleRateHz; uint16_t sampleRateHz;
float accADC[XYZ_AXIS_COUNT]; vector3_t accADC;
bool isAccelUpdatedAtLeastOnce; bool isAccelUpdatedAtLeastOnce;
float accMagnitude; float accMagnitude;
float accDelta; float accDelta;

10
src/main/sensors/acceleration_init.c
View file

@ -407,7 +407,7 @@ bool accInit(uint16_t accSampleRateHz)
} }
#endif #endif
acc.dev.accAlign = alignment; acc.dev.accAlign = alignment;
buildRotationMatrixFromAlignment(customAlignment, &acc.dev.rotationMatrix); buildRotationMatrixFromAngles(&acc.dev.rotationMatrix, customAlignment);
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
return false; return false;
@ -453,10 +453,10 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
} }
// Sum up CALIBRATING_ACC_CYCLES readings // Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += acc.accADC[axis]; a[axis] += acc.accADC.v[axis];
// Reset global variables to prevent other code from using un-calibrated data // Reset global variables to prevent other code from using un-calibrated data
acc.accADC[axis] = 0; acc.accADC.v[axis] = 0;
accelerationRuntime.accelerationTrims->raw[axis] = 0; accelerationRuntime.accelerationTrims->raw[axis] = 0;
} }
@ -495,9 +495,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
if (InflightcalibratingA == 50) if (InflightcalibratingA == 50)
b[axis] = 0; b[axis] = 0;
// Sum up 50 readings // Sum up 50 readings
b[axis] += acc.accADC[axis]; b[axis] += acc.accADC.v[axis];
// Clear global variables for next reading // Clear global variables for next reading
acc.accADC[axis] = 0; acc.accADC.v[axis] = 0;
accelerationRuntime.accelerationTrims->raw[axis] = 0; accelerationRuntime.accelerationTrims->raw[axis] = 0;
} }
// all values are measured // all values are measured

68
src/main/sensors/boardalignment.c
View file

@ -26,8 +26,6 @@
#include "platform.h" #include "platform.h"
#include "common/utils.h" #include "common/utils.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/sensor_alignment.h" #include "common/sensor_alignment.h"
#include "pg/pg.h" #include "pg/pg.h"
@ -38,7 +36,7 @@
#include "boardalignment.h" #include "boardalignment.h"
static bool standardBoardAlignment = true; // board orientation correction static bool standardBoardAlignment = true; // board orientation correction
static fp_rotationMatrix_t boardRotation; static matrix33_t boardRotation;
PG_REGISTER_WITH_RESET_TEMPLATE(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 1); PG_REGISTER_WITH_RESET_TEMPLATE(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 1);
@ -76,70 +74,68 @@ void initBoardAlignment(const boardAlignment_t *boardAlignment)
rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees); rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees ); rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees );
buildRotationMatrix(&rotationAngles, &boardRotation); buildRotationMatrix(&boardRotation, &rotationAngles);
} }
static void alignBoard(float *vec) static void alignBoard(vector3_t *vec)
{ {
applyMatrixRotation(vec, &boardRotation); applyRotationMatrix(vec, &boardRotation);
} }
FAST_CODE_NOINLINE void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* sensorRotationMatrix) FAST_CODE_NOINLINE void alignSensorViaMatrix(vector3_t *dest, matrix33_t *sensorRotationMatrix)
{ {
applyMatrixRotation(dest, sensorRotationMatrix); applyRotationMatrix(dest, sensorRotationMatrix);
if (!standardBoardAlignment) { if (!standardBoardAlignment) {
alignBoard(dest); alignBoard(dest);
} }
} }
void alignSensorViaRotation(float *dest, uint8_t rotation) void alignSensorViaRotation(vector3_t *dest, sensor_align_e rotation)
{ {
const float x = dest[X]; const vector3_t tmp = *dest;
const float y = dest[Y];
const float z = dest[Z];
switch (rotation) { switch (rotation) {
default: default:
case CW0_DEG: case CW0_DEG:
dest[X] = x; dest->x = tmp.x;
dest[Y] = y; dest->y = tmp.y;
dest[Z] = z; dest->z = tmp.z;
break; break;
case CW90_DEG: case CW90_DEG:
dest[X] = y; dest->x = tmp.y;
dest[Y] = -x; dest->y = -tmp.x;
dest[Z] = z; dest->z = tmp.z;
break; break;
case CW180_DEG: case CW180_DEG:
dest[X] = -x; dest->x = -tmp.x;
dest[Y] = -y; dest->y = -tmp.y;
dest[Z] = z; dest->z = tmp.z;
break; break;
case CW270_DEG: case CW270_DEG:
dest[X] = -y; dest->x = -tmp.y;
dest[Y] = x; dest->y = tmp.x;
dest[Z] = z; dest->z = tmp.z;
break; break;
case CW0_DEG_FLIP: case CW0_DEG_FLIP:
dest[X] = -x; dest->x = -tmp.x;
dest[Y] = y; dest->y = tmp.y;
dest[Z] = -z; dest->z = -tmp.z;
break; break;
case CW90_DEG_FLIP: case CW90_DEG_FLIP:
dest[X] = y; dest->x = tmp.y;
dest[Y] = x; dest->y = tmp.x;
dest[Z] = -z; dest->z = -tmp.z;
break; break;
case CW180_DEG_FLIP: case CW180_DEG_FLIP:
dest[X] = x; dest->x = tmp.x;
dest[Y] = -y; dest->y = -tmp.y;
dest[Z] = -z; dest->z = -tmp.z;
break; break;
case CW270_DEG_FLIP: case CW270_DEG_FLIP:
dest[X] = -y; dest->x = -tmp.y;
dest[Y] = -x; dest->y = -tmp.x;
dest[Z] = -z; dest->z = -tmp.z;
break; break;
} }

5
src/main/sensors/boardalignment.h
View file

@ -22,6 +22,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "pg/pg.h" #include "pg/pg.h"
@ -33,7 +34,7 @@ typedef struct boardAlignment_s {
PG_DECLARE(boardAlignment_t, boardAlignment); PG_DECLARE(boardAlignment_t, boardAlignment);
void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* rotationMatrix); void alignSensorViaMatrix(vector3_t *dest, matrix33_t *rotationMatrix);
void alignSensorViaRotation(float *dest, uint8_t rotation); void alignSensorViaRotation(vector3_t *dest, sensor_align_e rotation);
void initBoardAlignment(const boardAlignment_t *boardAlignment); void initBoardAlignment(const boardAlignment_t *boardAlignment);

26
src/main/sensors/compass.c
View file

@ -386,7 +386,7 @@ bool compassInit(void)
magDev.magAlignment = compassConfig()->mag_alignment; magDev.magAlignment = compassConfig()->mag_alignment;
} }
buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix); buildRotationMatrixFromAngles(&magDev.rotationMatrix, &compassConfig()->mag_customAlignment);
compassBiasEstimatorInit(&compassBiasEstimator, LAMBDA_MIN, P0); compassBiasEstimatorInit(&compassBiasEstimator, LAMBDA_MIN, P0);
@ -405,7 +405,7 @@ bool compassInit(void)
bool compassIsHealthy(void) bool compassIsHealthy(void)
{ {
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0); return (mag.magADC.x != 0) && (mag.magADC.y != 0) && (mag.magADC.z != 0);
} }
void compassStartCalibration(void) void compassStartCalibration(void)
@ -449,15 +449,15 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
// if we get here, we have new data to parse // if we get here, we have new data to parse
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] = magADCRaw[axis]; mag.magADC.v[axis] = magADCRaw[axis];
} }
// If debug_mode is DEBUG_GPS_RESCUE_HEADING, we should update the magYaw value, after which isNewMagADCFlag will be set false // If debug_mode is DEBUG_GPS_RESCUE_HEADING, we should update the magYaw value, after which isNewMagADCFlag will be set false
mag.isNewMagADCFlag = true; mag.isNewMagADCFlag = true;
if (magDev.magAlignment == ALIGN_CUSTOM) { if (magDev.magAlignment == ALIGN_CUSTOM) {
alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix); alignSensorViaMatrix(&mag.magADC, &magDev.rotationMatrix);
} else { } else {
alignSensorViaRotation(mag.magADC, magDev.magAlignment); alignSensorViaRotation(&mag.magADC, magDev.magAlignment);
} }
// get stored cal/bias values // get stored cal/bias values
@ -486,7 +486,7 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
if (didMovementStart) { if (didMovementStart) {
// LED will flash at task rate while calibrating, looks like 'ON' all the time. // LED will flash at task rate while calibrating, looks like 'ON' all the time.
LED0_ON; LED0_ON;
compassBiasEstimatorApply(&compassBiasEstimator, mag.magADC); compassBiasEstimatorApply(&compassBiasEstimator, &mag.magADC);
} }
} else { } else {
// mag cal process is not complete until the new cal values are saved // mag cal process is not complete until the new cal values are saved
@ -511,18 +511,18 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
// remove saved cal/bias; this is zero while calibrating // remove saved cal/bias; this is zero while calibrating
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] -= magZero->raw[axis]; mag.magADC.v[axis] -= magZero->raw[axis];
} }
if (debugMode == DEBUG_MAG_CALIB) { if (debugMode == DEBUG_MAG_CALIB) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// DEBUG 0-2: magADC[X], magADC[Y], magADC[Z] // DEBUG 0-2: magADC.x, magADC.y, magADC.z
DEBUG_SET(DEBUG_MAG_CALIB, axis, lrintf(mag.magADC[axis])); DEBUG_SET(DEBUG_MAG_CALIB, axis, lrintf(mag.magADC.v[axis]));
// DEBUG 4-6: estimated magnetometer bias, increases above zero when calibration starts // DEBUG 4-6: estimated magnetometer bias, increases above zero when calibration starts
DEBUG_SET(DEBUG_MAG_CALIB, axis + 4, lrintf(compassBiasEstimator.b[axis])); DEBUG_SET(DEBUG_MAG_CALIB, axis + 4, lrintf(compassBiasEstimator.b[axis]));
} }
// DEBUG 3: absolute vector length of magADC, should stay constant independent of the orientation of the quad // DEBUG 3: absolute vector length of magADC, should stay constant independent of the orientation of the quad
DEBUG_SET(DEBUG_MAG_CALIB, 3, lrintf(sqrtf(sq(mag.magADC[X]) + sq(mag.magADC[Y]) + sq(mag.magADC[Z])))); DEBUG_SET(DEBUG_MAG_CALIB, 3, lrintf(vector3Norm(&mag.magADC)));
// DEBUG 7: adaptive forgetting factor lambda, only while analysing cal data // DEBUG 7: adaptive forgetting factor lambda, only while analysing cal data
// after the transient phase it should converge to 2000 // after the transient phase it should converge to 2000
// set dsiplayed lambda to zero unless calibrating, to indicate start and finish in Sensors tab // set dsiplayed lambda to zero unless calibrating, to indicate start and finish in Sensors tab
@ -578,13 +578,13 @@ void compassBiasEstimatorUpdate(compassBiasEstimator_t *cBE, const float lambda_
} }
// apply one estimation step of the compass bias estimator // apply one estimation step of the compass bias estimator
void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag) void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, vector3_t *mag)
{ {
// update phi // update phi
float phi[4]; float phi[4];
phi[0] = sq(mag[0]) + sq(mag[1]) + sq(mag[2]); phi[0] = sq(mag->x) + sq(mag->y) + sq(mag->z);
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
phi[i + 1] = mag[i]; phi[i + 1] = mag->v[i];
} }
// update e // update e

8
src/main/sensors/compass.h
View file

@ -22,9 +22,13 @@
#include "common/time.h" #include "common/time.h"
#include "common/sensor_alignment.h" #include "common/sensor_alignment.h"
#include "common/vector.h"
#include "drivers/io_types.h" #include "drivers/io_types.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#define TASK_COMPASS_RATE_HZ 40 // the base mag update rate; faster intervals will apply for higher ODR mags #define TASK_COMPASS_RATE_HZ 40 // the base mag update rate; faster intervals will apply for higher ODR mags
@ -45,7 +49,7 @@ typedef enum {
typedef struct mag_s { typedef struct mag_s {
bool isNewMagADCFlag; bool isNewMagADCFlag;
float magADC[XYZ_AXIS_COUNT]; vector3_t magADC;
} mag_t; } mag_t;
extern mag_t mag; extern mag_t mag;
@ -81,4 +85,4 @@ void compassStartCalibration(void);
bool compassIsCalibrationComplete(void); bool compassIsCalibrationComplete(void);
void compassBiasEstimatorInit(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0); void compassBiasEstimatorInit(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0);
void compassBiasEstimatorUpdate(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0); void compassBiasEstimatorUpdate(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0);
void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag); void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, vector3_t *mag);

56
src/main/sensors/gyro.c
View file

@ -390,19 +390,19 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor)
// move 16-bit gyro data into 32-bit variables to avoid overflows in calculations // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
#if defined(USE_GYRO_SLEW_LIMITER) #if defined(USE_GYRO_SLEW_LIMITER)
gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X]; gyroSensor->gyroDev.gyroADC.x = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC.y = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y];
gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z]; gyroSensor->gyroDev.gyroADC.z = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
#else #else
gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; gyroSensor->gyroDev.gyroADC.x = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC.y = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; gyroSensor->gyroDev.gyroADC.z = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
#endif #endif
if (gyroSensor->gyroDev.gyroAlign == ALIGN_CUSTOM) { if (gyroSensor->gyroDev.gyroAlign == ALIGN_CUSTOM) {
alignSensorViaMatrix(gyroSensor->gyroDev.gyroADC, &gyroSensor->gyroDev.rotationMatrix); alignSensorViaMatrix(&gyroSensor->gyroDev.gyroADC, &gyroSensor->gyroDev.rotationMatrix);
} else { } else {
alignSensorViaRotation(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); alignSensorViaRotation(&gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
} }
} else { } else {
performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold); performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
@ -415,27 +415,27 @@ FAST_CODE void gyroUpdate(void)
case GYRO_CONFIG_USE_GYRO_1: case GYRO_CONFIG_USE_GYRO_1:
gyroUpdateSensor(&gyro.gyroSensor1); gyroUpdateSensor(&gyro.gyroSensor1);
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1)) { if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1)) {
gyro.gyroADC[X] = gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale; gyro.gyroADC[X] = gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale;
gyro.gyroADC[Y] = gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale; gyro.gyroADC[Y] = gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale;
gyro.gyroADC[Z] = gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale; gyro.gyroADC[Z] = gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale;
} }
break; break;
#ifdef USE_MULTI_GYRO #ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2: case GYRO_CONFIG_USE_GYRO_2:
gyroUpdateSensor(&gyro.gyroSensor2); gyroUpdateSensor(&gyro.gyroSensor2);
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) { if (isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
gyro.gyroADC[X] = gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale; gyro.gyroADC[X] = gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale;
gyro.gyroADC[Y] = gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale; gyro.gyroADC[Y] = gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale;
gyro.gyroADC[Z] = gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale; gyro.gyroADC[Z] = gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale;
} }
break; break;
case GYRO_CONFIG_USE_GYRO_BOTH: case GYRO_CONFIG_USE_GYRO_BOTH:
gyroUpdateSensor(&gyro.gyroSensor1); gyroUpdateSensor(&gyro.gyroSensor1);
gyroUpdateSensor(&gyro.gyroSensor2); gyroUpdateSensor(&gyro.gyroSensor2);
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) { if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
gyro.gyroADC[X] = ((gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f; gyro.gyroADC[X] = ((gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
gyro.gyroADC[Y] = ((gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f; gyro.gyroADC[Y] = ((gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
gyro.gyroADC[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f; gyro.gyroADC[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
} }
break; break;
#endif #endif
@ -490,16 +490,16 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
case GYRO_CONFIG_USE_GYRO_1: case GYRO_CONFIG_USE_GYRO_1:
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale));
break; break;
#ifdef USE_MULTI_GYRO #ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2: case GYRO_CONFIG_USE_GYRO_2:
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale));
break; break;
case GYRO_CONFIG_USE_GYRO_BOTH: case GYRO_CONFIG_USE_GYRO_BOTH:
@ -507,13 +507,13 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf((gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale))); DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale)));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf((gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale))); DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale)));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf((gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale))); DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale)));
break; break;
#endif #endif
} }

2
src/main/sensors/gyro_init.c
View file

@ -307,7 +307,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
{ {
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
gyroSensor->gyroDev.gyroAlign = config->alignment; gyroSensor->gyroDev.gyroAlign = config->alignment;
buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix); buildRotationMatrixFromAngles(&gyroSensor->gyroDev.rotationMatrix, &config->customAlignment);
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag; gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf; gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;

2
src/main/telemetry/frsky_hub.c
View file

@ -181,7 +181,7 @@ static void frSkyHubWriteByteInternal(const char data)
static void sendAccel(void) static void sendAccel(void)
{ {
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC[i] * acc.dev.acc_1G_rec) * 1000)); frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC.v[i] * acc.dev.acc_1G_rec) * 1000));
} }
} }
#endif #endif

2
src/main/telemetry/ibus_shared.c
View file

@ -275,7 +275,7 @@ static uint16_t getMode(void)
#if defined(USE_ACC) #if defined(USE_ACC)
static int16_t getACC(uint8_t index) static int16_t getACC(uint8_t index)
{ {
return (int16_t)((acc.accADC[index] * acc.dev.acc_1G_rec) * 1000); return (int16_t)((acc.accADC.v[index] * acc.dev.acc_1G_rec) * 1000);
} }
#endif #endif

6
src/main/telemetry/jetiexbus.c
View file

@ -380,15 +380,15 @@ int32_t getSensorValue(uint8_t sensor)
#if defined(USE_ACC) #if defined(USE_ACC)
case EX_GFORCE_X: case EX_GFORCE_X:
return (int16_t)(((float)acc.accADC[0] / acc.dev.acc_1G) * 1000); return (int16_t)(((float)acc.accADC.x / acc.dev.acc_1G) * 1000);
break; break;
case EX_GFORCE_Y: case EX_GFORCE_Y:
return (int16_t)(((float)acc.accADC[1] / acc.dev.acc_1G) * 1000); return (int16_t)(((float)acc.accADC.y / acc.dev.acc_1G) * 1000);
break; break;
case EX_GFORCE_Z: case EX_GFORCE_Z:
return (int16_t)(((float)acc.accADC[2] / acc.dev.acc_1G) * 1000); return (int16_t)(((float)acc.accADC.z / acc.dev.acc_1G) * 1000);
break; break;
#endif #endif

6
src/main/telemetry/smartport.c
View file

@ -738,15 +738,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
*clearToSend = false; *clearToSend = false;
break; break;
case FSSP_DATAID_ACCX : case FSSP_DATAID_ACCX :
smartPortSendPackage(id, lrintf(100 * acc.accADC[X] * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis smartPortSendPackage(id, lrintf(100 * acc.accADC.x * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis
*clearToSend = false; *clearToSend = false;
break; break;
case FSSP_DATAID_ACCY : case FSSP_DATAID_ACCY :
smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] * acc.dev.acc_1G_rec)); smartPortSendPackage(id, lrintf(100 * acc.accADC.y * acc.dev.acc_1G_rec));
*clearToSend = false; *clearToSend = false;
break; break;
case FSSP_DATAID_ACCZ : case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] * acc.dev.acc_1G_rec)); smartPortSendPackage(id, lrintf(100 * acc.accADC.z * acc.dev.acc_1G_rec));
*clearToSend = false; *clearToSend = false;
break; break;
#endif #endif

34
src/test/Makefile
View file

@ -33,7 +33,8 @@ VPATH := $(VPATH):$(USER_DIR):$(TEST_DIR)
alignsensor_unittest_SRC := \ alignsensor_unittest_SRC := \
$(USER_DIR)/sensors/boardalignment.c \ $(USER_DIR)/sensors/boardalignment.c \
$(USER_DIR)/common/sensor_alignment.c \ $(USER_DIR)/common/sensor_alignment.c \
$(USER_DIR)/common/maths.c $(USER_DIR)/common/maths.c \
$(USER_DIR)/common/vector.c
arming_prevention_unittest_SRC := \ arming_prevention_unittest_SRC := \
$(USER_DIR)/common/bitarray.c \ $(USER_DIR)/common/bitarray.c \
@ -153,6 +154,7 @@ flight_failsafe_unittest_DEFINES := \
flight_imu_unittest_SRC := \ flight_imu_unittest_SRC := \
$(USER_DIR)/common/bitarray.c \ $(USER_DIR)/common/bitarray.c \
$(USER_DIR)/common/maths.c \ $(USER_DIR)/common/maths.c \
$(USER_DIR)/common/vector.c \
$(USER_DIR)/config/feature.c \ $(USER_DIR)/config/feature.c \
$(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/flight/position.c \ $(USER_DIR)/flight/position.c \
@ -184,7 +186,8 @@ ledstrip_unittest_DEFINES := \
maths_unittest_SRC := \ maths_unittest_SRC := \
$(USER_DIR)/common/maths.c $(USER_DIR)/common/maths.c \
$(USER_DIR)/common/vector.c
motor_output_unittest_SRC := \ motor_output_unittest_SRC := \
@ -198,12 +201,13 @@ osd_unittest_SRC := \
$(USER_DIR)/osd/osd.c \ $(USER_DIR)/osd/osd.c \
$(USER_DIR)/osd/osd_elements.c \ $(USER_DIR)/osd/osd_elements.c \
$(USER_DIR)/osd/osd_warnings.c \ $(USER_DIR)/osd/osd_warnings.c \
$(USER_DIR)/common/typeconversion.c \ $(USER_DIR)/common/filter.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/common/maths.c \ $(USER_DIR)/common/maths.c \
$(USER_DIR)/common/printf.c \ $(USER_DIR)/common/printf.c \
$(USER_DIR)/common/time.c \ $(USER_DIR)/common/time.c \
$(USER_DIR)/common/filter.c \ $(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/common/vector.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/fc/runtime_config.c $(USER_DIR)/fc/runtime_config.c
osd_unittest_DEFINES := \ osd_unittest_DEFINES := \
@ -216,21 +220,22 @@ link_quality_unittest_SRC := \
$(USER_DIR)/osd/osd.c \ $(USER_DIR)/osd/osd.c \
$(USER_DIR)/osd/osd_elements.c \ $(USER_DIR)/osd/osd_elements.c \
$(USER_DIR)/osd/osd_warnings.c \ $(USER_DIR)/osd/osd_warnings.c \
$(USER_DIR)/common/typeconversion.c \ $(USER_DIR)/common/bitarray.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/drivers/serial.c \
$(USER_DIR)/common/crc.c \ $(USER_DIR)/common/crc.c \
$(USER_DIR)/common/maths.c \ $(USER_DIR)/common/maths.c \
$(USER_DIR)/common/printf.c \ $(USER_DIR)/common/printf.c \
$(USER_DIR)/common/streambuf.c \ $(USER_DIR)/common/streambuf.c \
$(USER_DIR)/common/time.c \ $(USER_DIR)/common/time.c \
$(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/common/bitarray.c \ $(USER_DIR)/common/vector.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/drivers/serial.c \
$(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/rx/rx.c \ $(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/pg/pg.c \ $(USER_DIR)/pg/pg.c \
$(USER_DIR)/rx/crsf.c \ $(USER_DIR)/pg/rx.c \
$(USER_DIR)/pg/rx.c $(USER_DIR)/rx/rx.c \
$(USER_DIR)/rx/crsf.c
link_quality_unittest_DEFINES := \ link_quality_unittest_DEFINES := \
USE_OSD= \ USE_OSD= \
@ -304,11 +309,12 @@ sensor_gyro_unittest_SRC := \
$(USER_DIR)/sensors/gyro.c \ $(USER_DIR)/sensors/gyro.c \
$(USER_DIR)/sensors/gyro_init.c \ $(USER_DIR)/sensors/gyro_init.c \
$(USER_DIR)/sensors/boardalignment.c \ $(USER_DIR)/sensors/boardalignment.c \
$(USER_DIR)/common/filter.c \
$(USER_DIR)/common/crc.c \ $(USER_DIR)/common/crc.c \
$(USER_DIR)/common/filter.c \
$(USER_DIR)/common/maths.c \ $(USER_DIR)/common/maths.c \
$(USER_DIR)/common/streambuf.c \ $(USER_DIR)/common/streambuf.c \
$(USER_DIR)/common/sensor_alignment.c \ $(USER_DIR)/common/sensor_alignment.c \
$(USER_DIR)/common/vector.c \
$(USER_DIR)/drivers/accgyro/accgyro_virtual.c \ $(USER_DIR)/drivers/accgyro/accgyro_virtual.c \
$(USER_DIR)/drivers/accgyro/gyro_sync.c \ $(USER_DIR)/drivers/accgyro/gyro_sync.c \
$(USER_DIR)/pg/pg.c \ $(USER_DIR)/pg/pg.c \

168
src/test/unit/alignsensor_unittest.cc
View file

@ -24,6 +24,7 @@ extern "C" {
#include "common/sensor_alignment.h" #include "common/sensor_alignment.h"
#include "common/sensor_alignment_impl.h" #include "common/sensor_alignment_impl.h"
#include "common/utils.h" #include "common/utils.h"
#include "common/vector.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
@ -43,21 +44,18 @@ extern "C" {
#define DEG2RAD 0.01745329251 #define DEG2RAD 0.01745329251
static void rotateVector(int32_t mat[3][3], float vec[3], float *out) static void rotateVector(int32_t mat[3][3], vector3_t *vec, vector3_t *out)
{ {
float tmp[3]; vector3_t tmp;
for(int i=0; i<3; i++) { for(int i = 0; i < 3; i++) {
tmp[i] = 0; tmp.v[i] = 0;
for(int j=0; j<3; j++) { for(int j = 0; j < 3; j++) {
tmp[i] += mat[j][i] * vec[j]; tmp.v[i] += mat[j][i] * vec->v[j];
} }
} }
out[0]=tmp[0]; *out = tmp;
out[1]=tmp[1];
out[2]=tmp[2];
} }
//static void initXAxisRotation(int32_t mat[][3], int32_t angle) //static void initXAxisRotation(int32_t mat[][3], int32_t angle)
@ -101,70 +99,70 @@ static void initZAxisRotation(int32_t mat[][3], int32_t angle)
#define TOL 1e-5 // TOLERANCE #define TOL 1e-5 // TOLERANCE
static void alignSensorViaMatrixFromRotation(float *dest, sensor_align_e alignment) static void alignSensorViaMatrixFromRotation(vector3_t *dest, sensor_align_e alignment)
{ {
fp_rotationMatrix_t sensorRotationMatrix; matrix33_t sensorRotationMatrix;
sensorAlignment_t sensorAlignment; sensorAlignment_t sensorAlignment;
buildAlignmentFromStandardAlignment(&sensorAlignment, alignment); buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
buildRotationMatrixFromAlignment(&sensorAlignment, &sensorRotationMatrix); buildRotationMatrixFromAngles(&sensorRotationMatrix, &sensorAlignment);
alignSensorViaMatrix(dest, &sensorRotationMatrix); alignSensorViaMatrix(dest, &sensorRotationMatrix);
} }
static void testCW(sensor_align_e rotation, int32_t angle) static void testCW(sensor_align_e rotation, int32_t angle)
{ {
float src[XYZ_AXIS_COUNT]; vector3_t src;
float test[XYZ_AXIS_COUNT]; vector3_t test;
// unit vector along x-axis // unit vector along x-axis
src[X] = 1; src.x = 1;
src[Y] = 0; src.y = 0;
src[Z] = 0; src.z = 0;
int32_t matrix[3][3]; int32_t matrix[3][3];
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "X-Unit alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "X-Unit alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "X-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
// unit vector along y-axis // unit vector along y-axis
src[X] = 0; src.x = 0;
src[Y] = 1; src.y = 1;
src[Z] = 0; src.z = 0;
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "Y-Unit alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "Y-Unit alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "Y-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
// unit vector along z-axis // unit vector along z-axis
src[X] = 0; src.x = 0;
src[Y] = 0; src.y = 0;
src[Z] = 1; src.z = 1;
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "Z-Unit alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "Z-Unit alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "Z-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
// random vector to test // random vector to test
src[X] = rand() % 5; src.x = rand() % 5;
src[Y] = rand() % 5; src.y = rand() % 5;
src[Z] = rand() % 5; src.z = rand() % 5;
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "Random alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "Random alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "Random alignment does not match in Z-Axis. " << test.z << " " << src.z;
} }
/* /*
@ -173,73 +171,73 @@ static void testCW(sensor_align_e rotation, int32_t angle)
*/ */
static void testCWFlip(sensor_align_e rotation, int32_t angle) static void testCWFlip(sensor_align_e rotation, int32_t angle)
{ {
float src[XYZ_AXIS_COUNT]; vector3_t src;
float test[XYZ_AXIS_COUNT]; vector3_t test;
// unit vector along x-axis // unit vector along x-axis
src[X] = 1; src.x = 1;
src[Y] = 0; src.y = 0;
src[Z] = 0; src.z = 0;
int32_t matrix[3][3]; int32_t matrix[3][3];
initYAxisRotation(matrix, 180); initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); rotateVector(matrix, &test, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "X-Unit alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "X-Unit alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "X-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
// unit vector along y-axis // unit vector along y-axis
src[X] = 0; src.x = 0;
src[Y] = 1; src.y = 1;
src[Z] = 0; src.z = 0;
initYAxisRotation(matrix, 180); initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); rotateVector(matrix, &test, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "Y-Unit alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "Y-Unit alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "Y-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
// unit vector along z-axis // unit vector along z-axis
src[X] = 0; src.x = 0;
src[Y] = 0; src.y = 0;
src[Z] = 1; src.z = 1;
initYAxisRotation(matrix, 180); initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); rotateVector(matrix, &test, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "Z-Unit alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "Z-Unit alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "Z-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
// random vector to test // random vector to test
src[X] = rand() % 5; src.x = rand() % 5;
src[Y] = rand() % 5; src.y = rand() % 5;
src[Z] = rand() % 5; src.z = rand() % 5;
initYAxisRotation(matrix, 180); initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test); rotateVector(matrix, &src, &test);
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); rotateVector(matrix, &test, &test);
alignSensorViaMatrixFromRotation(src, rotation); alignSensorViaMatrixFromRotation(&src, rotation);
EXPECT_NEAR(test[X], src[X], TOL) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_NEAR(test.x, src.x, TOL) << "Random alignment does not match in X-Axis. " << test.x << " " << src.x;
EXPECT_NEAR(test[Y], src[Y], TOL) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_NEAR(test.y, src.y, TOL) << "Random alignment does not match in Y-Axis. " << test.y << " " << src.y;
EXPECT_NEAR(test[Z], src[Z], TOL) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; EXPECT_NEAR(test.z, src.z, TOL) << "Random alignment does not match in Z-Axis. " << test.z << " " << src.z;
} }

119
src/test/unit/flight_imu_unittest.cc
View file

@ -68,7 +68,7 @@ extern "C" {
float imuCalcMagErr(void); float imuCalcMagErr(void);
float imuCalcCourseErr(float courseOverGround); float imuCalcCourseErr(float courseOverGround);
extern quaternion q; extern quaternion q;
extern float rMat[3][3]; extern matrix33_t rMat;
extern bool attitudeIsEstablished; extern bool attitudeIsEstablished;
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
@ -86,8 +86,8 @@ extern "C" {
const float sqrt2over2 = sqrtf(2) / 2.0f; const float sqrt2over2 = sqrtf(2) / 2.0f;
void quaternion_from_axis_angle(quaternion* q, float angle, float x, float y, float z) { void quaternion_from_axis_angle(quaternion* q, float angle, float x, float y, float z) {
fpVector3_t a = {{x, y, z}}; vector3_t a = {{x, y, z}};
vectorNormalize(&a, &a); vector3Normalize(&a, &a);
q->w = cos(angle / 2); q->w = cos(angle / 2);
q->x = a.x * sin(angle / 2); q->x = a.x * sin(angle / 2);
q->y = a.y * sin(angle / 2); q->y = a.y * sin(angle / 2);
@ -106,15 +106,15 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
imuComputeRotationMatrix(); imuComputeRotationMatrix();
EXPECT_FLOAT_EQ(1.0f, rMat[0][0]); EXPECT_FLOAT_EQ(1.0f, rMat.m[0][0]);
EXPECT_FLOAT_EQ(0.0f, rMat[0][1]); EXPECT_FLOAT_EQ(0.0f, rMat.m[0][1]);
EXPECT_FLOAT_EQ(0.0f, rMat[0][2]); EXPECT_FLOAT_EQ(0.0f, rMat.m[0][2]);
EXPECT_FLOAT_EQ(0.0f, rMat[1][0]); EXPECT_FLOAT_EQ(0.0f, rMat.m[1][0]);
EXPECT_FLOAT_EQ(1.0f, rMat[1][1]); EXPECT_FLOAT_EQ(1.0f, rMat.m[1][1]);
EXPECT_FLOAT_EQ(0.0f, rMat[1][2]); EXPECT_FLOAT_EQ(0.0f, rMat.m[1][2]);
EXPECT_FLOAT_EQ(0.0f, rMat[2][0]); EXPECT_FLOAT_EQ(0.0f, rMat.m[2][0]);
EXPECT_FLOAT_EQ(0.0f, rMat[2][1]); EXPECT_FLOAT_EQ(0.0f, rMat.m[2][1]);
EXPECT_FLOAT_EQ(1.0f, rMat[2][2]); EXPECT_FLOAT_EQ(1.0f, rMat.m[2][2]);
// 90 degrees around Z axis // 90 degrees around Z axis
q.w = sqrt2over2; q.w = sqrt2over2;
@ -124,15 +124,15 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
imuComputeRotationMatrix(); imuComputeRotationMatrix();
EXPECT_NEAR(0.0f, rMat[0][0], TOL); EXPECT_NEAR(0.0f, rMat.m[0][0], TOL);
EXPECT_NEAR(-1.0f, rMat[0][1], TOL); EXPECT_NEAR(-1.0f, rMat.m[0][1], TOL);
EXPECT_NEAR(0.0f, rMat[0][2], TOL); EXPECT_NEAR(0.0f, rMat.m[0][2], TOL);
EXPECT_NEAR(1.0f, rMat[1][0], TOL); EXPECT_NEAR(1.0f, rMat.m[1][0], TOL);
EXPECT_NEAR(0.0f, rMat[1][1], TOL); EXPECT_NEAR(0.0f, rMat.m[1][1], TOL);
EXPECT_NEAR(0.0f, rMat[1][2], TOL); EXPECT_NEAR(0.0f, rMat.m[1][2], TOL);
EXPECT_NEAR(0.0f, rMat[2][0], TOL); EXPECT_NEAR(0.0f, rMat.m[2][0], TOL);
EXPECT_NEAR(0.0f, rMat[2][1], TOL); EXPECT_NEAR(0.0f, rMat.m[2][1], TOL);
EXPECT_NEAR(1.0f, rMat[2][2], TOL); EXPECT_NEAR(1.0f, rMat.m[2][2], TOL);
// 60 degrees around X axis // 60 degrees around X axis
q.w = 0.866f; q.w = 0.866f;
@ -142,21 +142,21 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
imuComputeRotationMatrix(); imuComputeRotationMatrix();
EXPECT_NEAR(1.0f, rMat[0][0], TOL); EXPECT_NEAR(1.0f, rMat.m[0][0], TOL);
EXPECT_NEAR(0.0f, rMat[0][1], TOL); EXPECT_NEAR(0.0f, rMat.m[0][1], TOL);
EXPECT_NEAR(0.0f, rMat[0][2], TOL); EXPECT_NEAR(0.0f, rMat.m[0][2], TOL);
EXPECT_NEAR(0.0f, rMat[1][0], TOL); EXPECT_NEAR(0.0f, rMat.m[1][0], TOL);
EXPECT_NEAR(0.5f, rMat[1][1], TOL); EXPECT_NEAR(0.5f, rMat.m[1][1], TOL);
EXPECT_NEAR(-0.866f, rMat[1][2], TOL); EXPECT_NEAR(-0.866f, rMat.m[1][2], TOL);
EXPECT_NEAR(0.0f, rMat[2][0], TOL); EXPECT_NEAR(0.0f, rMat.m[2][0], TOL);
EXPECT_NEAR(0.866f, rMat[2][1], TOL); EXPECT_NEAR(0.866f, rMat.m[2][1], TOL);
EXPECT_NEAR(0.5f, rMat[2][2], TOL); EXPECT_NEAR(0.5f, rMat.m[2][2], TOL);
} }
TEST(FlightImuTest, TestUpdateEulerAngles) TEST(FlightImuTest, TestUpdateEulerAngles)
{ {
// No rotation // No rotation
memset(rMat, 0.0, sizeof(float) * 9); memset(&rMat, 0.0, sizeof(float) * 9);
imuUpdateEulerAngles(); imuUpdateEulerAngles();
@ -165,11 +165,11 @@ TEST(FlightImuTest, TestUpdateEulerAngles)
EXPECT_EQ(0, attitude.values.yaw); EXPECT_EQ(0, attitude.values.yaw);
// 45 degree yaw // 45 degree yaw
memset(rMat, 0.0, sizeof(float) * 9); memset(&rMat, 0.0, sizeof(float) * 9);
rMat[0][0] = sqrt2over2; rMat.m[0][0] = sqrt2over2;
rMat[0][1] = sqrt2over2; rMat.m[0][1] = sqrt2over2;
rMat[1][0] = -sqrt2over2; rMat.m[1][0] = -sqrt2over2;
rMat[1][1] = sqrt2over2; rMat.m[1][1] = sqrt2over2;
imuUpdateEulerAngles(); imuUpdateEulerAngles();
@ -189,7 +189,7 @@ TEST(FlightImuTest, TestSmallAngle)
attitudeIsEstablished = true; attitudeIsEstablished = true;
// and // and
memset(rMat, 0.0, sizeof(float) * 9); memset(&rMat, 0.0, sizeof(float) * 9);
// when // when
imuComputeRotationMatrix(); imuComputeRotationMatrix();
@ -198,10 +198,10 @@ TEST(FlightImuTest, TestSmallAngle)
EXPECT_FALSE(isUpright()); EXPECT_FALSE(isUpright());
// given // given
rMat[0][0] = r1; rMat.m[0][0] = r1;
rMat[0][2] = r2; rMat.m[0][2] = r2;
rMat[2][0] = -r2; rMat.m[2][0] = -r2;
rMat[2][2] = r1; rMat.m[2][2] = r1;
// when // when
imuComputeRotationMatrix(); imuComputeRotationMatrix();
@ -210,7 +210,7 @@ TEST(FlightImuTest, TestSmallAngle)
EXPECT_FALSE(isUpright()); EXPECT_FALSE(isUpright());
// given // given
memset(rMat, 0.0, sizeof(float) * 9); memset(&rMat, 0.0, sizeof(float) * 9);
// when // when
imuComputeRotationMatrix(); imuComputeRotationMatrix();
@ -246,19 +246,19 @@ testing::AssertionResult DoubleNearWrapPredFormat(const char* expr1, const char*
class MahonyFixture : public ::testing::Test { class MahonyFixture : public ::testing::Test {
protected: protected:
fpVector3_t gyro; vector3_t gyro;
bool useAcc; bool useAcc;
fpVector3_t acc; vector3_t acc;
bool useMag; bool useMag;
fpVector3_t magEF; vector3_t magEF;
float cogGain; float cogGain;
float cogDeg; float cogDeg;
float dcmKp; float dcmKp;
float dt; float dt;
void SetUp() override { void SetUp() override {
vectorZero(&gyro); vector3Zero(&gyro);
useAcc = false; useAcc = false;
vectorZero(&acc); vector3Zero(&acc);
cogGain = 0.0; // no cog cogGain = 0.0; // no cog
cogDeg = 0.0; cogDeg = 0.0;
dcmKp = .25; // default dcm_kp dcmKp = .25; // default dcm_kp
@ -268,7 +268,7 @@ protected:
// level, poiting north // level, poiting north
setOrientationAA(0, {{1,0,0}}); // identity setOrientationAA(0, {{1,0,0}}); // identity
} }
virtual void setOrientationAA(float angleDeg, fpVector3_t axis) { virtual void setOrientationAA(float angleDeg, vector3_t axis) {
quaternion_from_axis_angle(&q, DEGREES_TO_RADIANS(angleDeg), axis.x, axis.y, axis.z); quaternion_from_axis_angle(&q, DEGREES_TO_RADIANS(angleDeg), axis.x, axis.y, axis.z);
imuComputeRotationMatrix(); imuComputeRotationMatrix();
} }
@ -278,19 +278,19 @@ protected:
if (angle < 0) angle += 360; if (angle < 0) angle += 360;
return angle; return angle;
} }
float angleDiffNorm(fpVector3_t *a, fpVector3_t* b, fpVector3_t weight = {{1,1,1}}) { float angleDiffNorm(vector3_t *a, vector3_t* b, vector3_t weight = {{1,1,1}}) {
fpVector3_t tmp; vector3_t tmp;
vectorScale(&tmp, b, -1); vector3Scale(&tmp, b, -1);
vectorAdd(&tmp, &tmp, a); vector3Add(&tmp, &tmp, a);
for (int i = 0; i < 3; i++) for (int i = 0; i < 3; i++)
tmp.v[i] *= weight.v[i]; tmp.v[i] *= weight.v[i];
for (int i = 0; i < 3; i++) for (int i = 0; i < 3; i++)
tmp.v[i] = std::remainder(tmp.v[i], 360.0); tmp.v[i] = std::remainder(tmp.v[i], 360.0);
return vectorNorm(&tmp); return vector3Norm(&tmp);
} }
// run Mahony for some time // run Mahony for some time
// return time it took to get within 1deg from target // return time it took to get within 1deg from target
float imuIntegrate(float runTime, fpVector3_t * target) { float imuIntegrate(float runTime, vector3_t * target) {
float alignTime = -1; float alignTime = -1;
for (float t = 0; t < runTime; t += dt) { for (float t = 0; t < runTime; t += dt) {
// if (fmod(t, 1) < dt) printf("MagBF=%.2f %.2f %.2f\n", magBF.x, magBF.y, magBF.z); // if (fmod(t, 1) < dt) printf("MagBF=%.2f %.2f %.2f\n", magBF.x, magBF.y, magBF.z);
@ -312,7 +312,7 @@ protected:
// if (fmod(t, 1) < dt) printf("%3.1fs - %3.1f %3.1f %3.1f\n", t, attitude.values.roll / 10.0f, attitude.values.pitch / 10.0f, attitude.values.yaw / 10.0f); // if (fmod(t, 1) < dt) printf("%3.1fs - %3.1f %3.1f %3.1f\n", t, attitude.values.roll / 10.0f, attitude.values.pitch / 10.0f, attitude.values.yaw / 10.0f);
// remember how long it took // remember how long it took
if (alignTime < 0) { if (alignTime < 0) {
fpVector3_t rpy = {{attitude.values.roll / 10.0f, attitude.values.pitch / 10.0f, attitude.values.yaw / 10.0f}}; vector3_t rpy = {{attitude.values.roll / 10.0f, attitude.values.pitch / 10.0f, attitude.values.yaw / 10.0f}};
float error = angleDiffNorm(&rpy, target); float error = angleDiffNorm(&rpy, target);
if (error < 1) if (error < 1)
alignTime = t; alignTime = t;
@ -331,7 +331,7 @@ TEST_P(YawTest, TestCogAlign)
cogDeg = GetParam(); cogDeg = GetParam();
const float rollDeg = 30; // 30deg pitch forward const float rollDeg = 30; // 30deg pitch forward
setOrientationAA(rollDeg, {{0, 1, 0}}); setOrientationAA(rollDeg, {{0, 1, 0}});
fpVector3_t expect = {{0, rollDeg, wrap(cogDeg)}}; vector3_t expect = {{0, rollDeg, wrap(cogDeg)}};
// integrate IMU. about 25s is enough in worst case // integrate IMU. about 25s is enough in worst case
float alignTime = imuIntegrate(80, &expect); float alignTime = imuIntegrate(80, &expect);
@ -354,12 +354,11 @@ TEST_P(YawTest, TestMagAlign)
quaternion_from_axis_angle(&q, -DEGREES_TO_RADIANS(initialAngle), 0, 0, 1); quaternion_from_axis_angle(&q, -DEGREES_TO_RADIANS(initialAngle), 0, 0, 1);
imuComputeRotationMatrix(); imuComputeRotationMatrix();
fpVector3_t expect = {{0, 0, 0}}; // expect zero yaw vector3_t expect = {{0, 0, 0}}; // expect zero yaw
fpVector3_t magBF = {{1, 0, .5}}; // use arbitrary Z component, point north vector3_t magBF = {{1, 0, .5}}; // use arbitrary Z component, point north
for (int i = 0; i < 3; i++) mag.magADC = magBF;
mag.magADC[i] = magBF.v[i];
useMag = true; useMag = true;
// integrate IMU. about 25s is enough in worst case // integrate IMU. about 25s is enough in worst case

3
src/test/unit/link_quality_unittest.cc
View file

@ -32,6 +32,7 @@ extern "C" {
#include "common/streambuf.h" #include "common/streambuf.h"
#include "common/time.h" #include "common/time.h"
#include "common/utils.h" #include "common/utils.h"
#include "common/vector.h"
#include "config/config.h" #include "config/config.h"
@ -67,7 +68,7 @@ extern "C" {
#include "sensors/battery.h" #include "sensors/battery.h"
attitudeEulerAngles_t attitude; attitudeEulerAngles_t attitude;
float rMat[3][3]; matrix33_t rMat;
pidProfile_t *currentPidProfile; pidProfile_t *currentPidProfile;
extern float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; extern float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];

9
src/test/unit/maths_unittest.cc
View file

@ -26,6 +26,7 @@
extern "C" { extern "C" {
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
} }
#include "unittest_macros.h" #include "unittest_macros.h"
@ -195,11 +196,11 @@ TEST(MathsUnittest, TestApplyDeadband)
EXPECT_EQ(applyDeadband(-11, 10), -1); EXPECT_EQ(applyDeadband(-11, 10), -1);
} }
void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b, float absTol) void expectVectorsAreEqual(vector3_t *a, vector3_t *b, float absTol)
{ {
EXPECT_NEAR(a->X, b->X, absTol); EXPECT_NEAR(a->x, b->x, absTol);
EXPECT_NEAR(a->Y, b->Y, absTol); EXPECT_NEAR(a->y, b->y, absTol);
EXPECT_NEAR(a->Z, b->Z, absTol); EXPECT_NEAR(a->z, b->z, absTol);
} }
#if defined(FAST_MATH) || defined(VERY_FAST_MATH) #if defined(FAST_MATH) || defined(VERY_FAST_MATH)

3
src/test/unit/osd_unittest.cc
View file

@ -29,6 +29,7 @@ extern "C" {
#include "blackbox/blackbox_io.h" #include "blackbox/blackbox_io.h"
#include "common/time.h" #include "common/time.h"
#include "common/vector.h"
#include "config/config.h" #include "config/config.h"
#include "config/feature.h" #include "config/feature.h"
@ -70,7 +71,7 @@ extern "C" {
uint16_t rssi; uint16_t rssi;
attitudeEulerAngles_t attitude; attitudeEulerAngles_t attitude;
float rMat[3][3]; matrix33_t rMat;
pidProfile_t *currentPidProfile; pidProfile_t *currentPidProfile;
int16_t debug[DEBUG16_VALUE_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT];

1
src/test/unit/sensor_gyro_unittest.cc
View file

@ -29,6 +29,7 @@ extern "C" {
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
#include "common/vector.h"
#include "drivers/accgyro/accgyro_virtual.h" #include "drivers/accgyro/accgyro_virtual.h"
#include "drivers/accgyro/accgyro_mpu.h" #include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"