mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
Vector library expansion (#12968)
This commit is contained in:
Jan Post
GitHub
parent
fc4b7a0008
commit
f71170db1b
37 changed files with 630 additions and 587 deletions
|
|
@ -72,6 +72,7 @@ COMMON_SRC = \
|
|||
common/time.c \
|
||||
common/typeconversion.c \
|
||||
common/uvarint.c \
|
||||
common/vector.c \
|
||||
config/config.c \
|
||||
config/config_eeprom.c \
|
||||
config/config_streamer.c \
|
||||
|
|
@ -408,6 +409,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
common/sdft.c \
|
||||
common/stopwatch.c \
|
||||
common/typeconversion.c \
|
||||
common/vector.c \
|
||||
drivers/accgyro/accgyro_mpu.c \
|
||||
drivers/accgyro/accgyro_mpu3050.c \
|
||||
drivers/accgyro/accgyro_spi_bmi160.c \
|
||||
|
|
|
|||
|
|
@ -1144,10 +1144,10 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i] * blackboxHighResolutionScale);
|
||||
blackboxCurrent->gyroUnfilt[i] = lrintf(gyro.gyroADC[i] * blackboxHighResolutionScale);
|
||||
#if defined(USE_ACC)
|
||||
blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
|
||||
blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]);
|
||||
#endif
|
||||
#ifdef USE_MAG
|
||||
blackboxCurrent->magADC[i] = lrintf(mag.magADC[i]);
|
||||
blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,8 @@
|
|||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "axis.h"
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "maths.h"
|
||||
|
||||
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
|
||||
|
|
@ -188,44 +189,6 @@ float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float des
|
|||
return (a / b) + destFrom;
|
||||
}
|
||||
|
||||
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
|
||||
{
|
||||
float cosx, sinx, cosy, siny, cosz, sinz;
|
||||
float coszcosx, sinzcosx, coszsinx, sinzsinx;
|
||||
|
||||
cosx = cos_approx(delta->angles.roll);
|
||||
sinx = sin_approx(delta->angles.roll);
|
||||
cosy = cos_approx(delta->angles.pitch);
|
||||
siny = sin_approx(delta->angles.pitch);
|
||||
cosz = cos_approx(delta->angles.yaw);
|
||||
sinz = sin_approx(delta->angles.yaw);
|
||||
|
||||
coszcosx = cosz * cosx;
|
||||
sinzcosx = sinz * cosx;
|
||||
coszsinx = sinx * cosz;
|
||||
sinzsinx = sinx * sinz;
|
||||
|
||||
rotation->m[0][X] = cosz * cosy;
|
||||
rotation->m[0][Y] = -cosy * sinz;
|
||||
rotation->m[0][Z] = siny;
|
||||
rotation->m[1][X] = sinzcosx + (coszsinx * siny);
|
||||
rotation->m[1][Y] = coszcosx - (sinzsinx * siny);
|
||||
rotation->m[1][Z] = -sinx * cosy;
|
||||
rotation->m[2][X] = (sinzsinx) - (coszcosx * siny);
|
||||
rotation->m[2][Y] = (coszsinx) + (sinzcosx * siny);
|
||||
rotation->m[2][Z] = cosy * cosx;
|
||||
}
|
||||
|
||||
void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix)
|
||||
{
|
||||
struct fp_vector *vDest = (struct fp_vector *)v;
|
||||
struct fp_vector vTmp = *vDest;
|
||||
|
||||
vDest->X = (rotationMatrix->m[0][X] * vTmp.X + rotationMatrix->m[1][X] * vTmp.Y + rotationMatrix->m[2][X] * vTmp.Z);
|
||||
vDest->Y = (rotationMatrix->m[0][Y] * vTmp.X + rotationMatrix->m[1][Y] * vTmp.Y + rotationMatrix->m[2][Y] * vTmp.Z);
|
||||
vDest->Z = (rotationMatrix->m[0][Z] * vTmp.X + rotationMatrix->m[1][Z] * vTmp.Y + rotationMatrix->m[2][Z] * vTmp.Z);
|
||||
}
|
||||
|
||||
// Quick median filter implementation
|
||||
// (c) N. Devillard - 1998
|
||||
// http://ndevilla.free.fr/median/median.pdf
|
||||
|
|
|
|||
|
|
@ -75,18 +75,6 @@ typedef struct stdev_s
|
|||
int m_n;
|
||||
} stdev_t;
|
||||
|
||||
// Floating point 3 vector.
|
||||
typedef struct fp_vector {
|
||||
float X;
|
||||
float Y;
|
||||
float Z;
|
||||
} t_fp_vector_def;
|
||||
|
||||
typedef union u_fp_vector {
|
||||
float A[3];
|
||||
t_fp_vector_def V;
|
||||
} t_fp_vector;
|
||||
|
||||
// Floating point Euler angles.
|
||||
// Be carefull, could be either of degrees or radians.
|
||||
typedef struct fp_angles {
|
||||
|
|
@ -100,10 +88,6 @@ typedef union {
|
|||
fp_angles_def angles;
|
||||
} fp_angles_t;
|
||||
|
||||
typedef struct fp_rotationMatrix_s {
|
||||
float m[3][3]; // matrix
|
||||
} fp_rotationMatrix_t;
|
||||
|
||||
int gcd(int num, int denom);
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband);
|
||||
float fapplyDeadband(float value, float deadband);
|
||||
|
|
@ -117,9 +101,6 @@ float degreesToRadians(int16_t degrees);
|
|||
int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
|
||||
float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo);
|
||||
|
||||
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation);
|
||||
void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix);
|
||||
|
||||
int32_t quickMedianFilter3(const int32_t * v);
|
||||
int32_t quickMedianFilter5(const int32_t * v);
|
||||
int32_t quickMedianFilter7(const int32_t * v);
|
||||
|
|
|
|||
|
|
@ -27,29 +27,30 @@
|
|||
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "common/sensor_alignment_impl.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
void buildRotationMatrixFromAlignment(const sensorAlignment_t* sensorAlignment, fp_rotationMatrix_t* rm)
|
||||
void buildRotationMatrixFromAngles(matrix33_t *rm, const sensorAlignment_t *rpy)
|
||||
{
|
||||
fp_angles_t rotationAngles;
|
||||
rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(sensorAlignment->roll);
|
||||
rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(sensorAlignment->pitch);
|
||||
rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(sensorAlignment->yaw);
|
||||
rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(rpy->roll);
|
||||
rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(rpy->pitch);
|
||||
rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(rpy->yaw);
|
||||
|
||||
buildRotationMatrix(&rotationAngles, rm);
|
||||
buildRotationMatrix(rm, &rotationAngles);
|
||||
}
|
||||
|
||||
|
||||
void buildAlignmentFromStandardAlignment(sensorAlignment_t* sensorAlignment, sensor_align_e alignment)
|
||||
void buildAlignmentFromStandardAlignment(sensorAlignment_t* rpy, sensor_align_e stdAlignment)
|
||||
{
|
||||
if (alignment == ALIGN_CUSTOM || alignment == ALIGN_DEFAULT) {
|
||||
if (stdAlignment == ALIGN_CUSTOM || stdAlignment == ALIGN_DEFAULT) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t alignmentBits = ALIGNMENT_TO_BITMASK(alignment);
|
||||
uint8_t alignmentBits = ALIGNMENT_TO_BITMASK(stdAlignment);
|
||||
|
||||
memset(sensorAlignment, 0x00, sizeof(sensorAlignment_t));
|
||||
memset(rpy, 0x00, sizeof(sensorAlignment_t));
|
||||
|
||||
for (int axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
|
||||
sensorAlignment->raw[axis] = DEGREES_TO_DECIDEGREES(90) * ALIGNMENT_AXIS_ROTATIONS(alignmentBits, axis);
|
||||
rpy->raw[axis] = DEGREES_TO_DECIDEGREES(90) * ALIGNMENT_AXIS_ROTATIONS(alignmentBits, axis);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,8 +20,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "axis.h"
|
||||
#include "maths.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
typedef enum {
|
||||
ALIGN_DEFAULT = 0, // driver-provided alignment
|
||||
|
|
@ -69,5 +70,5 @@ typedef union sensorAlignment_u {
|
|||
#define CUSTOM_ALIGN_CW180_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 180)
|
||||
#define CUSTOM_ALIGN_CW270_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 270)
|
||||
|
||||
void buildRotationMatrixFromAlignment(const sensorAlignment_t* alignment, fp_rotationMatrix_t* rm);
|
||||
void buildAlignmentFromStandardAlignment(sensorAlignment_t* sensorAlignment, sensor_align_e alignment);
|
||||
void buildRotationMatrixFromAngles(matrix33_t *rm, const sensorAlignment_t *rpy);
|
||||
void buildAlignmentFromStandardAlignment(sensorAlignment_t *rpy, sensor_align_e stdAlignment);
|
||||
|
|
|
|||
209
src/main/common/vector.c
Normal file
209
src/main/common/vector.c
Normal 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;
|
||||
}
|
||||
|
|
@ -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
|
||||
* 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 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.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
* You should have received a copy of the GNU General Public
|
||||
* License along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
|
@ -24,176 +25,52 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
|
||||
typedef union {
|
||||
typedef union vector2_u {
|
||||
float v[2];
|
||||
struct {
|
||||
float x,y;
|
||||
float x, y;
|
||||
};
|
||||
} fpVector2_t;
|
||||
} vector2_t;
|
||||
|
||||
|
||||
typedef union {
|
||||
typedef union vector3_u {
|
||||
float v[3];
|
||||
struct {
|
||||
float x, y, z;
|
||||
};
|
||||
} fpVector3_t;
|
||||
} vector3_t;
|
||||
|
||||
typedef struct {
|
||||
typedef struct matrix33_s {
|
||||
float m[3][3];
|
||||
} fpMat33_t;
|
||||
} matrix33_t;
|
||||
|
||||
static inline fpVector3_t * vectorZero(fpVector3_t *v)
|
||||
{
|
||||
v->x = 0.0f;
|
||||
v->y = 0.0f;
|
||||
v->z = 0.0f;
|
||||
return v;
|
||||
}
|
||||
bool vector2Equal(const vector2_t *a, const vector2_t *b);
|
||||
void vector2Zero(vector2_t *v);
|
||||
void vector2Add(vector2_t *result, const vector2_t *a, const vector2_t *b);
|
||||
void vector2Scale(vector2_t *result, const vector2_t *v, const float k);
|
||||
float vector2Dot(const vector2_t *a, const vector2_t *b);
|
||||
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)
|
||||
{
|
||||
return sq(v->x) + sq(v->y) + sq(v->z);
|
||||
}
|
||||
bool vector3Equal(const vector3_t *a, const vector3_t *b);
|
||||
void vector3Zero(vector3_t *v);
|
||||
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)
|
||||
{
|
||||
return sqrtf(vectorNormSquared(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);
|
||||
|
||||
static inline fpVector3_t * vectorCrossProduct(fpVector3_t *result, const fpVector3_t *a, const fpVector3_t *b)
|
||||
{
|
||||
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 buildRotationMatrix(matrix33_t *result, const fp_angles_t *rpy);
|
||||
void applyRotationMatrix(vector3_t *v, const matrix33_t *rotationMatrix);
|
||||
|
||||
void yawToRotationMatrixZ(matrix33_t *result, const float yaw);
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "common/time.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro_mpu.h"
|
||||
#include "drivers/bus.h"
|
||||
|
|
@ -103,7 +104,7 @@ typedef struct gyroDev_s {
|
|||
extDevice_t dev;
|
||||
float scale; // scalefactor
|
||||
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];
|
||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; // raw data from sensor
|
||||
int16_t temperature;
|
||||
|
|
@ -125,7 +126,7 @@ typedef struct gyroDev_s {
|
|||
ioTag_t mpuIntExtiTag;
|
||||
uint8_t gyroHasOverflowProtection;
|
||||
gyroHardware_e gyroHardware;
|
||||
fp_rotationMatrix_t rotationMatrix;
|
||||
matrix33_t rotationMatrix;
|
||||
uint16_t gyroSampleRateHz;
|
||||
uint16_t accSampleRateHz;
|
||||
uint8_t accDataReg;
|
||||
|
|
@ -148,7 +149,7 @@ typedef struct accDev_s {
|
|||
bool acc_high_fsr;
|
||||
char revisionCode; // a revision code for the sensor, if known
|
||||
uint8_t filler[2];
|
||||
fp_rotationMatrix_t rotationMatrix;
|
||||
matrix33_t rotationMatrix;
|
||||
} accDev_t;
|
||||
|
||||
static inline void accDevLock(accDev_t *acc)
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
|
@ -33,7 +34,7 @@ typedef struct magDev_s {
|
|||
extDevice_t dev;
|
||||
busDevice_t bus; // For MPU slave bus instance
|
||||
sensor_align_e magAlignment;
|
||||
fp_rotationMatrix_t rotationMatrix;
|
||||
matrix33_t rotationMatrix;
|
||||
ioTag_t magIntExtiTag;
|
||||
int16_t magGain[3];
|
||||
uint16_t magOdrHz;
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "config/feature.h"
|
||||
|
|
@ -781,20 +782,20 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
|
|||
}
|
||||
}
|
||||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
static t_fp_vector_def rcCommandBuff;
|
||||
static vector3_t rcCommandBuff;
|
||||
|
||||
rcCommandBuff.X = rcCommand[ROLL];
|
||||
rcCommandBuff.Y = rcCommand[PITCH];
|
||||
rcCommandBuff.x = rcCommand[ROLL];
|
||||
rcCommandBuff.y = rcCommand[PITCH];
|
||||
if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
|
||||
rcCommandBuff.Z = rcCommand[YAW];
|
||||
rcCommandBuff.z = rcCommand[YAW];
|
||||
} else {
|
||||
rcCommandBuff.Z = 0;
|
||||
rcCommandBuff.z = 0;
|
||||
}
|
||||
imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
|
||||
rcCommand[ROLL] = rcCommandBuff.X;
|
||||
rcCommand[PITCH] = rcCommandBuff.Y;
|
||||
rcCommand[ROLL] = rcCommandBuff.x;
|
||||
rcCommand[PITCH] = rcCommandBuff.y;
|
||||
if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
|
||||
rcCommand[YAW] = rcCommandBuff.Z;
|
||||
rcCommand[YAW] = rcCommandBuff.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,9 +30,6 @@
|
|||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
|
|
@ -96,8 +93,8 @@ static float smallAngleCosZ = 0;
|
|||
|
||||
static imuRuntimeConfig_t imuRuntimeConfig;
|
||||
|
||||
float rMat[3][3];
|
||||
static fpVector2_t north_ef;
|
||||
matrix33_t rMat;
|
||||
static vector2_t north_ef;
|
||||
|
||||
#if defined(USE_ACC)
|
||||
STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
|
||||
|
|
@ -147,21 +144,21 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
|
|||
{
|
||||
imuQuaternionComputeProducts(&q, &qP);
|
||||
|
||||
rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
|
||||
rMat[0][1] = 2.0f * (qP.xy + -qP.wz);
|
||||
rMat[0][2] = 2.0f * (qP.xz - -qP.wy);
|
||||
rMat.m[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
|
||||
rMat.m[0][1] = 2.0f * (qP.xy + -qP.wz);
|
||||
rMat.m[0][2] = 2.0f * (qP.xz - -qP.wy);
|
||||
|
||||
rMat[1][0] = 2.0f * (qP.xy - -qP.wz);
|
||||
rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
|
||||
rMat[1][2] = 2.0f * (qP.yz + -qP.wx);
|
||||
rMat.m[1][0] = 2.0f * (qP.xy - -qP.wz);
|
||||
rMat.m[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
|
||||
rMat.m[1][2] = 2.0f * (qP.yz + -qP.wx);
|
||||
|
||||
rMat[2][0] = 2.0f * (qP.xz + -qP.wy);
|
||||
rMat[2][1] = 2.0f * (qP.yz - -qP.wx);
|
||||
rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy;
|
||||
rMat.m[2][0] = 2.0f * (qP.xz + -qP.wy);
|
||||
rMat.m[2][1] = 2.0f * (qP.yz - -qP.wx);
|
||||
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)
|
||||
rMat[1][0] = -2.0f * (qP.xy - -qP.wz);
|
||||
rMat[2][0] = -2.0f * (qP.xz + -qP.wy);
|
||||
rMat.m[1][0] = -2.0f * (qP.xy - -qP.wz);
|
||||
rMat.m[2][0] = -2.0f * (qP.xz + -qP.wy);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -230,9 +227,9 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt,
|
|||
|
||||
// Add error from magnetometer and Cog
|
||||
// just rotate input value to body frame
|
||||
ex += rMat[Z][X] * (headingErrCog + headingErrMag);
|
||||
ey += rMat[Z][Y] * (headingErrCog + headingErrMag);
|
||||
ez += rMat[Z][Z] * (headingErrCog + headingErrMag);
|
||||
ex += rMat.m[Z][X] * (headingErrCog + headingErrMag);
|
||||
ey += rMat.m[Z][Y] * (headingErrCog + headingErrMag);
|
||||
ez += rMat.m[Z][Z] * (headingErrCog + headingErrMag);
|
||||
|
||||
DEBUG_SET(DEBUG_ATTITUDE, 3, (headingErrCog * 100));
|
||||
DEBUG_SET(DEBUG_ATTITUDE, 7, lrintf(dcmKpGain * 100.0f));
|
||||
|
|
@ -248,9 +245,9 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt,
|
|||
az *= recipAccNorm;
|
||||
|
||||
// Error is sum of cross product between estimated direction and measured direction of gravity
|
||||
ex += (ay * rMat[2][2] - az * rMat[2][1]);
|
||||
ey += (az * rMat[2][0] - ax * rMat[2][2]);
|
||||
ez += (ax * rMat[2][1] - ay * rMat[2][0]);
|
||||
ex += (ay * rMat.m[2][2] - az * rMat.m[2][1]);
|
||||
ey += (az * rMat.m[2][0] - ax * rMat.m[2][2]);
|
||||
ez += (ax * rMat.m[2][1] - ay * rMat.m[2][0]);
|
||||
}
|
||||
|
||||
// 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.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf)));
|
||||
} else {
|
||||
attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
|
||||
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
|
||||
attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
|
||||
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.m[2][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) {
|
||||
|
|
@ -455,10 +452,10 @@ STATIC_UNIT_TESTED float imuCalcCourseErr(float 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)
|
||||
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
|
||||
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
|
||||
|
||||
// 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
|
||||
// Only re-calculate magYaw when there is a new Mag data reading, to avoid spikes
|
||||
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
|
||||
|
||||
vector3_t mag_bf = mag.magADC;
|
||||
vector3_t mag_ef;
|
||||
matrixVectorMul(&mag_ef, &rMat, &mag_bf); // BF->EF true north
|
||||
|
||||
fpMat33_t rMatZTrans;
|
||||
yawToRotationMatrixZ(&rMatZTrans, -atan2_approx(rMat[1][0], rMat[0][0]));
|
||||
fpVector3_t mag_ef_yawed;
|
||||
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
|
||||
|
||||
// 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)));
|
||||
if (magYaw < 0) {
|
||||
|
|
@ -518,19 +518,19 @@ static void imuDebug_GPS_RESCUE_HEADING(void)
|
|||
STATIC_UNIT_TESTED float imuCalcMagErr(void)
|
||||
{
|
||||
// Use measured magnetic field vector
|
||||
fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
|
||||
float magNormSquared = vectorNormSquared(&mag_bf);
|
||||
vector3_t mag_bf = mag.magADC;
|
||||
float magNormSquared = vector3NormSq(&mag_bf);
|
||||
|
||||
if (magNormSquared > 0.01f) {
|
||||
// project magnetometer reading into Earth frame
|
||||
fpVector3_t mag_ef;
|
||||
matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF true north
|
||||
vector3_t mag_ef;
|
||||
matrixVectorMul(&mag_ef, &rMat, &mag_bf); // BF->EF true north
|
||||
// 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).
|
||||
// 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)
|
||||
// north_ef - reference mag field vector heading due North in EF (2D ground plane projection).
|
||||
// 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
|
||||
imuMahonyAHRSupdate(dt,
|
||||
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,
|
||||
imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
|
||||
|
||||
|
|
@ -731,9 +731,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
|
|||
mixerSetThrottleAngleCorrection(throttleAngleCorrection);
|
||||
|
||||
} else {
|
||||
acc.accADC[X] = 0;
|
||||
acc.accADC[Y] = 0;
|
||||
acc.accADC[Z] = 0;
|
||||
acc.accADC.x = 0;
|
||||
acc.accADC.y = 0;
|
||||
acc.accADC.z = 0;
|
||||
schedulerIgnoreTaskStateTime();
|
||||
}
|
||||
|
||||
|
|
@ -746,12 +746,12 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
|
|||
// Positive angle - nose down, negative angle - nose up.
|
||||
float getSinPitchAngle(void)
|
||||
{
|
||||
return -rMat[2][0];
|
||||
return -rMat.m[2][0];
|
||||
}
|
||||
|
||||
float getCosTiltAngle(void)
|
||||
{
|
||||
return rMat[2][2];
|
||||
return rMat.m[2][2];
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v)
|
||||
void imuQuaternionHeadfreeTransformVectorEarthToBody(vector3_t *v)
|
||||
{
|
||||
quaternionProducts buffer;
|
||||
|
||||
imuQuaternionMultiplication(&offset, &q, &headfree);
|
||||
imuQuaternionComputeProducts(&headfree, &buffer);
|
||||
|
||||
const float x = (buffer.ww + buffer.xx - buffer.yy - buffer.zz) * v->X + 2.0f * (buffer.xy + buffer.wz) * v->Y + 2.0f * (buffer.xz - buffer.wy) * v->Z;
|
||||
const float y = 2.0f * (buffer.xy - buffer.wz) * v->X + (buffer.ww - buffer.xx + buffer.yy - buffer.zz) * v->Y + 2.0f * (buffer.yz + buffer.wx) * v->Z;
|
||||
const float z = 2.0f * (buffer.xz + buffer.wy) * v->X + 2.0f * (buffer.yz - buffer.wx) * v->Y + (buffer.ww - buffer.xx - buffer.yy + buffer.zz) * v->Z;
|
||||
const float x = (buffer.ww + buffer.xx - buffer.yy - buffer.zz) * v->x + 2.0f * (buffer.xy + buffer.wz) * v->y + 2.0f * (buffer.xz - buffer.wy) * v->z;
|
||||
const float y = 2.0f * (buffer.xy - buffer.wz) * v->x + (buffer.ww - buffer.xx + buffer.yy - buffer.zz) * v->y + 2.0f * (buffer.yz + buffer.wx) * v->z;
|
||||
const float z = 2.0f * (buffer.xz + buffer.wy) * v->x + 2.0f * (buffer.yz - buffer.wx) * v->y + (buffer.ww - buffer.xx - buffer.yy + buffer.zz) * v->z;
|
||||
|
||||
v->X = x;
|
||||
v->Y = y;
|
||||
v->Z = z;
|
||||
v->x = x;
|
||||
v->y = y;
|
||||
v->z = z;
|
||||
}
|
||||
|
||||
bool isUpright(void)
|
||||
|
|
|
|||
|
|
@ -23,6 +23,8 @@
|
|||
#include "common/axis.h"
|
||||
#include "common/time.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
||||
// Exported symbols
|
||||
|
|
@ -50,7 +52,7 @@ typedef union {
|
|||
#define EULER_INITIALIZE { { 0, 0, 0 } }
|
||||
|
||||
extern attitudeEulerAngles_t attitude;
|
||||
extern float rMat[3][3];
|
||||
extern matrix33_t rMat;
|
||||
|
||||
typedef struct imuConfig_s {
|
||||
uint16_t imu_dcm_kp; // DCM filter proportional gain ( x 10000)
|
||||
|
|
@ -85,6 +87,6 @@ void imuSetHasNewData(uint32_t dt);
|
|||
#endif
|
||||
|
||||
bool imuQuaternionHeadfreeOffsetSet(void);
|
||||
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v);
|
||||
void imuQuaternionHeadfreeTransformVectorEarthToBody(vector3_t *v);
|
||||
bool shouldInitializeGPSHeading(void);
|
||||
bool isUpright(void);
|
||||
|
|
|
|||
|
|
@ -487,7 +487,7 @@ static void showSensorsPage(void)
|
|||
|
||||
#if defined(USE_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();
|
||||
i2c_OLED_set_line(dev, rowIndex++);
|
||||
i2c_OLED_send_string(dev, lineBuffer);
|
||||
|
|
@ -503,7 +503,7 @@ static void showSensorsPage(void)
|
|||
|
||||
#ifdef USE_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();
|
||||
i2c_OLED_set_line(dev, rowIndex++);
|
||||
i2c_OLED_send_string(dev, lineBuffer);
|
||||
|
|
|
|||
|
|
@ -1136,7 +1136,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
|
|||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#if defined(USE_ACC)
|
||||
sbufWriteU16(dst, lrintf(acc.accADC[i]));
|
||||
sbufWriteU16(dst, lrintf(acc.accADC.v[i]));
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
|
@ -1146,7 +1146,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
|
|||
}
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#if defined(USE_MAG)
|
||||
sbufWriteU16(dst, lrintf(mag.magADC[i]));
|
||||
sbufWriteU16(dst, lrintf(mag.magADC.v[i]));
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1316,11 +1316,7 @@ void osdProcessStats3(void)
|
|||
if (sensors(SENSOR_ACC)
|
||||
&& (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
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
const float a = acc.accADC[axis];
|
||||
osdGForce += a * a;
|
||||
}
|
||||
osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
|
||||
osdGForce = vector3Norm(&acc.accADC) * acc.dev.acc_1G_rec;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -761,7 +761,7 @@ static void osdElementArtificialHorizon(osdElementParms_t *element)
|
|||
static void osdElementUpDownReference(osdElementParms_t *element)
|
||||
{
|
||||
// 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) {
|
||||
float thetaB; // pitch from body frame to zenith/nadir
|
||||
|
|
|
|||
|
|
@ -43,9 +43,9 @@ FAST_DATA_ZERO_INIT acc_t acc; // acc access functions
|
|||
|
||||
static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
|
||||
{
|
||||
acc.accADC[X] -= accelerationTrims->raw[X];
|
||||
acc.accADC[Y] -= accelerationTrims->raw[Y];
|
||||
acc.accADC[Z] -= accelerationTrims->raw[Z];
|
||||
acc.accADC.x -= accelerationTrims->raw[X];
|
||||
acc.accADC.y -= accelerationTrims->raw[Y];
|
||||
acc.accADC.z -= accelerationTrims->raw[Z];
|
||||
}
|
||||
|
||||
void accUpdate(timeUs_t currentTimeUs)
|
||||
|
|
@ -60,13 +60,13 @@ void accUpdate(timeUs_t currentTimeUs)
|
|||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
const int16_t val = acc.dev.ADCRaw[axis];
|
||||
acc.accADC[axis] = val;
|
||||
acc.accADC.v[axis] = val;
|
||||
}
|
||||
|
||||
if (acc.dev.accAlign == ALIGN_CUSTOM) {
|
||||
alignSensorViaMatrix(acc.accADC, &acc.dev.rotationMatrix);
|
||||
alignSensorViaMatrix(&acc.accADC, &acc.dev.rotationMatrix);
|
||||
} else {
|
||||
alignSensorViaRotation(acc.accADC, acc.dev.accAlign);
|
||||
alignSensorViaRotation(&acc.accADC, acc.dev.accAlign);
|
||||
}
|
||||
|
||||
if (!accIsCalibrationComplete()) {
|
||||
|
|
@ -81,9 +81,9 @@ void accUpdate(timeUs_t currentTimeUs)
|
|||
|
||||
float accAdcSquaredSum = 0.0f;
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
const float val = acc.accADC[axis];
|
||||
acc.accADC[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val;
|
||||
accAdcSquaredSum += sq(acc.accADC[axis]);
|
||||
const float val = acc.accADC.v[axis];
|
||||
acc.accADC.v[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val;
|
||||
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.accDelta = (acc.accMagnitude - previousAccMagnitude) * acc.sampleRateHz;
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ typedef enum {
|
|||
typedef struct acc_s {
|
||||
accDev_t dev;
|
||||
uint16_t sampleRateHz;
|
||||
float accADC[XYZ_AXIS_COUNT];
|
||||
vector3_t accADC;
|
||||
bool isAccelUpdatedAtLeastOnce;
|
||||
float accMagnitude;
|
||||
float accDelta;
|
||||
|
|
|
|||
|
|
@ -407,7 +407,7 @@ bool accInit(uint16_t accSampleRateHz)
|
|||
}
|
||||
#endif
|
||||
acc.dev.accAlign = alignment;
|
||||
buildRotationMatrixFromAlignment(customAlignment, &acc.dev.rotationMatrix);
|
||||
buildRotationMatrixFromAngles(&acc.dev.rotationMatrix, customAlignment);
|
||||
|
||||
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
|
||||
return false;
|
||||
|
|
@ -453,10 +453,10 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
|
|||
}
|
||||
|
||||
// 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
|
||||
acc.accADC[axis] = 0;
|
||||
acc.accADC.v[axis] = 0;
|
||||
accelerationRuntime.accelerationTrims->raw[axis] = 0;
|
||||
}
|
||||
|
||||
|
|
@ -495,9 +495,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
|
|||
if (InflightcalibratingA == 50)
|
||||
b[axis] = 0;
|
||||
// Sum up 50 readings
|
||||
b[axis] += acc.accADC[axis];
|
||||
b[axis] += acc.accADC.v[axis];
|
||||
// Clear global variables for next reading
|
||||
acc.accADC[axis] = 0;
|
||||
acc.accADC.v[axis] = 0;
|
||||
accelerationRuntime.accelerationTrims->raw[axis] = 0;
|
||||
}
|
||||
// all values are measured
|
||||
|
|
|
|||
|
|
@ -26,8 +26,6 @@
|
|||
#include "platform.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
|
@ -38,7 +36,7 @@
|
|||
#include "boardalignment.h"
|
||||
|
||||
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);
|
||||
|
||||
|
|
@ -76,70 +74,68 @@ void initBoardAlignment(const boardAlignment_t *boardAlignment)
|
|||
rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
|
||||
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) {
|
||||
alignBoard(dest);
|
||||
}
|
||||
}
|
||||
|
||||
void alignSensorViaRotation(float *dest, uint8_t rotation)
|
||||
void alignSensorViaRotation(vector3_t *dest, sensor_align_e rotation)
|
||||
{
|
||||
const float x = dest[X];
|
||||
const float y = dest[Y];
|
||||
const float z = dest[Z];
|
||||
const vector3_t tmp = *dest;
|
||||
|
||||
switch (rotation) {
|
||||
default:
|
||||
case CW0_DEG:
|
||||
dest[X] = x;
|
||||
dest[Y] = y;
|
||||
dest[Z] = z;
|
||||
dest->x = tmp.x;
|
||||
dest->y = tmp.y;
|
||||
dest->z = tmp.z;
|
||||
break;
|
||||
case CW90_DEG:
|
||||
dest[X] = y;
|
||||
dest[Y] = -x;
|
||||
dest[Z] = z;
|
||||
dest->x = tmp.y;
|
||||
dest->y = -tmp.x;
|
||||
dest->z = tmp.z;
|
||||
break;
|
||||
case CW180_DEG:
|
||||
dest[X] = -x;
|
||||
dest[Y] = -y;
|
||||
dest[Z] = z;
|
||||
dest->x = -tmp.x;
|
||||
dest->y = -tmp.y;
|
||||
dest->z = tmp.z;
|
||||
break;
|
||||
case CW270_DEG:
|
||||
dest[X] = -y;
|
||||
dest[Y] = x;
|
||||
dest[Z] = z;
|
||||
dest->x = -tmp.y;
|
||||
dest->y = tmp.x;
|
||||
dest->z = tmp.z;
|
||||
break;
|
||||
case CW0_DEG_FLIP:
|
||||
dest[X] = -x;
|
||||
dest[Y] = y;
|
||||
dest[Z] = -z;
|
||||
dest->x = -tmp.x;
|
||||
dest->y = tmp.y;
|
||||
dest->z = -tmp.z;
|
||||
break;
|
||||
case CW90_DEG_FLIP:
|
||||
dest[X] = y;
|
||||
dest[Y] = x;
|
||||
dest[Z] = -z;
|
||||
dest->x = tmp.y;
|
||||
dest->y = tmp.x;
|
||||
dest->z = -tmp.z;
|
||||
break;
|
||||
case CW180_DEG_FLIP:
|
||||
dest[X] = x;
|
||||
dest[Y] = -y;
|
||||
dest[Z] = -z;
|
||||
dest->x = tmp.x;
|
||||
dest->y = -tmp.y;
|
||||
dest->z = -tmp.z;
|
||||
break;
|
||||
case CW270_DEG_FLIP:
|
||||
dest[X] = -y;
|
||||
dest[Y] = -x;
|
||||
dest[Z] = -z;
|
||||
dest->x = -tmp.y;
|
||||
dest->y = -tmp.x;
|
||||
dest->z = -tmp.z;
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
||||
|
|
@ -33,7 +34,7 @@ typedef struct boardAlignment_s {
|
|||
|
||||
PG_DECLARE(boardAlignment_t, boardAlignment);
|
||||
|
||||
void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* rotationMatrix);
|
||||
void alignSensorViaRotation(float *dest, uint8_t rotation);
|
||||
void alignSensorViaMatrix(vector3_t *dest, matrix33_t *rotationMatrix);
|
||||
void alignSensorViaRotation(vector3_t *dest, sensor_align_e rotation);
|
||||
|
||||
void initBoardAlignment(const boardAlignment_t *boardAlignment);
|
||||
|
|
|
|||
|
|
@ -386,7 +386,7 @@ bool compassInit(void)
|
|||
magDev.magAlignment = compassConfig()->mag_alignment;
|
||||
}
|
||||
|
||||
buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix);
|
||||
buildRotationMatrixFromAngles(&magDev.rotationMatrix, &compassConfig()->mag_customAlignment);
|
||||
|
||||
compassBiasEstimatorInit(&compassBiasEstimator, LAMBDA_MIN, P0);
|
||||
|
||||
|
|
@ -405,7 +405,7 @@ bool compassInit(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)
|
||||
|
|
@ -449,15 +449,15 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
|
|||
|
||||
// if we get here, we have new data to parse
|
||||
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
|
||||
mag.isNewMagADCFlag = true;
|
||||
|
||||
if (magDev.magAlignment == ALIGN_CUSTOM) {
|
||||
alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix);
|
||||
alignSensorViaMatrix(&mag.magADC, &magDev.rotationMatrix);
|
||||
} else {
|
||||
alignSensorViaRotation(mag.magADC, magDev.magAlignment);
|
||||
alignSensorViaRotation(&mag.magADC, magDev.magAlignment);
|
||||
}
|
||||
|
||||
// get stored cal/bias values
|
||||
|
|
@ -486,7 +486,7 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
|
|||
if (didMovementStart) {
|
||||
// LED will flash at task rate while calibrating, looks like 'ON' all the time.
|
||||
LED0_ON;
|
||||
compassBiasEstimatorApply(&compassBiasEstimator, mag.magADC);
|
||||
compassBiasEstimatorApply(&compassBiasEstimator, &mag.magADC);
|
||||
}
|
||||
} else {
|
||||
// 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
|
||||
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) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
// DEBUG 0-2: magADC[X], magADC[Y], magADC[Z]
|
||||
DEBUG_SET(DEBUG_MAG_CALIB, axis, lrintf(mag.magADC[axis]));
|
||||
// DEBUG 0-2: magADC.x, magADC.y, magADC.z
|
||||
DEBUG_SET(DEBUG_MAG_CALIB, axis, lrintf(mag.magADC.v[axis]));
|
||||
// DEBUG 4-6: estimated magnetometer bias, increases above zero when calibration starts
|
||||
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_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
|
||||
// after the transient phase it should converge to 2000
|
||||
// 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
|
||||
void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag)
|
||||
void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, vector3_t *mag)
|
||||
{
|
||||
// update phi
|
||||
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++) {
|
||||
phi[i + 1] = mag[i];
|
||||
phi[i + 1] = mag->v[i];
|
||||
}
|
||||
|
||||
// update e
|
||||
|
|
|
|||
|
|
@ -22,9 +22,13 @@
|
|||
|
||||
#include "common/time.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "drivers/io_types.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#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 {
|
||||
bool isNewMagADCFlag;
|
||||
float magADC[XYZ_AXIS_COUNT];
|
||||
vector3_t magADC;
|
||||
} mag_t;
|
||||
|
||||
extern mag_t mag;
|
||||
|
|
@ -81,4 +85,4 @@ void compassStartCalibration(void);
|
|||
bool compassIsCalibrationComplete(void);
|
||||
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 compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag);
|
||||
void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, vector3_t *mag);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
#if defined(USE_GYRO_SLEW_LIMITER)
|
||||
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[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
|
||||
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.z = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
|
||||
#else
|
||||
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[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
|
||||
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.z = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
|
||||
#endif
|
||||
|
||||
if (gyroSensor->gyroDev.gyroAlign == ALIGN_CUSTOM) {
|
||||
alignSensorViaMatrix(gyroSensor->gyroDev.gyroADC, &gyroSensor->gyroDev.rotationMatrix);
|
||||
alignSensorViaMatrix(&gyroSensor->gyroDev.gyroADC, &gyroSensor->gyroDev.rotationMatrix);
|
||||
} else {
|
||||
alignSensorViaRotation(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
|
||||
alignSensorViaRotation(&gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
|
||||
}
|
||||
} else {
|
||||
performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
|
||||
|
|
@ -415,27 +415,27 @@ FAST_CODE void gyroUpdate(void)
|
|||
case GYRO_CONFIG_USE_GYRO_1:
|
||||
gyroUpdateSensor(&gyro.gyroSensor1);
|
||||
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1)) {
|
||||
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[Z] = gyro.gyroSensor1.gyroDev.gyroADC[Z] * 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[Z] = gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale;
|
||||
}
|
||||
break;
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroUpdateSensor(&gyro.gyroSensor2);
|
||||
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
|
||||
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[Z] = gyro.gyroSensor2.gyroDev.gyroADC[Z] * 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[Z] = gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale;
|
||||
}
|
||||
break;
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
gyroUpdateSensor(&gyro.gyroSensor1);
|
||||
gyroUpdateSensor(&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[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[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[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -490,16 +490,16 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
|
|||
case GYRO_CONFIG_USE_GYRO_1:
|
||||
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_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, 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));
|
||||
break;
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
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_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, 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));
|
||||
break;
|
||||
|
||||
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, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
|
||||
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, 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, 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, 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_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, 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_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, 2, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale)));
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -307,7 +307,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
|
|||
{
|
||||
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
|
||||
gyroSensor->gyroDev.gyroAlign = config->alignment;
|
||||
buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
|
||||
buildRotationMatrixFromAngles(&gyroSensor->gyroDev.rotationMatrix, &config->customAlignment);
|
||||
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
|
||||
gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
|
||||
|
||||
|
|
|
|||
|
|
@ -181,7 +181,7 @@ static void frSkyHubWriteByteInternal(const char data)
|
|||
static void sendAccel(void)
|
||||
{
|
||||
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
|
||||
|
|
|
|||
|
|
@ -275,7 +275,7 @@ static uint16_t getMode(void)
|
|||
#if defined(USE_ACC)
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -380,15 +380,15 @@ int32_t getSensorValue(uint8_t sensor)
|
|||
|
||||
#if defined(USE_ACC)
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -738,15 +738,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
|
|||
*clearToSend = false;
|
||||
break;
|
||||
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;
|
||||
break;
|
||||
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;
|
||||
break;
|
||||
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;
|
||||
break;
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -33,7 +33,8 @@ VPATH := $(VPATH):$(USER_DIR):$(TEST_DIR)
|
|||
alignsensor_unittest_SRC := \
|
||||
$(USER_DIR)/sensors/boardalignment.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 := \
|
||||
$(USER_DIR)/common/bitarray.c \
|
||||
|
|
@ -153,6 +154,7 @@ flight_failsafe_unittest_DEFINES := \
|
|||
flight_imu_unittest_SRC := \
|
||||
$(USER_DIR)/common/bitarray.c \
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/vector.c \
|
||||
$(USER_DIR)/config/feature.c \
|
||||
$(USER_DIR)/fc/rc_modes.c \
|
||||
$(USER_DIR)/flight/position.c \
|
||||
|
|
@ -184,7 +186,8 @@ ledstrip_unittest_DEFINES := \
|
|||
|
||||
|
||||
maths_unittest_SRC := \
|
||||
$(USER_DIR)/common/maths.c
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/vector.c
|
||||
|
||||
|
||||
motor_output_unittest_SRC := \
|
||||
|
|
@ -198,12 +201,13 @@ osd_unittest_SRC := \
|
|||
$(USER_DIR)/osd/osd.c \
|
||||
$(USER_DIR)/osd/osd_elements.c \
|
||||
$(USER_DIR)/osd/osd_warnings.c \
|
||||
$(USER_DIR)/common/typeconversion.c \
|
||||
$(USER_DIR)/drivers/display.c \
|
||||
$(USER_DIR)/common/filter.c \
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/printf.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
|
||||
|
||||
osd_unittest_DEFINES := \
|
||||
|
|
@ -216,21 +220,22 @@ link_quality_unittest_SRC := \
|
|||
$(USER_DIR)/osd/osd.c \
|
||||
$(USER_DIR)/osd/osd_elements.c \
|
||||
$(USER_DIR)/osd/osd_warnings.c \
|
||||
$(USER_DIR)/common/typeconversion.c \
|
||||
$(USER_DIR)/drivers/display.c \
|
||||
$(USER_DIR)/drivers/serial.c \
|
||||
$(USER_DIR)/common/bitarray.c \
|
||||
$(USER_DIR)/common/crc.c \
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/printf.c \
|
||||
$(USER_DIR)/common/streambuf.c \
|
||||
$(USER_DIR)/common/time.c \
|
||||
$(USER_DIR)/fc/runtime_config.c \
|
||||
$(USER_DIR)/common/bitarray.c \
|
||||
$(USER_DIR)/common/typeconversion.c \
|
||||
$(USER_DIR)/common/vector.c \
|
||||
$(USER_DIR)/drivers/display.c \
|
||||
$(USER_DIR)/drivers/serial.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)/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 := \
|
||||
USE_OSD= \
|
||||
|
|
@ -304,11 +309,12 @@ sensor_gyro_unittest_SRC := \
|
|||
$(USER_DIR)/sensors/gyro.c \
|
||||
$(USER_DIR)/sensors/gyro_init.c \
|
||||
$(USER_DIR)/sensors/boardalignment.c \
|
||||
$(USER_DIR)/common/filter.c \
|
||||
$(USER_DIR)/common/crc.c \
|
||||
$(USER_DIR)/common/filter.c \
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/streambuf.c \
|
||||
$(USER_DIR)/common/sensor_alignment.c \
|
||||
$(USER_DIR)/common/vector.c \
|
||||
$(USER_DIR)/drivers/accgyro/accgyro_virtual.c \
|
||||
$(USER_DIR)/drivers/accgyro/gyro_sync.c \
|
||||
$(USER_DIR)/pg/pg.c \
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ extern "C" {
|
|||
#include "common/sensor_alignment.h"
|
||||
#include "common/sensor_alignment_impl.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/vector.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
|
@ -43,21 +44,18 @@ extern "C" {
|
|||
|
||||
#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++) {
|
||||
tmp[i] = 0;
|
||||
for(int j=0; j<3; j++) {
|
||||
tmp[i] += mat[j][i] * vec[j];
|
||||
for(int i = 0; i < 3; i++) {
|
||||
tmp.v[i] = 0;
|
||||
for(int j = 0; j < 3; j++) {
|
||||
tmp.v[i] += mat[j][i] * vec->v[j];
|
||||
}
|
||||
}
|
||||
|
||||
out[0]=tmp[0];
|
||||
out[1]=tmp[1];
|
||||
out[2]=tmp[2];
|
||||
|
||||
*out = tmp;
|
||||
}
|
||||
|
||||
//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
|
||||
|
||||
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;
|
||||
|
||||
buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
|
||||
|
||||
buildRotationMatrixFromAlignment(&sensorAlignment, &sensorRotationMatrix);
|
||||
buildRotationMatrixFromAngles(&sensorRotationMatrix, &sensorAlignment);
|
||||
|
||||
alignSensorViaMatrix(dest, &sensorRotationMatrix);
|
||||
}
|
||||
|
||||
static void testCW(sensor_align_e rotation, int32_t angle)
|
||||
{
|
||||
float src[XYZ_AXIS_COUNT];
|
||||
float test[XYZ_AXIS_COUNT];
|
||||
vector3_t src;
|
||||
vector3_t test;
|
||||
|
||||
// unit vector along x-axis
|
||||
src[X] = 1;
|
||||
src[Y] = 0;
|
||||
src[Z] = 0;
|
||||
src.x = 1;
|
||||
src.y = 0;
|
||||
src.z = 0;
|
||||
|
||||
int32_t matrix[3][3];
|
||||
initZAxisRotation(matrix, angle);
|
||||
rotateVector(matrix, src, test);
|
||||
rotateVector(matrix, &src, &test);
|
||||
|
||||
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[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];
|
||||
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.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;
|
||||
|
||||
// unit vector along y-axis
|
||||
src[X] = 0;
|
||||
src[Y] = 1;
|
||||
src[Z] = 0;
|
||||
src.x = 0;
|
||||
src.y = 1;
|
||||
src.z = 0;
|
||||
|
||||
rotateVector(matrix, src, test);
|
||||
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[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];
|
||||
rotateVector(matrix, &src, &test);
|
||||
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.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;
|
||||
|
||||
// unit vector along z-axis
|
||||
src[X] = 0;
|
||||
src[Y] = 0;
|
||||
src[Z] = 1;
|
||||
src.x = 0;
|
||||
src.y = 0;
|
||||
src.z = 1;
|
||||
|
||||
rotateVector(matrix, src, test);
|
||||
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[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];
|
||||
rotateVector(matrix, &src, &test);
|
||||
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.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;
|
||||
|
||||
// random vector to test
|
||||
src[X] = rand() % 5;
|
||||
src[Y] = rand() % 5;
|
||||
src[Z] = rand() % 5;
|
||||
src.x = rand() % 5;
|
||||
src.y = rand() % 5;
|
||||
src.z = rand() % 5;
|
||||
|
||||
rotateVector(matrix, src, test);
|
||||
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[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];
|
||||
rotateVector(matrix, &src, &test);
|
||||
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.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;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -173,73 +171,73 @@ static void testCW(sensor_align_e rotation, int32_t angle)
|
|||
*/
|
||||
static void testCWFlip(sensor_align_e rotation, int32_t angle)
|
||||
{
|
||||
float src[XYZ_AXIS_COUNT];
|
||||
float test[XYZ_AXIS_COUNT];
|
||||
vector3_t src;
|
||||
vector3_t test;
|
||||
|
||||
// unit vector along x-axis
|
||||
src[X] = 1;
|
||||
src[Y] = 0;
|
||||
src[Z] = 0;
|
||||
src.x = 1;
|
||||
src.y = 0;
|
||||
src.z = 0;
|
||||
|
||||
int32_t matrix[3][3];
|
||||
initYAxisRotation(matrix, 180);
|
||||
rotateVector(matrix, src, test);
|
||||
rotateVector(matrix, &src, &test);
|
||||
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[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.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.z, src.z, TOL) << "X-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
|
||||
|
||||
// unit vector along y-axis
|
||||
src[X] = 0;
|
||||
src[Y] = 1;
|
||||
src[Z] = 0;
|
||||
src.x = 0;
|
||||
src.y = 1;
|
||||
src.z = 0;
|
||||
|
||||
initYAxisRotation(matrix, 180);
|
||||
rotateVector(matrix, src, test);
|
||||
rotateVector(matrix, &src, &test);
|
||||
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[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.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.z, src.z, TOL) << "Y-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
|
||||
|
||||
// unit vector along z-axis
|
||||
src[X] = 0;
|
||||
src[Y] = 0;
|
||||
src[Z] = 1;
|
||||
src.x = 0;
|
||||
src.y = 0;
|
||||
src.z = 1;
|
||||
|
||||
initYAxisRotation(matrix, 180);
|
||||
rotateVector(matrix, src, test);
|
||||
rotateVector(matrix, &src, &test);
|
||||
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[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.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.z, src.z, TOL) << "Z-Unit alignment does not match in Z-Axis. " << test.z << " " << src.z;
|
||||
|
||||
// random vector to test
|
||||
src[X] = rand() % 5;
|
||||
src[Y] = rand() % 5;
|
||||
src[Z] = rand() % 5;
|
||||
src.x = rand() % 5;
|
||||
src.y = rand() % 5;
|
||||
src.z = rand() % 5;
|
||||
|
||||
initYAxisRotation(matrix, 180);
|
||||
rotateVector(matrix, src, test);
|
||||
rotateVector(matrix, &src, &test);
|
||||
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[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.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.z, src.z, TOL) << "Random alignment does not match in Z-Axis. " << test.z << " " << src.z;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ extern "C" {
|
|||
float imuCalcMagErr(void);
|
||||
float imuCalcCourseErr(float courseOverGround);
|
||||
extern quaternion q;
|
||||
extern float rMat[3][3];
|
||||
extern matrix33_t rMat;
|
||||
extern bool attitudeIsEstablished;
|
||||
|
||||
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
|
||||
|
|
@ -86,8 +86,8 @@ extern "C" {
|
|||
const float sqrt2over2 = sqrtf(2) / 2.0f;
|
||||
|
||||
void quaternion_from_axis_angle(quaternion* q, float angle, float x, float y, float z) {
|
||||
fpVector3_t a = {{x, y, z}};
|
||||
vectorNormalize(&a, &a);
|
||||
vector3_t a = {{x, y, z}};
|
||||
vector3Normalize(&a, &a);
|
||||
q->w = cos(angle / 2);
|
||||
q->x = a.x * sin(angle / 2);
|
||||
q->y = a.y * sin(angle / 2);
|
||||
|
|
@ -106,15 +106,15 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
|
|||
|
||||
imuComputeRotationMatrix();
|
||||
|
||||
EXPECT_FLOAT_EQ(1.0f, rMat[0][0]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat[0][1]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat[0][2]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat[1][0]);
|
||||
EXPECT_FLOAT_EQ(1.0f, rMat[1][1]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat[1][2]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat[2][0]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat[2][1]);
|
||||
EXPECT_FLOAT_EQ(1.0f, rMat[2][2]);
|
||||
EXPECT_FLOAT_EQ(1.0f, rMat.m[0][0]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat.m[0][1]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat.m[0][2]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat.m[1][0]);
|
||||
EXPECT_FLOAT_EQ(1.0f, rMat.m[1][1]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat.m[1][2]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat.m[2][0]);
|
||||
EXPECT_FLOAT_EQ(0.0f, rMat.m[2][1]);
|
||||
EXPECT_FLOAT_EQ(1.0f, rMat.m[2][2]);
|
||||
|
||||
// 90 degrees around Z axis
|
||||
q.w = sqrt2over2;
|
||||
|
|
@ -124,15 +124,15 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
|
|||
|
||||
imuComputeRotationMatrix();
|
||||
|
||||
EXPECT_NEAR(0.0f, rMat[0][0], TOL);
|
||||
EXPECT_NEAR(-1.0f, rMat[0][1], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[0][2], TOL);
|
||||
EXPECT_NEAR(1.0f, rMat[1][0], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[1][1], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[1][2], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[2][0], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[2][1], TOL);
|
||||
EXPECT_NEAR(1.0f, rMat[2][2], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[0][0], TOL);
|
||||
EXPECT_NEAR(-1.0f, rMat.m[0][1], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[0][2], TOL);
|
||||
EXPECT_NEAR(1.0f, rMat.m[1][0], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[1][1], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[1][2], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[2][0], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[2][1], TOL);
|
||||
EXPECT_NEAR(1.0f, rMat.m[2][2], TOL);
|
||||
|
||||
// 60 degrees around X axis
|
||||
q.w = 0.866f;
|
||||
|
|
@ -142,21 +142,21 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
|
|||
|
||||
imuComputeRotationMatrix();
|
||||
|
||||
EXPECT_NEAR(1.0f, rMat[0][0], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[0][1], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[0][2], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[1][0], TOL);
|
||||
EXPECT_NEAR(0.5f, rMat[1][1], TOL);
|
||||
EXPECT_NEAR(-0.866f, rMat[1][2], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat[2][0], TOL);
|
||||
EXPECT_NEAR(0.866f, rMat[2][1], TOL);
|
||||
EXPECT_NEAR(0.5f, rMat[2][2], TOL);
|
||||
EXPECT_NEAR(1.0f, rMat.m[0][0], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[0][1], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[0][2], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[1][0], TOL);
|
||||
EXPECT_NEAR(0.5f, rMat.m[1][1], TOL);
|
||||
EXPECT_NEAR(-0.866f, rMat.m[1][2], TOL);
|
||||
EXPECT_NEAR(0.0f, rMat.m[2][0], TOL);
|
||||
EXPECT_NEAR(0.866f, rMat.m[2][1], TOL);
|
||||
EXPECT_NEAR(0.5f, rMat.m[2][2], TOL);
|
||||
}
|
||||
|
||||
TEST(FlightImuTest, TestUpdateEulerAngles)
|
||||
{
|
||||
// No rotation
|
||||
memset(rMat, 0.0, sizeof(float) * 9);
|
||||
memset(&rMat, 0.0, sizeof(float) * 9);
|
||||
|
||||
imuUpdateEulerAngles();
|
||||
|
||||
|
|
@ -165,11 +165,11 @@ TEST(FlightImuTest, TestUpdateEulerAngles)
|
|||
EXPECT_EQ(0, attitude.values.yaw);
|
||||
|
||||
// 45 degree yaw
|
||||
memset(rMat, 0.0, sizeof(float) * 9);
|
||||
rMat[0][0] = sqrt2over2;
|
||||
rMat[0][1] = sqrt2over2;
|
||||
rMat[1][0] = -sqrt2over2;
|
||||
rMat[1][1] = sqrt2over2;
|
||||
memset(&rMat, 0.0, sizeof(float) * 9);
|
||||
rMat.m[0][0] = sqrt2over2;
|
||||
rMat.m[0][1] = sqrt2over2;
|
||||
rMat.m[1][0] = -sqrt2over2;
|
||||
rMat.m[1][1] = sqrt2over2;
|
||||
|
||||
imuUpdateEulerAngles();
|
||||
|
||||
|
|
@ -189,7 +189,7 @@ TEST(FlightImuTest, TestSmallAngle)
|
|||
attitudeIsEstablished = true;
|
||||
|
||||
// and
|
||||
memset(rMat, 0.0, sizeof(float) * 9);
|
||||
memset(&rMat, 0.0, sizeof(float) * 9);
|
||||
|
||||
// when
|
||||
imuComputeRotationMatrix();
|
||||
|
|
@ -198,10 +198,10 @@ TEST(FlightImuTest, TestSmallAngle)
|
|||
EXPECT_FALSE(isUpright());
|
||||
|
||||
// given
|
||||
rMat[0][0] = r1;
|
||||
rMat[0][2] = r2;
|
||||
rMat[2][0] = -r2;
|
||||
rMat[2][2] = r1;
|
||||
rMat.m[0][0] = r1;
|
||||
rMat.m[0][2] = r2;
|
||||
rMat.m[2][0] = -r2;
|
||||
rMat.m[2][2] = r1;
|
||||
|
||||
// when
|
||||
imuComputeRotationMatrix();
|
||||
|
|
@ -210,7 +210,7 @@ TEST(FlightImuTest, TestSmallAngle)
|
|||
EXPECT_FALSE(isUpright());
|
||||
|
||||
// given
|
||||
memset(rMat, 0.0, sizeof(float) * 9);
|
||||
memset(&rMat, 0.0, sizeof(float) * 9);
|
||||
|
||||
// when
|
||||
imuComputeRotationMatrix();
|
||||
|
|
@ -246,19 +246,19 @@ testing::AssertionResult DoubleNearWrapPredFormat(const char* expr1, const char*
|
|||
|
||||
class MahonyFixture : public ::testing::Test {
|
||||
protected:
|
||||
fpVector3_t gyro;
|
||||
vector3_t gyro;
|
||||
bool useAcc;
|
||||
fpVector3_t acc;
|
||||
vector3_t acc;
|
||||
bool useMag;
|
||||
fpVector3_t magEF;
|
||||
vector3_t magEF;
|
||||
float cogGain;
|
||||
float cogDeg;
|
||||
float dcmKp;
|
||||
float dt;
|
||||
void SetUp() override {
|
||||
vectorZero(&gyro);
|
||||
vector3Zero(&gyro);
|
||||
useAcc = false;
|
||||
vectorZero(&acc);
|
||||
vector3Zero(&acc);
|
||||
cogGain = 0.0; // no cog
|
||||
cogDeg = 0.0;
|
||||
dcmKp = .25; // default dcm_kp
|
||||
|
|
@ -268,7 +268,7 @@ protected:
|
|||
// level, poiting north
|
||||
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);
|
||||
imuComputeRotationMatrix();
|
||||
}
|
||||
|
|
@ -278,19 +278,19 @@ protected:
|
|||
if (angle < 0) angle += 360;
|
||||
return angle;
|
||||
}
|
||||
float angleDiffNorm(fpVector3_t *a, fpVector3_t* b, fpVector3_t weight = {{1,1,1}}) {
|
||||
fpVector3_t tmp;
|
||||
vectorScale(&tmp, b, -1);
|
||||
vectorAdd(&tmp, &tmp, a);
|
||||
float angleDiffNorm(vector3_t *a, vector3_t* b, vector3_t weight = {{1,1,1}}) {
|
||||
vector3_t tmp;
|
||||
vector3Scale(&tmp, b, -1);
|
||||
vector3Add(&tmp, &tmp, a);
|
||||
for (int i = 0; i < 3; i++)
|
||||
tmp.v[i] *= weight.v[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
tmp.v[i] = std::remainder(tmp.v[i], 360.0);
|
||||
return vectorNorm(&tmp);
|
||||
return vector3Norm(&tmp);
|
||||
}
|
||||
// run Mahony for some time
|
||||
// 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;
|
||||
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);
|
||||
|
|
@ -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);
|
||||
// remember how long it took
|
||||
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);
|
||||
if (error < 1)
|
||||
alignTime = t;
|
||||
|
|
@ -331,7 +331,7 @@ TEST_P(YawTest, TestCogAlign)
|
|||
cogDeg = GetParam();
|
||||
const float rollDeg = 30; // 30deg pitch forward
|
||||
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
|
||||
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);
|
||||
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[i] = magBF.v[i];
|
||||
mag.magADC = magBF;
|
||||
|
||||
useMag = true;
|
||||
// integrate IMU. about 25s is enough in worst case
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ extern "C" {
|
|||
#include "common/streambuf.h"
|
||||
#include "common/time.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "config/config.h"
|
||||
|
||||
|
|
@ -67,7 +68,7 @@ extern "C" {
|
|||
#include "sensors/battery.h"
|
||||
|
||||
attitudeEulerAngles_t attitude;
|
||||
float rMat[3][3];
|
||||
matrix33_t rMat;
|
||||
|
||||
pidProfile_t *currentPidProfile;
|
||||
extern float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
extern "C" {
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
|
@ -195,11 +196,11 @@ TEST(MathsUnittest, TestApplyDeadband)
|
|||
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->Y, b->Y, absTol);
|
||||
EXPECT_NEAR(a->Z, b->Z, absTol);
|
||||
EXPECT_NEAR(a->x, b->x, absTol);
|
||||
EXPECT_NEAR(a->y, b->y, absTol);
|
||||
EXPECT_NEAR(a->z, b->z, absTol);
|
||||
}
|
||||
|
||||
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@ extern "C" {
|
|||
#include "blackbox/blackbox_io.h"
|
||||
|
||||
#include "common/time.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "config/feature.h"
|
||||
|
|
@ -70,7 +71,7 @@ extern "C" {
|
|||
|
||||
uint16_t rssi;
|
||||
attitudeEulerAngles_t attitude;
|
||||
float rMat[3][3];
|
||||
matrix33_t rMat;
|
||||
|
||||
pidProfile_t *currentPidProfile;
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@ extern "C" {
|
|||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/vector.h"
|
||||
#include "drivers/accgyro/accgyro_virtual.h"
|
||||
#include "drivers/accgyro/accgyro_mpu.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue