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

Got alignsensor unit test working

Browse source
This commit is contained in:
Martin Budden 2016-10-16 12:51:30 +01:00
parent 2e71ac3b84
commit f6bcbff239
1 changed files with 12 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/test/unit/alignsensor_unittest.cc.txt → src/test/unit/alignsensor_unittest.cc
View file

@ -39,7 +39,7 @@ extern "C" {
#define DEG2RAD 0.01745329251 #define DEG2RAD 0.01745329251
static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out) static void rotateVector(int32_t mat[3][3], int32_t vec[3], int32_t *out)
{ {
int16_t tmp[3]; int16_t tmp[3];
@ -56,7 +56,7 @@ static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out)
} }
//static void initXAxisRotation(int16_t mat[][3], int16_t angle) //static void initXAxisRotation(int32_t mat[][3], int16_t angle)
//{ //{
// mat[0][0] = 1; // mat[0][0] = 1;
// mat[0][1] = 0; // mat[0][1] = 0;
@ -69,7 +69,7 @@ static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out)
// mat[2][2] = cos(angle*DEG2RAD); // mat[2][2] = cos(angle*DEG2RAD);
//} //}
static void initYAxisRotation(int16_t mat[][3], int16_t angle) static void initYAxisRotation(int32_t mat[][3], int16_t angle)
{ {
mat[0][0] = cos(angle*DEG2RAD); mat[0][0] = cos(angle*DEG2RAD);
mat[0][1] = 0; mat[0][1] = 0;
@ -82,7 +82,7 @@ static void initYAxisRotation(int16_t mat[][3], int16_t angle)
mat[2][2] = cos(angle*DEG2RAD); mat[2][2] = cos(angle*DEG2RAD);
} }
static void initZAxisRotation(int16_t mat[][3], int16_t angle) static void initZAxisRotation(int32_t mat[][3], int16_t angle)
{ {
mat[0][0] = cos(angle*DEG2RAD); mat[0][0] = cos(angle*DEG2RAD);
mat[0][1] = -sin(angle*DEG2RAD); mat[0][1] = -sin(angle*DEG2RAD);
@ -97,16 +97,16 @@ static void initZAxisRotation(int16_t mat[][3], int16_t angle)
static void testCW(sensor_align_e rotation, int16_t angle) static void testCW(sensor_align_e rotation, int16_t angle)
{ {
int16_t src[XYZ_AXIS_COUNT]; int32_t src[XYZ_AXIS_COUNT];
int16_t dest[XYZ_AXIS_COUNT]; int32_t dest[XYZ_AXIS_COUNT];
int16_t test[XYZ_AXIS_COUNT]; int32_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis // unit vector along x-axis
src[X] = 1; src[X] = 1;
src[Y] = 0; src[Y] = 0;
src[Z] = 0; src[Z] = 0;
int16_t matrix[3][3]; int32_t matrix[3][3];
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);
rotateVector(matrix, src, test); rotateVector(matrix, src, test);
@ -155,16 +155,16 @@ static void testCW(sensor_align_e rotation, int16_t angle)
*/ */
static void testCWFlip(sensor_align_e rotation, int16_t angle) static void testCWFlip(sensor_align_e rotation, int16_t angle)
{ {
int16_t src[XYZ_AXIS_COUNT]; int32_t src[XYZ_AXIS_COUNT];
int16_t dest[XYZ_AXIS_COUNT]; int32_t dest[XYZ_AXIS_COUNT];
int16_t test[XYZ_AXIS_COUNT]; int32_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis // unit vector along x-axis
src[X] = 1; src[X] = 1;
src[Y] = 0; src[Y] = 0;
src[Z] = 0; src[Z] = 0;
int16_t matrix[3][3]; int32_t matrix[3][3];
initYAxisRotation(matrix, 180); initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test); rotateVector(matrix, src, test);
initZAxisRotation(matrix, angle); initZAxisRotation(matrix, angle);