Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow non-standard IMU orientation (mostly for Bolt) #829

Merged
merged 4 commits into from
Sep 29, 2021
Merged

Allow non-standard IMU orientation (mostly for Bolt) #829

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4efcd2c
Allow non-standard IMU orientation (mostly for Bolt)
matejkarasek Aug 17, 2021
9e575dc
Cleanup
matejkarasek Aug 17, 2021
f289930
Formating cleanup
matejkarasek Aug 19, 2021
e30dd86
Merge branch 'master' into IMU_orientation
matejkarasek Sep 24, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions src/hal/interface/imu.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,17 @@ bool imu6IsCalibrated(void);
bool imuHasBarometer(void);
bool imuHasMangnetometer(void);

// Euler angles defining IMU orientation on the airframe
#ifndef IMU_PHI
#define IMU_PHI 0.0f
#endif
#ifndef IMU_THETA
#define IMU_THETA 0.0f
#endif
#ifndef IMU_PSI
#define IMU_PSI 0.0f
#endif



#endif /* IMU_H_ */
52 changes: 44 additions & 8 deletions src/hal/src/sensors_bmi088_bmp388.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,16 @@ static void applyAxis3fLpf(lpf2pData *data, Axis3f* in);
static bool isBarometerPresent = false;
static uint8_t baroMeasDelayMin = SENSORS_DELAY_BARO;

// Precalculated values for IMU alignment
static float sphi = sinf(IMU_PHI * (float) M_PI / 180);
static float cphi = cosf(IMU_PHI * (float) M_PI / 180);
static float stheta = sinf(IMU_THETA * (float) M_PI / 180);
static float ctheta = cosf(IMU_THETA * (float) M_PI / 180);
static float spsi = sinf(IMU_PSI * (float) M_PI / 180);
static float cpsi = cosf(IMU_PSI * (float) M_PI / 180);

static float R[3][3];

// Pre-calculated values for accelerometer alignment
static float cosPitch;
static float sinPitch;
Expand All @@ -162,6 +172,7 @@ static void sensorsCalculateVarianceAndMean(BiasObj* bias, Axis3f* varOut, Axis3
static void sensorsCalculateBiasMean(BiasObj* bias, Axis3i32* meanOut);
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z);
static bool sensorsFindBiasValue(BiasObj* bias);
static void sensorsAlignToAirframe(Axis3f* in, Axis3f* out);
static void sensorsAccAlignToGravity(Axis3f* in, Axis3f* out);

STATIC_MEM_TASK_ALLOC(sensorsTask, SENSORS_TASK_STACKSIZE);
Expand Down Expand Up @@ -278,6 +289,8 @@ static void sensorsTask(void *param)
{
systemWaitStart();

Axis3f gyroScaledIMU;
Axis3f accScaledIMU;
Axis3f accScaled;
measurement_t measurement;
/* wait an additional second the keep bus free
Expand All @@ -304,20 +317,23 @@ static void sensorsTask(void *param)
{
processAccScale(accelRaw.x, accelRaw.y, accelRaw.z);
}

/* Gyro */
sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_BMI088_DEG_PER_LSB_CFG;
sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_BMI088_DEG_PER_LSB_CFG;
sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_BMI088_DEG_PER_LSB_CFG;
gyroScaledIMU.x = (gyroRaw.x - gyroBias.x) * SENSORS_BMI088_DEG_PER_LSB_CFG;
gyroScaledIMU.y = (gyroRaw.y - gyroBias.y) * SENSORS_BMI088_DEG_PER_LSB_CFG;
gyroScaledIMU.z = (gyroRaw.z - gyroBias.z) * SENSORS_BMI088_DEG_PER_LSB_CFG;
sensorsAlignToAirframe(&gyroScaledIMU, &sensorData.gyro);
applyAxis3fLpf((lpf2pData*)(&gyroLpf), &sensorData.gyro);

measurement.type = MeasurementTypeGyroscope;
measurement.data.gyroscope.gyro = sensorData.gyro;
estimatorEnqueue(&measurement);

/* Accelerometer */
accScaled.x = accelRaw.x * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
accScaled.y = accelRaw.y * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
accScaled.z = accelRaw.z * SENSORS_BMI088_G_PER_LSB_CFG / accScale;

/* Acelerometer */
accScaledIMU.x = accelRaw.x * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
accScaledIMU.y = accelRaw.y * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
accScaledIMU.z = accelRaw.z * SENSORS_BMI088_G_PER_LSB_CFG / accScale;
sensorsAlignToAirframe(&accScaledIMU, &accScaled);
sensorsAccAlignToGravity(&accScaled, &sensorData.acc);
applyAxis3fLpf((lpf2pData*)(&accLpf), &sensorData.acc);

Expand Down Expand Up @@ -858,6 +874,26 @@ bool sensorsBmi088Bmp388ManufacturingTest(void)
return testStatus;
}

/**
* Align the sensors to the Airframe axes
*/
static void sensorsAlignToAirframe(Axis3f* in, Axis3f* out)
{
R[0][0] = ctheta * cpsi;
R[0][1] = ctheta * spsi;
R[0][2] = -stheta;
R[1][0] = sphi * stheta * cpsi - cphi * spsi;
R[1][1] = sphi * stheta * spsi + cphi * cpsi;
R[1][2] = sphi * ctheta;
R[2][0] = cphi * stheta * cpsi + sphi * spsi;
R[2][1] = cphi * stheta * spsi - sphi * cpsi;
R[2][2] = cphi * ctheta;

out->x = in->x*R[0][0] + in->y*R[0][1] + in->z*R[0][2];
out->y = in->x*R[1][0] + in->y*R[1][1] + in->z*R[1][2];
out->z = in->x*R[2][0] + in->y*R[2][1] + in->z*R[2][2];
}

/**
* Compensate for a miss-aligned accelerometer. It uses the trim
* data gathered from the UI and written in the config-block to
Expand Down
47 changes: 41 additions & 6 deletions src/hal/src/sensors_mpu9250_lps25h.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,16 @@ static bool isMpu6500TestPassed = false;
static bool isAK8963TestPassed = false;
static bool isLPS25HTestPassed = false;

// Precalculated values for IMU alignment
static float sphi = sinf(IMU_PHI * (float) M_PI / 180);
static float cphi = cosf(IMU_PHI * (float) M_PI / 180);
static float stheta = sinf(IMU_THETA * (float) M_PI / 180);
static float ctheta = cosf(IMU_THETA * (float) M_PI / 180);
static float spsi = sinf(IMU_PSI * (float) M_PI / 180);
static float cpsi = cosf(IMU_PSI * (float) M_PI / 180);

static float R[3][3];

// Pre-calculated values for accelerometer alignment
static float cosPitch;
static float sinPitch;
Expand All @@ -173,6 +183,7 @@ static void sensorsCalculateVarianceAndMean(BiasObj* bias, Axis3f* varOut, Axis3
static void sensorsCalculateBiasMean(BiasObj* bias, Axis3i32* meanOut);
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z);
static bool sensorsFindBiasValue(BiasObj* bias);
static void sensorsAlignToAirframe(Axis3f* in, Axis3f* out);
static void sensorsAccAlignToGravity(Axis3f* in, Axis3f* out);

STATIC_MEM_TASK_ALLOC(sensorsTask, SENSORS_TASK_STACKSIZE);
Expand Down Expand Up @@ -307,6 +318,8 @@ void processMagnetometerMeasurements(const uint8_t *buffer)

void processAccGyroMeasurements(const uint8_t *buffer)
{
Axis3f gyroScaledIMU;
Axis3f accScaledIMU;
Axis3f accScaled;
// Note the ordering to correct the rotated 90º IMU coordinate system
accelRaw.y = (((int16_t) buffer[0]) << 8) | buffer[1];
Expand All @@ -327,14 +340,16 @@ void processAccGyroMeasurements(const uint8_t *buffer)
processAccScale(accelRaw.x, accelRaw.y, accelRaw.z);
}

sensorData.gyro.x = -(gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
gyroScaledIMU.x = -(gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
gyroScaledIMU.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
gyroScaledIMU.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
sensorsAlignToAirframe(&gyroScaledIMU, &sensorData.gyro);
applyAxis3fLpf((lpf2pData*)(&gyroLpf), &sensorData.gyro);

accScaled.x = -(accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
accScaled.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
accScaled.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
accScaledIMU.x = -(accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
accScaledIMU.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
accScaledIMU.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
sensorsAlignToAirframe(&accScaledIMU, &accScaled);
sensorsAccAlignToGravity(&accScaled, &sensorData.acc);
applyAxis3fLpf((lpf2pData*)(&accLpf), &sensorData.acc);
}
Expand Down Expand Up @@ -869,6 +884,26 @@ void __attribute__((used)) EXTI13_Callback(void)
}
}

/**
* Align the sensors to the Airframe axes
*/
static void sensorsAlignToAirframe(Axis3f* in, Axis3f* out)
{
R[0][0] = ctheta * cpsi;
R[0][1] = ctheta * spsi;
R[0][2] = -stheta;
R[1][0] = sphi * stheta * cpsi - cphi * spsi;
R[1][1] = sphi * stheta * spsi + cphi * cpsi;
R[1][2] = sphi * ctheta;
R[2][0] = cphi * stheta * cpsi + sphi * spsi;
R[2][1] = cphi * stheta * spsi - sphi * cpsi;
R[2][2] = cphi * ctheta;

out->x = in->x*R[0][0] + in->y*R[0][1] + in->z*R[0][2];
out->y = in->x*R[1][0] + in->y*R[1][1] + in->z*R[1][2];
out->z = in->x*R[2][0] + in->y*R[2][1] + in->z*R[2][2];
}

/**
* Compensate for a miss-aligned accelerometer. It uses the trim
* data gathered from the UI and written in the config-block to
Expand Down
5 changes: 5 additions & 0 deletions tools/make/config.mk.example
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,3 +111,8 @@
## Lighthouse handling
# If lighthouse will need to act as a ground truth (so not entered in the kalman filter)
# CFLAGS += -DLIGHTHOUSE_AS_GROUNDTRUTH

## Euler angles defining nonstandard IMU orientation on the airframe (in degrees)
# CFLAGS += -DIMU_PHI=0.0f
# CFLAGS += -DIMU_THETA=0.0f
# CFLAGS += -DIMU_PSI=0.0f