mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 14:55:21 +03:00
Refactored arbitrary gyro and mag alignment.
The original implementation:
* removed the old 'alignment' variable
* did not require 'ALIGN_CUSTOM'
* always used rotation matrix
* had no additional per-pid-loop conditional logic.
Extract currently unused code into tests.
In preparation for either deleting or re-using in validateAndFixConfig.
Fix code style of some old boardalignment code.
De-duplicate vector rotation code.
Now that rotation code is exacted from `alignBoard` and now doesn't use
`boardRotation` some if it was similar to the code in `rotateV` in
maths.c
Use DECIDEGREES for mag and gyro/acc custom alignments.
Use unnamed structure instead of `values`.
Redefine what 'custom' orientation means.
Move alignment test-only code into the tests.
Ensure gyro/mag custom alignment settings follow the enum variations.
This can't be applied to ALIGN_DEFAULT because, in the case of the MAG,
the default isn't actually known until the gyro is detected, see
`compassDetect`.
OMNIBUSF4/F7 - Don't use ALIGN_DEFAULT in target.h,
common_defaults_post.h does this now.
Comment cleanup.
Delete unused alignment code left from various tests/refactoring
efforts.
* Please do not squash this commit.
Fix SITL build by avoiding structure assignment with anonymous inner
struct.
The error from the build server was as follows:
```./src/main/common/sensor_alignment.c:49:5: error: missing initializer
for field ‘yaw’ of ‘struct <anonymous>’
[-Werror=missing-field-initializers]
*sensorAlignment = CUSTOM_ALIGN_CW0_DEG;
^
In file included from ./src/main/common/sensor_alignment.c:27:0:
./src/main/common/sensor_alignment.h:80:17: note: ‘yaw’ declared here
int16_t yaw;
^
```
Cleanup sensor_alignment API.
This commit is contained in:
Dominic Clifton
mikeller
parent
494b559277
commit
980df1536f
27 changed files with 558 additions and 139 deletions
|
|
@ -171,7 +171,8 @@ static const char * const lookupTableAlignment[] = {
|
|||
"CW0FLIP",
|
||||
"CW90FLIP",
|
||||
"CW180FLIP",
|
||||
"CW270FLIP"
|
||||
"CW270FLIP",
|
||||
"CUSTOM",
|
||||
};
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
|
|
@ -643,7 +644,10 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
// PG_COMPASS_CONFIG
|
||||
#ifdef USE_MAG
|
||||
{ "align_mag", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) },
|
||||
{ "align_mag", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_alignment) },
|
||||
{ "mag_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.roll) },
|
||||
{ "mag_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.pitch) },
|
||||
{ "mag_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.yaw) },
|
||||
{ "mag_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_bustype) },
|
||||
{ "mag_i2c_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_device) },
|
||||
{ "mag_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_address) },
|
||||
|
|
@ -1436,13 +1440,19 @@ const clivalue_t valueTable[] = {
|
|||
{ "gyro_1_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, spiBus) },
|
||||
{ "gyro_1_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cBus) },
|
||||
{ "gyro_1_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cAddress) },
|
||||
{ "gyro_1_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, align) },
|
||||
{ "gyro_1_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, alignment) },
|
||||
{ "gyro_1_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.roll) },
|
||||
{ "gyro_1_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.pitch) },
|
||||
{ "gyro_1_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.yaw) },
|
||||
#ifdef USE_MULTI_GYRO
|
||||
{ "gyro_2_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, bustype) },
|
||||
{ "gyro_2_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, spiBus) },
|
||||
{ "gyro_2_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cBus) },
|
||||
{ "gyro_2_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cAddress) },
|
||||
{ "gyro_2_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, align) },
|
||||
{ "gyro_2_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, alignment) },
|
||||
{ "gyro_2_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.roll) },
|
||||
{ "gyro_2_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.pitch) },
|
||||
{ "gyro_2_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.yaw) },
|
||||
#endif
|
||||
#ifdef I2C_FULL_RECONFIGURABILITY
|
||||
#ifdef USE_I2C_DEVICE_1
|
||||
|
|
|
|||
|
|
@ -23,6 +23,8 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "axis.h"
|
||||
#include "maths.h"
|
||||
|
||||
|
|
@ -197,7 +199,7 @@ void normalizeV(struct fp_vector *src, struct fp_vector *dest)
|
|||
}
|
||||
}
|
||||
|
||||
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
|
||||
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
|
||||
{
|
||||
float cosx, sinx, cosy, siny, cosz, sinz;
|
||||
float coszcosx, sinzcosx, coszsinx, sinzsinx;
|
||||
|
|
@ -214,15 +216,25 @@ void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
|
|||
coszsinx = sinx * cosz;
|
||||
sinzsinx = sinx * sinz;
|
||||
|
||||
matrix[0][X] = cosz * cosy;
|
||||
matrix[0][Y] = -cosy * sinz;
|
||||
matrix[0][Z] = siny;
|
||||
matrix[1][X] = sinzcosx + (coszsinx * siny);
|
||||
matrix[1][Y] = coszcosx - (sinzsinx * siny);
|
||||
matrix[1][Z] = -sinx * cosy;
|
||||
matrix[2][X] = (sinzsinx) - (coszcosx * siny);
|
||||
matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
|
||||
matrix[2][Z] = cosy * cosx;
|
||||
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;
|
||||
}
|
||||
|
||||
FAST_CODE void applyRotation(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);
|
||||
}
|
||||
|
||||
// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
|
||||
|
|
@ -230,13 +242,11 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
|
|||
{
|
||||
struct fp_vector v_tmp = *v;
|
||||
|
||||
float matrix[3][3];
|
||||
fp_rotationMatrix_t rotationMatrix;
|
||||
|
||||
buildRotationMatrix(delta, matrix);
|
||||
buildRotationMatrix(delta, &rotationMatrix);
|
||||
|
||||
v->X = v_tmp.X * matrix[0][X] + v_tmp.Y * matrix[1][X] + v_tmp.Z * matrix[2][X];
|
||||
v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y];
|
||||
v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z];
|
||||
applyRotation((float *)&v_tmp, &rotationMatrix);
|
||||
}
|
||||
|
||||
// Quick median filter implementation
|
||||
|
|
|
|||
|
|
@ -93,6 +93,10 @@ 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);
|
||||
float powerf(float base, int exp);
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband);
|
||||
|
|
@ -110,7 +114,8 @@ float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float des
|
|||
void normalizeV(struct fp_vector *src, struct fp_vector *dest);
|
||||
|
||||
void rotateV(struct fp_vector *v, fp_angles_t *delta);
|
||||
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);
|
||||
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation);
|
||||
void applyRotation(float *v, fp_rotationMatrix_t *rotationMatrix);
|
||||
|
||||
int32_t quickMedianFilter3(int32_t * v);
|
||||
int32_t quickMedianFilter5(int32_t * v);
|
||||
|
|
|
|||
55
src/main/common/sensor_alignment.c
Normal file
55
src/main/common/sensor_alignment.c
Normal file
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and 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.
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "stdbool.h"
|
||||
#include "string.h"
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "common/sensor_alignment_impl.h"
|
||||
|
||||
void buildRotationMatrixFromAlignment(const sensorAlignment_t* sensorAlignment, fp_rotationMatrix_t* rm)
|
||||
{
|
||||
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);
|
||||
|
||||
buildRotationMatrix(&rotationAngles, rm);
|
||||
}
|
||||
|
||||
|
||||
void buildAlignmentFromStandardAlignment(sensorAlignment_t* sensorAlignment, sensor_align_e alignment)
|
||||
{
|
||||
if (alignment == ALIGN_CUSTOM || alignment == ALIGN_DEFAULT) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t alignmentBits = ALIGNMENT_TO_BITMASK(alignment);
|
||||
|
||||
memset(sensorAlignment, 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);
|
||||
}
|
||||
}
|
||||
73
src/main/common/sensor_alignment.h
Normal file
73
src/main/common/sensor_alignment.h
Normal file
|
|
@ -0,0 +1,73 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and 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.
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "axis.h"
|
||||
#include "maths.h"
|
||||
|
||||
typedef enum {
|
||||
ALIGN_DEFAULT = 0, // driver-provided alignment
|
||||
|
||||
// the order of these 8 values also correlate to corresponding code in ALIGNMENT_TO_BITMASK.
|
||||
|
||||
// R, P, Y
|
||||
CW0_DEG = 1, // 00,00,00
|
||||
CW90_DEG = 2, // 00,00,01
|
||||
CW180_DEG = 3, // 00,00,10
|
||||
CW270_DEG = 4, // 00,00,11
|
||||
CW0_DEG_FLIP = 5, // 00,10,00 // _FLIP = 2x90 degree PITCH rotations
|
||||
CW90_DEG_FLIP = 6, // 00,10,01
|
||||
CW180_DEG_FLIP = 7, // 00,10,10
|
||||
CW270_DEG_FLIP = 8, // 00,10,11
|
||||
|
||||
ALIGN_CUSTOM = 9, // arbitrary sensor angles, e.g. for external sensors
|
||||
} sensor_align_e;
|
||||
|
||||
typedef union sensorAlignment_u {
|
||||
// value order is the same as axis_e
|
||||
|
||||
// values are in DECIDEGREES, and should be limited to +/- 3600
|
||||
|
||||
int16_t raw[XYZ_AXIS_COUNT];
|
||||
struct {
|
||||
int16_t roll;
|
||||
int16_t pitch;
|
||||
int16_t yaw;
|
||||
};
|
||||
} sensorAlignment_t;
|
||||
|
||||
#define SENSOR_ALIGNMENT(ROLL, PITCH, YAW) ((sensorAlignment_t){\
|
||||
.roll = DEGREES_TO_DECIDEGREES(ROLL), \
|
||||
.pitch = DEGREES_TO_DECIDEGREES(PITCH), \
|
||||
.yaw = DEGREES_TO_DECIDEGREES(YAW), \
|
||||
})
|
||||
|
||||
#define CUSTOM_ALIGN_CW0_DEG SENSOR_ALIGNMENT( 0, 0, 0)
|
||||
#define CUSTOM_ALIGN_CW90_DEG SENSOR_ALIGNMENT( 0, 0, 90)
|
||||
#define CUSTOM_ALIGN_CW180_DEG SENSOR_ALIGNMENT( 0, 0, 180)
|
||||
#define CUSTOM_ALIGN_CW270_DEG SENSOR_ALIGNMENT( 0, 0, 270)
|
||||
#define CUSTOM_ALIGN_CW0_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 0)
|
||||
#define CUSTOM_ALIGN_CW90_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 90)
|
||||
#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);
|
||||
48
src/main/common/sensor_alignment_impl.h
Normal file
48
src/main/common/sensor_alignment_impl.h
Normal file
|
|
@ -0,0 +1,48 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and 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.
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
//
|
||||
// alignment bitmasks
|
||||
//
|
||||
|
||||
#define ALIGNMENT_ROTATION_WIDTH 2
|
||||
|
||||
#define ALIGNMENT_YAW_ROTATIONS_POSITION (0 * ALIGNMENT_ROTATION_WIDTH)
|
||||
#define ALIGNMENT_PITCH_ROTATIONS_POSITION (1 * ALIGNMENT_ROTATION_WIDTH)
|
||||
#define ALIGNMENT_ROLL_ROTATIONS_POSITION (2 * ALIGNMENT_ROTATION_WIDTH)
|
||||
|
||||
#define ALIGNMENT_YAW_ROTATIONS_MASK (0x3 << ALIGNMENT_YAW_ROTATIONS_POSITION)
|
||||
#define ALIGNMENT_PITCH_ROTATIONS_MASK (0x3 << ALIGNMENT_PITCH_ROTATIONS_POSITION)
|
||||
#define ALIGNMENT_ROLL_ROTATIONS_MASK (0x3 << ALIGNMENT_ROLL_ROTATIONS_POSITION)
|
||||
|
||||
#define ALIGNMENT_AXIS_ROTATIONS_MASK(axis) (0x3 << ((FD_YAW - axis) * ALIGNMENT_ROTATION_WIDTH))
|
||||
|
||||
#define ALIGNMENT_YAW_ROTATIONS(bits) ((bits & ALIGNMENT_YAW_ROTATIONS_MASK) >> ALIGNMENT_YAW_ROTATIONS_POSITION)
|
||||
#define ALIGNMENT_PITCH_ROTATIONS(bits) ((bits & ALIGNMENT_PITCH_ROTATIONS_MASK) >> ALIGNMENT_PITCH_ROTATIONS_POSITION)
|
||||
#define ALIGNMENT_ROLL_ROTATIONS(bits) ((bits & ALIGNMENT_ROLL_ROTATIONS_MASK) >> ALIGNMENT_ROLL_ROTATIONS_POSITION)
|
||||
|
||||
#define ALIGNMENT_AXIS_ROTATIONS(bits, axis) ((bits & ALIGNMENT_AXIS_ROTATIONS_MASK(axis)) >> ((FD_YAW - axis) * ALIGNMENT_ROTATION_WIDTH))
|
||||
|
||||
// [1:0] count of 90 degree rotations from 0
|
||||
// [3:2] indicates 90 degree rotations on pitch
|
||||
// [5:4] indicates 90 degree rotations on roll
|
||||
#define ALIGNMENT_TO_BITMASK(alignment) ((alignment - CW0_DEG) & 0x3) | (((alignment - CW0_DEG) & 0x4) << 1)
|
||||
|
|
@ -23,6 +23,8 @@
|
|||
#include "platform.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
|
@ -103,6 +105,7 @@ typedef struct gyroDev_s {
|
|||
ioTag_t mpuIntExtiTag;
|
||||
uint8_t gyroHasOverflowProtection;
|
||||
gyroHardware_e gyroHardware;
|
||||
fp_rotationMatrix_t rotationMatrix;
|
||||
} gyroDev_t;
|
||||
|
||||
typedef struct accDev_s {
|
||||
|
|
@ -121,6 +124,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;
|
||||
} accDev_t;
|
||||
|
||||
static inline void accDevLock(accDev_t *acc)
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "common/sensor_alignment.h"
|
||||
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/exti.h"
|
||||
|
|
@ -29,7 +31,8 @@ typedef struct magDev_s {
|
|||
sensorMagReadFuncPtr read; // read 3 axis data function
|
||||
extiCallbackRec_t exti;
|
||||
busDevice_t busdev;
|
||||
sensor_align_e magAlign;
|
||||
sensor_align_e magAlignment;
|
||||
fp_rotationMatrix_t rotationMatrix;
|
||||
ioTag_t magIntExtiTag;
|
||||
int16_t magGain[3];
|
||||
} magDev_t;
|
||||
|
|
|
|||
|
|
@ -23,18 +23,6 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
typedef enum {
|
||||
ALIGN_DEFAULT = 0, // driver-provided alignment
|
||||
CW0_DEG = 1,
|
||||
CW90_DEG = 2,
|
||||
CW180_DEG = 3,
|
||||
CW270_DEG = 4,
|
||||
CW0_DEG_FLIP = 5,
|
||||
CW90_DEG_FLIP = 6,
|
||||
CW180_DEG_FLIP = 7,
|
||||
CW270_DEG_FLIP = 8
|
||||
} sensor_align_e;
|
||||
|
||||
typedef bool (*sensorInterruptFuncPtr)(void);
|
||||
struct magDev_s;
|
||||
typedef bool (*sensorMagInitFuncPtr)(struct magDev_s *magdev);
|
||||
|
|
|
|||
|
|
@ -63,6 +63,7 @@
|
|||
#include "pg/pg_ids.h"
|
||||
#include "pg/motor.h"
|
||||
#include "pg/rx.h"
|
||||
#include "pg/gyrodev.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
|
@ -71,6 +72,9 @@
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/compass.h"
|
||||
|
||||
#include "common/sensor_alignment.h"
|
||||
|
||||
static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */
|
||||
|
||||
|
|
@ -256,6 +260,12 @@ static void validateAndFixConfig(void)
|
|||
|
||||
validateAndFixGyroConfig();
|
||||
|
||||
buildAlignmentFromStandardAlignment(&compassConfigMutable()->mag_customAlignment, compassConfig()->mag_alignment);
|
||||
buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(0)->customAlignment, gyroDeviceConfig(0)->alignment);
|
||||
#if defined(USE_MULTI_GYRO)
|
||||
buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(1)->customAlignment, gyroDeviceConfig(1)->alignment);
|
||||
#endif
|
||||
|
||||
if (!(featureIsEnabled(FEATURE_RX_PARALLEL_PWM) || featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_SERIAL) || featureIsEnabled(FEATURE_RX_MSP) || featureIsEnabled(FEATURE_RX_SPI))) {
|
||||
featureEnable(DEFAULT_RX_FEATURE);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1368,41 +1368,41 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
|
||||
break;
|
||||
|
||||
|
||||
case MSP_SENSOR_ALIGNMENT: {
|
||||
uint8_t gyroAlignment;
|
||||
#ifdef USE_MULTI_GYRO
|
||||
switch (gyroConfig()->gyro_to_use) {
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroAlignment = gyroDeviceConfig(1)->align;
|
||||
gyroAlignment = gyroDeviceConfig(1)->alignment;
|
||||
break;
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
// for dual-gyro in "BOTH" mode we only read/write gyro 0
|
||||
default:
|
||||
gyroAlignment = gyroDeviceConfig(0)->align;
|
||||
gyroAlignment = gyroDeviceConfig(0)->alignment;
|
||||
break;
|
||||
}
|
||||
#else
|
||||
gyroAlignment = gyroDeviceConfig(0)->align;
|
||||
gyroAlignment = gyroDeviceConfig(0)->alignment;
|
||||
#endif
|
||||
sbufWriteU8(dst, gyroAlignment);
|
||||
sbufWriteU8(dst, gyroAlignment); // Starting with 4.0 gyro and acc alignment are the same
|
||||
sbufWriteU8(dst, compassConfig()->mag_align);
|
||||
sbufWriteU8(dst, compassConfig()->mag_alignment);
|
||||
|
||||
// API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
|
||||
sbufWriteU8(dst, getGyroDetectionFlags());
|
||||
#ifdef USE_MULTI_GYRO
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(0)->align);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(1)->align);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(1)->alignment);
|
||||
#else
|
||||
sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(0)->align);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
|
||||
sbufWriteU8(dst, ALIGN_DEFAULT);
|
||||
#endif
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case MSP_ADVANCED_CONFIG:
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
|
||||
sbufWriteU8(dst, pidConfig()->pid_process_denom);
|
||||
|
|
@ -2007,21 +2007,22 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
case MSP_SET_RESET_CURR_PID:
|
||||
resetPidProfile(currentPidProfile);
|
||||
break;
|
||||
|
||||
case MSP_SET_SENSOR_ALIGNMENT: {
|
||||
// maintain backwards compatibility for API < 1.41
|
||||
const uint8_t gyroAlignment = sbufReadU8(src);
|
||||
sbufReadU8(src); // discard deprecated acc_align
|
||||
compassConfigMutable()->mag_align = sbufReadU8(src);
|
||||
compassConfigMutable()->mag_alignment = sbufReadU8(src);
|
||||
|
||||
if (sbufBytesRemaining(src) >= 3) {
|
||||
// API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
|
||||
gyroDeviceConfigMutable(0)->align = sbufReadU8(src);
|
||||
gyroDeviceConfigMutable(1)->align = sbufReadU8(src);
|
||||
gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
|
||||
gyroDeviceConfigMutable(1)->alignment = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src); // unused gyro_to_use
|
||||
gyroDeviceConfigMutable(0)->align = sbufReadU8(src);
|
||||
gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
|
||||
sbufReadU8(src); // unused gyro_2_sensor_align
|
||||
#endif
|
||||
} else {
|
||||
|
|
@ -2029,16 +2030,16 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
#ifdef USE_MULTI_GYRO
|
||||
switch (gyroConfig()->gyro_to_use) {
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroDeviceConfigMutable(1)->align = gyroAlignment;
|
||||
gyroDeviceConfigMutable(1)->alignment = gyroAlignment;
|
||||
break;
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
// For dual-gyro in "BOTH" mode we'll only update gyro 0
|
||||
default:
|
||||
gyroDeviceConfigMutable(0)->align = gyroAlignment;
|
||||
gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
|
||||
break;
|
||||
}
|
||||
#else
|
||||
gyroDeviceConfigMutable(0)->align = gyroAlignment;
|
||||
gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,6 +23,8 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/sensor_alignment.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "pg/gyrodev.h"
|
||||
|
|
@ -36,30 +38,31 @@
|
|||
ioTag_t selectMPUIntExtiConfigByHardwareRevision(void); // XXX Should be gone
|
||||
|
||||
#if defined(USE_SPI_GYRO) || defined(USE_I2C_GYRO)
|
||||
static void gyroResetCommonDeviceConfig(gyroDeviceConfig_t *devconf, ioTag_t extiTag, uint8_t align)
|
||||
static void gyroResetCommonDeviceConfig(gyroDeviceConfig_t *devconf, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment)
|
||||
{
|
||||
devconf->extiTag = extiTag;
|
||||
devconf->align = align;
|
||||
devconf->alignment = alignment;
|
||||
devconf->customAlignment = customAlignment;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SPI_GYRO
|
||||
static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, uint8_t align)
|
||||
static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment)
|
||||
{
|
||||
devconf->bustype = BUSTYPE_SPI;
|
||||
devconf->spiBus = SPI_DEV_TO_CFG(spiDeviceByInstance(instance));
|
||||
devconf->csnTag = csnTag;
|
||||
gyroResetCommonDeviceConfig(devconf, extiTag, align);
|
||||
gyroResetCommonDeviceConfig(devconf, extiTag, alignment, customAlignment);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO)
|
||||
static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, uint8_t align)
|
||||
static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment)
|
||||
{
|
||||
devconf->bustype = BUSTYPE_I2C;
|
||||
devconf->i2cBus = I2C_DEV_TO_CFG(i2cbus);
|
||||
devconf->i2cAddress = GYRO_I2C_ADDRESS;
|
||||
gyroResetCommonDeviceConfig(devconf, extiTag, align);
|
||||
gyroResetCommonDeviceConfig(devconf, extiTag, alignment, customAlignment);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -71,17 +74,17 @@ void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf)
|
|||
|
||||
// All multi-gyro boards use SPI based gyros.
|
||||
#ifdef USE_SPI_GYRO
|
||||
gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN);
|
||||
gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN, GYRO_1_CUSTOM_ALIGN);
|
||||
#ifdef USE_MULTI_GYRO
|
||||
devconf[1].index = 1;
|
||||
gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), GYRO_2_ALIGN);
|
||||
gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), GYRO_2_ALIGN, GYRO_2_CUSTOM_ALIGN);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// I2C gyros appear as a sole gyro in single gyro boards.
|
||||
#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO)
|
||||
devconf[0].i2cBus = I2C_DEV_TO_CFG(I2CINVALID); // XXX Not required?
|
||||
gyroResetI2cDeviceConfig(&devconf[0], I2C_DEVICE, IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN);
|
||||
gyroResetI2cDeviceConfig(&devconf[0], I2C_DEVICE, IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN, GYRO_1_CUSTOM_ALIGN);
|
||||
#endif
|
||||
|
||||
// Special treatment for very rare F3 targets with variants having either I2C or SPI acc/gyro chip; mark it for run time detection.
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "drivers/io_types.h"
|
||||
|
||||
typedef struct gyroDeviceConfig_s {
|
||||
|
|
@ -34,7 +35,8 @@ typedef struct gyroDeviceConfig_s {
|
|||
uint8_t i2cBus;
|
||||
uint8_t i2cAddress;
|
||||
ioTag_t extiTag;
|
||||
uint8_t align; // sensor_align_e
|
||||
uint8_t alignment; // sensor_align_e
|
||||
sensorAlignment_t customAlignment;
|
||||
} gyroDeviceConfig_t;
|
||||
|
||||
PG_DECLARE_ARRAY(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig);
|
||||
|
|
|
|||
|
|
@ -320,12 +320,18 @@ bool accInit(uint32_t gyroSamplingInverval)
|
|||
// Copy alignment from active gyro, as all production boards use acc-gyro-combi chip.
|
||||
// Exceptions are STM32F3DISCOVERY and STM32F411DISCOVERY, and (may be) handled in future enhancement.
|
||||
|
||||
acc.dev.accAlign = gyroDeviceConfig(0)->align;
|
||||
sensor_align_e alignment = gyroDeviceConfig(0)->alignment;
|
||||
const sensorAlignment_t* customAlignment = &gyroDeviceConfig(0)->customAlignment;
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) {
|
||||
acc.dev.accAlign = gyroDeviceConfig(1)->align;
|
||||
alignment = gyroDeviceConfig(1)->alignment;
|
||||
|
||||
customAlignment = &gyroDeviceConfig(1)->customAlignment;
|
||||
}
|
||||
#endif
|
||||
acc.dev.accAlign = alignment;
|
||||
buildRotationMatrixFromAlignment(customAlignment, &acc.dev.rotationMatrix);
|
||||
|
||||
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
|
||||
return false;
|
||||
|
|
@ -485,7 +491,11 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
|
|||
}
|
||||
}
|
||||
|
||||
alignSensors(acc.accADC, acc.dev.accAlign);
|
||||
if (acc.dev.accAlign == ALIGN_CUSTOM) {
|
||||
alignSensorViaMatrix(acc.accADC, &acc.dev.rotationMatrix);
|
||||
} else {
|
||||
alignSensorViaRotation(acc.accADC, acc.dev.accAlign);
|
||||
}
|
||||
|
||||
if (!accIsCalibrationComplete()) {
|
||||
performAcclerationCalibration(rollAndPitchTrims);
|
||||
|
|
|
|||
|
|
@ -25,8 +25,10 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
|
@ -36,7 +38,7 @@
|
|||
#include "boardalignment.h"
|
||||
|
||||
static bool standardBoardAlignment = true; // board orientation correction
|
||||
static float boardRotation[3][3]; // matrix
|
||||
static fp_rotationMatrix_t boardRotation;
|
||||
|
||||
// no template required since defaults are zero
|
||||
PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0);
|
||||
|
|
@ -59,21 +61,24 @@ void initBoardAlignment(const boardAlignment_t *boardAlignment)
|
|||
rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
|
||||
rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees );
|
||||
|
||||
buildRotationMatrix(&rotationAngles, boardRotation);
|
||||
buildRotationMatrix(&rotationAngles, &boardRotation);
|
||||
}
|
||||
|
||||
static void alignBoard(float *vec)
|
||||
FAST_CODE static void alignBoard(float *vec)
|
||||
{
|
||||
float x = vec[X];
|
||||
float y = vec[Y];
|
||||
float z = vec[Z];
|
||||
|
||||
vec[X] = (boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z);
|
||||
vec[Y] = (boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z);
|
||||
vec[Z] = (boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
|
||||
applyRotation(vec, &boardRotation);
|
||||
}
|
||||
|
||||
FAST_CODE void alignSensors(float *dest, uint8_t rotation)
|
||||
FAST_CODE void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* sensorRotationMatrix)
|
||||
{
|
||||
applyRotation(dest, sensorRotationMatrix);
|
||||
|
||||
if (!standardBoardAlignment) {
|
||||
alignBoard(dest);
|
||||
}
|
||||
}
|
||||
|
||||
FAST_CODE void alignSensorViaRotation(float *dest, uint8_t rotation)
|
||||
{
|
||||
const float x = dest[X];
|
||||
const float y = dest[Y];
|
||||
|
|
@ -123,6 +128,7 @@ FAST_CODE void alignSensors(float *dest, uint8_t rotation)
|
|||
break;
|
||||
}
|
||||
|
||||
if (!standardBoardAlignment)
|
||||
if (!standardBoardAlignment) {
|
||||
alignBoard(dest);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,6 +20,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
||||
typedef struct boardAlignment_s {
|
||||
|
|
@ -30,5 +33,7 @@ typedef struct boardAlignment_s {
|
|||
|
||||
PG_DECLARE(boardAlignment_t, boardAlignment);
|
||||
|
||||
void alignSensors(float *dest, uint8_t rotation);
|
||||
void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* rotationMatrix);
|
||||
void alignSensorViaRotation(float *dest, uint8_t rotation);
|
||||
|
||||
void initBoardAlignment(const boardAlignment_t *boardAlignment);
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -61,7 +62,8 @@ PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 1);
|
|||
|
||||
void pgResetFn_compassConfig(compassConfig_t *compassConfig)
|
||||
{
|
||||
compassConfig->mag_align = ALIGN_DEFAULT;
|
||||
compassConfig->mag_alignment = ALIGN_DEFAULT;
|
||||
memset(&compassConfig->mag_customAlignment, 0x00, sizeof(compassConfig->mag_customAlignment));
|
||||
compassConfig->mag_declination = 0;
|
||||
compassConfig->mag_hardware = MAG_DEFAULT;
|
||||
|
||||
|
|
@ -115,8 +117,10 @@ void compassPreInit(void)
|
|||
}
|
||||
|
||||
#if !defined(SIMULATOR_BUILD)
|
||||
bool compassDetect(magDev_t *dev)
|
||||
bool compassDetect(magDev_t *dev, uint8_t *alignment)
|
||||
{
|
||||
*alignment = ALIGN_DEFAULT; // may be overridden if target specifies MAG_*_ALIGN
|
||||
|
||||
magSensor_e magHardware = MAG_NONE;
|
||||
|
||||
busDevice_t *busdev = &dev->busdev;
|
||||
|
|
@ -167,8 +171,6 @@ bool compassDetect(magDev_t *dev)
|
|||
return false;
|
||||
}
|
||||
|
||||
dev->magAlign = ALIGN_DEFAULT;
|
||||
|
||||
switch (compassConfig()->mag_hardware) {
|
||||
case MAG_DEFAULT:
|
||||
FALLTHROUGH;
|
||||
|
|
@ -181,7 +183,7 @@ bool compassDetect(magDev_t *dev)
|
|||
|
||||
if (hmc5883lDetect(dev)) {
|
||||
#ifdef MAG_HMC5883_ALIGN
|
||||
dev->magAlign = MAG_HMC5883_ALIGN;
|
||||
*alignment = MAG_HMC5883_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_HMC5883;
|
||||
break;
|
||||
|
|
@ -197,7 +199,7 @@ bool compassDetect(magDev_t *dev)
|
|||
|
||||
if (lis3mdlDetect(dev)) {
|
||||
#ifdef MAG_LIS3MDL_ALIGN
|
||||
dev->magAlign = MAG_LIS3MDL_ALIGN;
|
||||
*alignment = MAG_LIS3MDL_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_LIS3MDL;
|
||||
break;
|
||||
|
|
@ -213,7 +215,7 @@ bool compassDetect(magDev_t *dev)
|
|||
|
||||
if (ak8975Detect(dev)) {
|
||||
#ifdef MAG_AK8975_ALIGN
|
||||
dev->magAlign = MAG_AK8975_ALIGN;
|
||||
*alignment = MAG_AK8975_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_AK8975;
|
||||
break;
|
||||
|
|
@ -234,7 +236,7 @@ bool compassDetect(magDev_t *dev)
|
|||
|
||||
if (ak8963Detect(dev)) {
|
||||
#ifdef MAG_AK8963_ALIGN
|
||||
dev->magAlign = MAG_AK8963_ALIGN;
|
||||
*alignment = MAG_AK8963_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_AK8963;
|
||||
break;
|
||||
|
|
@ -250,7 +252,7 @@ bool compassDetect(magDev_t *dev)
|
|||
|
||||
if (qmc5883lDetect(dev)) {
|
||||
#ifdef MAG_QMC5883L_ALIGN
|
||||
dev->magAlign = MAG_QMC5883L_ALIGN;
|
||||
*alignment = MAG_QMC5883L_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_QMC5883;
|
||||
break;
|
||||
|
|
@ -272,9 +274,10 @@ bool compassDetect(magDev_t *dev)
|
|||
return true;
|
||||
}
|
||||
#else
|
||||
bool compassDetect(magDev_t *dev)
|
||||
bool compassDetect(magDev_t *dev, sensor_align_e *alignment)
|
||||
{
|
||||
UNUSED(dev);
|
||||
UNUSED(alignment);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
@ -286,7 +289,9 @@ bool compassInit(void)
|
|||
// calculate magnetic declination
|
||||
mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
|
||||
|
||||
if (!compassDetect(&magDev)) {
|
||||
sensor_align_e alignment;
|
||||
|
||||
if (!compassDetect(&magDev, &alignment)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -297,9 +302,15 @@ bool compassInit(void)
|
|||
magDev.init(&magDev);
|
||||
LED1_OFF;
|
||||
magInit = 1;
|
||||
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
|
||||
magDev.magAlign = compassConfig()->mag_align;
|
||||
|
||||
magDev.magAlignment = alignment;
|
||||
|
||||
if (compassConfig()->mag_alignment != ALIGN_DEFAULT) {
|
||||
magDev.magAlignment = compassConfig()->mag_alignment;
|
||||
}
|
||||
|
||||
buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -318,7 +329,11 @@ void compassUpdate(timeUs_t currentTimeUs)
|
|||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
mag.magADC[axis] = magADCRaw[axis];
|
||||
}
|
||||
alignSensors(mag.magADC, magDev.magAlign);
|
||||
if (magDev.magAlignment == ALIGN_CUSTOM) {
|
||||
alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix);
|
||||
} else {
|
||||
alignSensorViaRotation(mag.magADC, magDev.magAlignment);
|
||||
}
|
||||
|
||||
flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
|
||||
if (STATE(CALIBRATE_MAG)) {
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "common/time.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "drivers/io_types.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "pg/pg.h"
|
||||
|
|
@ -48,7 +49,7 @@ extern mag_t mag;
|
|||
typedef struct compassConfig_s {
|
||||
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
|
||||
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
|
||||
sensor_align_e mag_align; // mag alignment
|
||||
uint8_t mag_alignment; // mag alignment
|
||||
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
|
||||
uint8_t mag_bustype;
|
||||
uint8_t mag_i2c_device;
|
||||
|
|
@ -57,6 +58,7 @@ typedef struct compassConfig_s {
|
|||
ioTag_t mag_spi_csn;
|
||||
ioTag_t interruptTag;
|
||||
flightDynamicsTrims_t magZero;
|
||||
sensorAlignment_t mag_customAlignment;
|
||||
} compassConfig_t;
|
||||
|
||||
PG_DECLARE(compassConfig_t, compassConfig);
|
||||
|
|
|
|||
|
|
@ -433,7 +433,8 @@ static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
|
|||
{
|
||||
gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
|
||||
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
|
||||
gyroSensor->gyroDev.gyroAlign = config->align;
|
||||
gyroSensor->gyroDev.gyroAlign = config->alignment;
|
||||
buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
|
||||
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
|
||||
|
||||
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
|
||||
|
|
@ -1037,7 +1038,11 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
|
|||
gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
|
||||
#endif
|
||||
|
||||
alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
|
||||
if (gyroSensor->gyroDev.gyroAlign == ALIGN_CUSTOM) {
|
||||
alignSensorViaMatrix(gyroSensor->gyroDev.gyroADC, &gyroSensor->gyroDev.rotationMatrix);
|
||||
} else {
|
||||
alignSensorViaRotation(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
|
||||
}
|
||||
} else {
|
||||
performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
|
||||
// still calibrating, so no need to further process gyro data
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@
|
|||
void targetConfiguration(void)
|
||||
{
|
||||
if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) {
|
||||
gyroDeviceConfigMutable(0)->align = CW180_DEG;
|
||||
gyroDeviceConfigMutable(0)->alignment = CW180_DEG;
|
||||
beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -39,10 +39,10 @@
|
|||
void targetConfiguration(void)
|
||||
{
|
||||
if (hardwareRevision == FF_RACEPIT_REV_1) {
|
||||
gyroDeviceConfigMutable(0)->align = CW180_DEG;
|
||||
gyroDeviceConfigMutable(0)->alignment = CW180_DEG;
|
||||
}
|
||||
else {
|
||||
gyroDeviceConfigMutable(0)->align = CW90_DEG_FLIP;
|
||||
gyroDeviceConfigMutable(0)->alignment = CW90_DEG_FLIP;
|
||||
}
|
||||
|
||||
telemetryConfigMutable()->halfDuplex = false;
|
||||
|
|
|
|||
|
|
@ -113,7 +113,6 @@
|
|||
// Dummy defines
|
||||
#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE
|
||||
#define GYRO_2_CS_PIN NONE
|
||||
#define GYRO_2_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_2_EXTI_PIN NONE
|
||||
|
||||
#if !defined(SYNERGYF4) //No mag sensor on SYNERGYF4
|
||||
|
|
|
|||
|
|
@ -62,7 +62,6 @@
|
|||
#define GYRO_2_SPI_INSTANCE SPI1
|
||||
#define GYRO_2_CS_PIN PA4
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
#define GYRO_2_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_1_EXTI_PIN PD0 // MPU6000
|
||||
#define GYRO_2_EXTI_PIN PE8 // ICM20608
|
||||
|
||||
|
|
@ -81,8 +80,6 @@
|
|||
#define GYRO_1_CS_PIN PA15
|
||||
#define GYRO_2_SPI_INSTANCE SPI1
|
||||
#define GYRO_2_CS_PIN PA4
|
||||
#define GYRO_1_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_2_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_1_EXTI_PIN PE8 // ICM20608
|
||||
#define GYRO_2_EXTI_PIN PD0 // MPU6000
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -320,6 +320,8 @@
|
|||
#endif
|
||||
#endif
|
||||
|
||||
// gyro hardware
|
||||
|
||||
#if !defined(GYRO_1_SPI_INSTANCE)
|
||||
#define GYRO_1_SPI_INSTANCE NULL
|
||||
#endif
|
||||
|
|
@ -332,15 +334,10 @@
|
|||
#define GYRO_1_EXTI_PIN NONE
|
||||
#endif
|
||||
|
||||
#if !defined(GYRO_1_ALIGN)
|
||||
#define GYRO_1_ALIGN ALIGN_DEFAULT
|
||||
#endif
|
||||
|
||||
// F4 and F7 single gyro boards
|
||||
#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE)
|
||||
#define GYRO_2_SPI_INSTANCE NULL
|
||||
#define GYRO_2_CS_PIN NONE
|
||||
#define GYRO_2_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_2_EXTI_PIN NONE
|
||||
#endif
|
||||
|
||||
|
|
@ -358,6 +355,24 @@
|
|||
#define MAX_ACCDEV_COUNT 1
|
||||
#endif
|
||||
|
||||
// gyro alignments
|
||||
|
||||
#if !defined(GYRO_1_ALIGN)
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
#endif
|
||||
|
||||
#if !defined(GYRO_2_ALIGN)
|
||||
#define GYRO_2_ALIGN CW0_DEG
|
||||
#endif
|
||||
|
||||
#if !defined(GYRO_1_CUSTOM_ALIGN)
|
||||
#define GYRO_1_CUSTOM_ALIGN CUSTOM_ALIGN_CW0_DEG
|
||||
#endif
|
||||
|
||||
#if !defined(GYRO_2_CUSTOM_ALIGN)
|
||||
#define GYRO_2_CUSTOM_ALIGN CUSTOM_ALIGN_CW0_DEG
|
||||
#endif
|
||||
|
||||
#ifdef USE_VCP
|
||||
#ifndef USB_DETECT_PIN
|
||||
#define USB_DETECT_PIN NONE
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ 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
|
||||
|
||||
arming_prevention_unittest_SRC := \
|
||||
|
|
@ -279,6 +280,7 @@ sensor_gyro_unittest_SRC := \
|
|||
$(USER_DIR)/sensors/boardalignment.c \
|
||||
$(USER_DIR)/common/filter.c \
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/sensor_alignment.c \
|
||||
$(USER_DIR)/drivers/accgyro/accgyro_fake.c \
|
||||
$(USER_DIR)/drivers/accgyro/gyro_sync.c \
|
||||
$(USER_DIR)/pg/pg.c \
|
||||
|
|
|
|||
|
|
@ -21,6 +21,8 @@
|
|||
|
||||
extern "C" {
|
||||
#include "common/axis.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "common/sensor_alignment_impl.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
|
@ -33,7 +35,7 @@ extern "C" {
|
|||
* The output of alignSensor() is compared to the output of the test
|
||||
* rotation method.
|
||||
*
|
||||
* For each alignment condition (CW0, CW90, etc) the source vector under
|
||||
* For each alignment condition (ALIGN_CW0, CW90, etc) the source vector under
|
||||
* test is set to a unit vector along each axis (x-axis, y-axis, z-axis)
|
||||
* plus one additional random vector is tested.
|
||||
*/
|
||||
|
|
@ -96,6 +98,21 @@ static void initZAxisRotation(int32_t mat[][3], int32_t angle)
|
|||
mat[2][2] = 1;
|
||||
}
|
||||
|
||||
#define TOL 1e-5 // TOLERANCE
|
||||
|
||||
static void alignSensorViaMatrixFromRotation(float *dest, sensor_align_e alignment)
|
||||
{
|
||||
fp_rotationMatrix_t sensorRotationMatrix;
|
||||
|
||||
sensorAlignment_t sensorAlignment;
|
||||
|
||||
buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
|
||||
|
||||
buildRotationMatrixFromAlignment(&sensorAlignment, &sensorRotationMatrix);
|
||||
|
||||
alignSensorViaMatrix(dest, &sensorRotationMatrix);
|
||||
}
|
||||
|
||||
static void testCW(sensor_align_e rotation, int32_t angle)
|
||||
{
|
||||
float src[XYZ_AXIS_COUNT];
|
||||
|
|
@ -110,10 +127,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
|
|||
initZAxisRotation(matrix, angle);
|
||||
rotateVector(matrix, src, test);
|
||||
|
||||
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];
|
||||
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;
|
||||
|
|
@ -121,10 +138,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
|
|||
src[Z] = 0;
|
||||
|
||||
rotateVector(matrix, src, test);
|
||||
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];
|
||||
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;
|
||||
|
|
@ -132,10 +149,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
|
|||
src[Z] = 1;
|
||||
|
||||
rotateVector(matrix, src, test);
|
||||
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];
|
||||
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;
|
||||
|
|
@ -143,10 +160,10 @@ static void testCW(sensor_align_e rotation, int32_t angle)
|
|||
src[Z] = rand() % 5;
|
||||
|
||||
rotateVector(matrix, src, test);
|
||||
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];
|
||||
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];
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -169,11 +186,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
|
|||
initZAxisRotation(matrix, angle);
|
||||
rotateVector(matrix, test, test);
|
||||
|
||||
alignSensors(src, rotation);
|
||||
alignSensorViaMatrixFromRotation(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];
|
||||
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;
|
||||
|
|
@ -185,11 +202,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
|
|||
initZAxisRotation(matrix, angle);
|
||||
rotateVector(matrix, test, test);
|
||||
|
||||
alignSensors(src, rotation);
|
||||
alignSensorViaMatrixFromRotation(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];
|
||||
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;
|
||||
|
|
@ -201,11 +218,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
|
|||
initZAxisRotation(matrix, angle);
|
||||
rotateVector(matrix, test, test);
|
||||
|
||||
alignSensors(src, rotation);
|
||||
alignSensorViaMatrixFromRotation(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];
|
||||
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;
|
||||
|
|
@ -217,11 +234,11 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle)
|
|||
initZAxisRotation(matrix, angle);
|
||||
rotateVector(matrix, test, test);
|
||||
|
||||
alignSensors(src, rotation);
|
||||
alignSensorViaMatrixFromRotation(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];
|
||||
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];
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -265,3 +282,137 @@ TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
|
|||
{
|
||||
testCWFlip(CW270_DEG_FLIP, 270);
|
||||
}
|
||||
|
||||
static void testBuildAlignmentWithStandardAlignment(sensor_align_e alignment, sensorAlignment_t expectedSensorAlignment)
|
||||
{
|
||||
sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(6, 6, 6);
|
||||
|
||||
buildAlignmentFromStandardAlignment(&sensorAlignment, alignment);
|
||||
|
||||
for (int i = 0; i < (int)(sizeof(sensorAlignment.raw) / sizeof(sensorAlignment.raw[0])); i++) {
|
||||
EXPECT_EQ(expectedSensorAlignment.raw[i], sensorAlignment.raw[i]) << "Sensor alignment was not updated. alignment: " << alignment;
|
||||
}
|
||||
}
|
||||
|
||||
TEST(AlignSensorTest, AttemptBuildAlignmentWithStandardAlignment)
|
||||
{
|
||||
testBuildAlignmentWithStandardAlignment(CW0_DEG, CUSTOM_ALIGN_CW0_DEG);
|
||||
testBuildAlignmentWithStandardAlignment(CW90_DEG, CUSTOM_ALIGN_CW90_DEG);
|
||||
testBuildAlignmentWithStandardAlignment(CW180_DEG, CUSTOM_ALIGN_CW180_DEG);
|
||||
testBuildAlignmentWithStandardAlignment(CW270_DEG, CUSTOM_ALIGN_CW270_DEG);
|
||||
testBuildAlignmentWithStandardAlignment(CW0_DEG_FLIP, CUSTOM_ALIGN_CW0_DEG_FLIP);
|
||||
testBuildAlignmentWithStandardAlignment(CW90_DEG_FLIP, CUSTOM_ALIGN_CW90_DEG_FLIP);
|
||||
testBuildAlignmentWithStandardAlignment(CW180_DEG_FLIP, CUSTOM_ALIGN_CW180_DEG_FLIP);
|
||||
testBuildAlignmentWithStandardAlignment(CW270_DEG_FLIP, CUSTOM_ALIGN_CW270_DEG_FLIP);
|
||||
}
|
||||
|
||||
TEST(AlignSensorTest, AttemptBuildAlignmentFromCustomAlignment)
|
||||
{
|
||||
sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
|
||||
|
||||
buildAlignmentFromStandardAlignment(&sensorAlignment, ALIGN_CUSTOM);
|
||||
|
||||
sensorAlignment_t expectedSensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
|
||||
|
||||
for (int i = 0; i < (int)(sizeof(sensorAlignment.raw) / sizeof(sensorAlignment.raw[0])); i++) {
|
||||
EXPECT_EQ(expectedSensorAlignment.raw[i], sensorAlignment.raw[i]) << "Custom alignment should not be updated.";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(AlignSensorTest, AttemptBuildAlignmentFromDefaultAlignment)
|
||||
{
|
||||
sensorAlignment_t sensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
|
||||
|
||||
buildAlignmentFromStandardAlignment(&sensorAlignment, ALIGN_DEFAULT);
|
||||
|
||||
sensorAlignment_t expectedSensorAlignment = SENSOR_ALIGNMENT(1, 2, 3);
|
||||
|
||||
for (int i = 0; i < (int)(sizeof(sensorAlignment.raw) / sizeof(sensorAlignment.raw[0])); i++) {
|
||||
EXPECT_EQ(expectedSensorAlignment.raw[i], sensorAlignment.raw[i]) << "Default alignment should not be updated.";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(AlignSensorTest, AlignmentBitmasks)
|
||||
{
|
||||
uint8_t bits;
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW0_DEG);
|
||||
EXPECT_EQ(0x0, bits); // 000000
|
||||
EXPECT_EQ(0, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW90_DEG);
|
||||
EXPECT_EQ(0x1, bits); // 000001
|
||||
EXPECT_EQ(1, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(1, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW180_DEG);
|
||||
EXPECT_EQ(0x2, bits); // 000010
|
||||
EXPECT_EQ(2, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW270_DEG);
|
||||
EXPECT_EQ(0x3, bits); // 000011
|
||||
EXPECT_EQ(3, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(3, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW0_DEG_FLIP);
|
||||
EXPECT_EQ(0x8, bits); // 001000
|
||||
EXPECT_EQ(0, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW90_DEG_FLIP);
|
||||
EXPECT_EQ(0x9, bits); // 001001
|
||||
EXPECT_EQ(1, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(1, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW180_DEG_FLIP);
|
||||
EXPECT_EQ(0xA, bits); // 001010
|
||||
EXPECT_EQ(2, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
|
||||
bits = ALIGNMENT_TO_BITMASK(CW270_DEG_FLIP);
|
||||
EXPECT_EQ(0xB, bits); // 001011
|
||||
EXPECT_EQ(3, ALIGNMENT_YAW_ROTATIONS(bits));
|
||||
EXPECT_EQ(2, ALIGNMENT_PITCH_ROTATIONS(bits));
|
||||
EXPECT_EQ(0, ALIGNMENT_ROLL_ROTATIONS(bits));
|
||||
|
||||
EXPECT_EQ(3, ALIGNMENT_AXIS_ROTATIONS(bits, FD_YAW));
|
||||
EXPECT_EQ(2, ALIGNMENT_AXIS_ROTATIONS(bits, FD_PITCH));
|
||||
EXPECT_EQ(0, ALIGNMENT_AXIS_ROTATIONS(bits, FD_ROLL));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -144,9 +144,9 @@ TEST(SensorGyro, Update)
|
|||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
|
||||
fakeGyroSet(gyroDevPtr, 15, 26, 97);
|
||||
gyroUpdate(currentTimeUs);
|
||||
EXPECT_FLOAT_EQ(10 * gyroDevPtr->scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
|
||||
EXPECT_FLOAT_EQ(20 * gyroDevPtr->scale, gyro.gyroADCf[Y]);
|
||||
EXPECT_FLOAT_EQ(90 * gyroDevPtr->scale, gyro.gyroADCf[Z]);
|
||||
EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADCf[X], 1e-3); // gyroADCf values are scaled
|
||||
EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADCf[Y], 1e-3);
|
||||
EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADCf[Z], 1e-3);
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue