Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +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()) { if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims); 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); 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 x = dest[X];
const int32_t y = src[Y]; const int32_t y = dest[Y];
const int32_t z = src[Z]; const int32_t z = dest[Z];
// note src and dest may point to the same address
switch (rotation) { switch (rotation) {
default: default:
case CW0_DEG: case CW0_DEG:

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

@ -23,5 +23,5 @@ typedef struct boardAlignment_s {
int32_t yawDegrees; int32_t yawDegrees;
} boardAlignment_t; } 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); 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); mag.read(magADCRaw);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; // int32_t copy to work with 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)) { if (STATE(CALIBRATE_MAG)) {
tCal = currentTime; tCal = currentTime;

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

@ -170,7 +170,7 @@ void gyroUpdate(void)
gyroADC[Y] = gyroADCRaw[Y]; gyroADC[Y] = gyroADCRaw[Y];
gyroADC[Z] = gyroADCRaw[Z]; gyroADC[Z] = gyroADCRaw[Z];
alignSensors(gyroADC, gyroADC, gyroAlign); alignSensors(gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) { if (!isGyroCalibrationComplete()) {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); 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) static void testCW(sensor_align_e rotation, int32_t angle)
{ {
int32_t src[XYZ_AXIS_COUNT]; int32_t src[XYZ_AXIS_COUNT];
int32_t dest[XYZ_AXIS_COUNT];
int32_t test[XYZ_AXIS_COUNT]; int32_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis // unit vector along x-axis
@ -110,10 +109,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, src, test); rotateVector(matrix, src, 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[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; EXPECT_EQ(test[Z], src[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along y-axis // unit vector along y-axis
src[X] = 0; src[X] = 0;
@ -121,10 +120,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
src[Z] = 0; src[Z] = 0;
rotateVector(matrix, src, test); rotateVector(matrix, src, 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[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; EXPECT_EQ(test[Z], src[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along z-axis // unit vector along z-axis
src[X] = 0; src[X] = 0;
@ -132,10 +131,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
src[Z] = 1; src[Z] = 1;
rotateVector(matrix, src, test); rotateVector(matrix, src, 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[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; EXPECT_EQ(test[Z], src[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// random vector to test // random vector to test
src[X] = rand() % 5; src[X] = rand() % 5;
@ -143,10 +142,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
src[Z] = rand() % 5; src[Z] = rand() % 5;
rotateVector(matrix, src, test); rotateVector(matrix, src, 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[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; 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) static void testCWFlip(sensor_align_e rotation, int32_t angle)
{ {
int32_t src[XYZ_AXIS_COUNT]; int32_t src[XYZ_AXIS_COUNT];
int32_t dest[XYZ_AXIS_COUNT];
int32_t test[XYZ_AXIS_COUNT]; int32_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis // unit vector along x-axis
@ -170,11 +168,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); 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[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; EXPECT_EQ(test[Z], src[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along y-axis // unit vector along y-axis
src[X] = 0; src[X] = 0;
@ -186,11 +184,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); 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[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; EXPECT_EQ(test[Z], src[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// unit vector along z-axis // unit vector along z-axis
src[X] = 0; src[X] = 0;
@ -202,11 +200,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); 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[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; EXPECT_EQ(test[Z], src[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
// random vector to test // random vector to test
src[X] = rand() % 5; src[X] = rand() % 5;
@ -218,11 +216,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test); 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[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X];
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y]; EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z]; EXPECT_EQ(test[Z], src[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z];
} }