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

Merge pull request #1620 from martinbudden/bf_align_sensors_opt

Browse source 
Changed alignSensors to have common src/dest
This commit is contained in:
J Blackman 2016-11-21 08:37:38 +11:00 • committed by GitHub
commit c52094c4af
6 changed files with 40 additions and 43 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -219,7 +219,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
}
}
alignSensors(accSmooth, accSmooth, accAlign);
alignSensors(accSmooth, accAlign);
if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims);

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

@ -62,13 +62,12 @@ static void alignBoard(int32_t *vec)
vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
}
void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation)
void alignSensors(int32_t *dest, uint8_t rotation)
{
const int32_t x = src[X];
const int32_t y = src[Y];
const int32_t z = src[Z];
const int32_t x = dest[X];
const int32_t y = dest[Y];
const int32_t z = dest[Z];
// note src and dest may point to the same address
switch (rotation) {
default:
case CW0_DEG:

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

@ -23,5 +23,5 @@ typedef struct boardAlignment_s {
int32_t yawDegrees;
} boardAlignment_t;
void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation);
void alignSensors(int32_t *dest, uint8_t rotation);
void initBoardAlignment(const boardAlignment_t *boardAlignment);

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

@ -62,7 +62,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
mag.read(magADCRaw);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; // int32_t copy to work with
alignSensors(magADC, magADC, magAlign);
alignSensors(magADC, magAlign);
if (STATE(CALIBRATE_MAG)) {
tCal = currentTime;

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

@ -170,7 +170,7 @@ void gyroUpdate(void)
gyroADC[Y] = gyroADCRaw[Y];
gyroADC[Z] = gyroADCRaw[Z];
alignSensors(gyroADC, gyroADC, gyroAlign);
alignSensors(gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);

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

@ -98,7 +98,6 @@ static void initZAxisRotation(int32_t mat[][3], int32_t angle)
static void testCW(sensor_align_e rotation, int32_t angle)
{
int32_t src[XYZ_AXIS_COUNT];
int32_t dest[XYZ_AXIS_COUNT];
int32_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis
@ -110,10 +109,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle);
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
alignSensors(src, rotation);
EXPECT_EQ(test[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along y-axis
src[X] = 0;
@ -121,10 +120,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
src[Z] = 0;
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
alignSensors(src, rotation);
EXPECT_EQ(test[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along z-axis
src[X] = 0;
@ -132,10 +131,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
src[Z] = 1;
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
alignSensors(src, rotation);
EXPECT_EQ(test[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// random vector to test
src[X] = rand() % 5;
@ -143,10 +142,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
src[Z] = rand() % 5;
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
alignSensors(src, rotation);
EXPECT_EQ(test[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
}
/*
@ -156,7 +155,6 @@ static void testCW(sensor_align_e rotation, int32_t angle)
static void testCWFlip(sensor_align_e rotation, int32_t angle)
{
int32_t src[XYZ_AXIS_COUNT];
int32_t dest[XYZ_AXIS_COUNT];
int32_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis
@ -170,11 +168,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
alignSensors(src, rotation);
EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
EXPECT_EQ(test[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along y-axis
src[X] = 0;
@ -186,11 +184,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
alignSensors(src, rotation);
EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
EXPECT_EQ(test[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along z-axis
src[X] = 0;
@ -202,11 +200,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
alignSensors(src, rotation);
EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
EXPECT_EQ(test[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// random vector to test
src[X] = rand() % 5;
@ -218,11 +216,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
alignSensors(src, rotation);
EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
EXPECT_EQ(test[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], src[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
}