Skip to content

Commit

Permalink
Vector library expansion (betaflight#12968)
Browse files Browse the repository at this point in the history
  • Loading branch information
KarateBrot authored Sep 15, 2024
1 parent fc4b7a0 commit f71170d
Show file tree
Hide file tree
Showing 37 changed files with 636 additions and 593 deletions.
2 changes: 2 additions & 0 deletions mk/source.mk
Original file line number Diff line number Diff line change
Expand Up @@ -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 \
Expand Down Expand Up @@ -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 \
Expand Down
4 changes: 2 additions & 2 deletions src/main/blackbox/blackbox.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

Expand Down
41 changes: 2 additions & 39 deletions src/main/common/maths.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
19 changes: 0 additions & 19 deletions src/main/common/maths.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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);
Expand All @@ -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);
Expand Down
21 changes: 11 additions & 10 deletions src/main/common/sensor_alignment.c
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
9 changes: 5 additions & 4 deletions src/main/common/sensor_alignment.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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 changes: 209 additions & 0 deletions src/main/common/vector.c
Original file line number Diff line number Diff line change
@@ -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;
}
Loading

0 comments on commit f71170d

Please sign in to comment.