Skip to content

Commit

Permalink
imu all gravity direction
Browse files Browse the repository at this point in the history
  • Loading branch information
@AzulRadio
AzulRadio committed Feb 22, 2022
1 parent b1b71c0 commit cc6ee9a
Show file tree
Hide file tree
Showing 3 changed files with 132 additions and 29 deletions.
11 changes: 10 additions & 1 deletion examples/imu/poseEstimate.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,16 @@ void RM_RTOS_Default_Task(const void* arguments) {

// Set alpha for the complementary filter in the pose estimator
poseEstimator.SetAlpha(0.95);


//const uint8_t X = 0;
//const uint8_t Y = 1;
const uint8_t Z = 2;

// Set Gravity Direction
//poseEstimator.SetGravityDir(X);
//poseEstimator.SetGravityDir(Y);
poseEstimator.SetGravityDir(Z);

// init params
float roll = 0;
float pitch = 0;
Expand Down
136 changes: 109 additions & 27 deletions shared/libraries/pose.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,18 @@

// Factor from us to s
static const float USEC_TO_SEC = 1000000.0;
static const uint8_t X = 0;
static const uint8_t Y = 1;
static const uint8_t Z = 2;

namespace control {

Pose::Pose(bsp::MPU6500* _imu) : imu(_imu) {
if (_imu == nullptr) {
RM_ASSERT_TRUE(false, "invalid imu");
}
gravityDir = Z;

// Set weight for gyro data in complementary filter to 0.98. This is an arbitrary design.
SetAlpha(0.98);
// Set all IMU offsets to 0
Expand All @@ -48,6 +53,7 @@ void Pose::PoseInit(void) {
timestamp = imu->timestamp;
roll = 0;
pitch = 0;
yaw = 0;
}

void Pose::Calibrate(void) {
Expand All @@ -58,6 +64,7 @@ void Pose::Calibrate(void) {
void Pose::Calibrate(int16_t _num) {
float acce_x = 0;
float acce_y = 0;
float acce_z = 0;
float gyro_x = 0;
float gyro_y = 0;
float gyro_z = 0;
Expand All @@ -66,6 +73,7 @@ void Pose::Calibrate(int16_t _num) {
for (int i = 0; i < _num; i++) {
acce_x += imu->acce.x;
acce_y += imu->acce.y;
acce_z += imu->acce.z;
gyro_x += imu->gyro.x;
gyro_y += imu->gyro.y;
gyro_z += imu->gyro.z;
Expand All @@ -74,6 +82,7 @@ void Pose::Calibrate(int16_t _num) {

acc_x_off = acce_x / (float)_num;
acc_y_off = acce_y / (float)_num;
acc_z_off = acce_z / (float)_num;

/** Explanation: For calibration, we want the mean of noise distribution (bias)
* to be zeroed when IMU is placed on a flat plane.
Expand All @@ -82,21 +91,47 @@ void Pose::Calibrate(int16_t _num) {
* Since the value of the gravity doesn't matter (only ratio matters), we can
* assign the noise bias to the gravity noise bias. So acc_z_off is always zero.
**/
acc_z_off = 0;

switch (gravityDir) {
case X:
acc_x_off = 0;
break;
case Y:
acc_y_off = 0;
break;
case Z:
acc_z_off = 0;
break;
}

gyro_x_off = gyro_x / (float)_num;
gyro_y_off = gyro_y / (float)_num;
gyro_z_off = gyro_z / (float)_num;
}

float Pose::GetGravity(void) {
float acce_z = 0;
float acce = 0;
const int32_t NUM = 100;
for (int i = 0; i < NUM; i++) {
acce_z += imu->acce.z;
switch (gravityDir) {
case X:
acce += imu->acce.x;
break;
case Y:
acce += imu->acce.y;
break;
case Z:
acce += imu->acce.z;
break;
}
osDelay(10);
}
return acce_z / (float)NUM;
return acce / (float)NUM;
}

void Pose::SetGravityDir(uint8_t _gd) {
if (_gd == X || _gd == Y || _gd == Z) {
gravityDir = _gd;
}
}

void Pose::SetOffset(float _acc_x_off, float _acc_y_off, float _acc_z_off, float _gyro_x_off,
Expand All @@ -109,11 +144,17 @@ void Pose::SetOffset(float _acc_x_off, float _acc_y_off, float _acc_z_off, float
gyro_z_off = _gyro_z_off;
}

float Pose::GetPitch(void) { return pitch; }
float Pose::GetPitch(void) {
return pitch;
}

float Pose::GetRoll(void) { return roll; }
float Pose::GetRoll(void) {
return roll;
}

float Pose::GetYaw(void) { return yaw; }
float Pose::GetYaw(void) {
return yaw;
}

void Pose::SetAlpha(float _alpha) {
if (_alpha > 1.0 || _alpha < 0.0) {
Expand All @@ -123,25 +164,66 @@ void Pose::SetAlpha(float _alpha) {
}

void Pose::ComplementaryFilterUpdate(void) {
// compute pitch and roll based on acce meter
pitchAcc = atan2f(imu->acce.y - acc_y_off, imu->acce.z - acc_z_off);
rollAcc = atan2f(imu->acce.x - acc_x_off, imu->acce.z - acc_z_off);

// estimate pose from gyro and acce
pitch = alpha * (pitch +
(imu->gyro.x - gyro_x_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) +
(1.0 - alpha) * pitchAcc;

roll = alpha * (roll +
(imu->gyro.y - gyro_y_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) -
(1.0 - alpha) * rollAcc;

// yaw cannot rely on acce.
// TODO fuse yaw with
yaw = yaw + (imu->gyro.z - gyro_z_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC;
// limit yaw to -Pi to Pi
yaw = wrap<float>(yaw, -PI, PI);

if (gravityDir == X) {
// compute pitch and roll based on acce meter
pitchAcc = atan2f(imu->acce.z - acc_z_off, imu->acce.x - acc_x_off);
rollAcc = atan2f(imu->acce.y - acc_y_off, imu->acce.x - acc_x_off);

// estimate pose from gyro and acce
pitch = alpha * (pitch +
(imu->gyro.y - gyro_y_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) +
(1.0 - alpha) * pitchAcc;

roll = alpha * (roll +
(imu->gyro.z - gyro_z_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) -
(1.0 - alpha) * rollAcc;

// yaw cannot rely on acce.
yaw = yaw + (imu->gyro.x - gyro_x_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC;
// limit yaw to -Pi to Pi
yaw = wrap<float>(yaw, -PI, PI);


} else if (gravityDir == Y) {
// compute pitch and roll based on acce meter
pitchAcc = atan2f(imu->acce.x - acc_x_off, imu->acce.y - acc_y_off);
rollAcc = atan2f(imu->acce.z - acc_z_off, imu->acce.y - acc_y_off);

// estimate pose from gyro and acce
pitch = alpha * (pitch +
(imu->gyro.z - gyro_z_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) +
(1.0 - alpha) * pitchAcc;

roll = alpha * (roll +
(imu->gyro.x - gyro_x_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) -
(1.0 - alpha) * rollAcc;

// yaw cannot rely on acce.
yaw = yaw + (imu->gyro.y - gyro_y_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC;
// limit yaw to -Pi to Pi
yaw = wrap<float>(yaw, -PI, PI);


} else if (gravityDir == Z) {
// compute pitch and roll based on acce meter
pitchAcc = atan2f(imu->acce.y - acc_y_off, imu->acce.z - acc_z_off);
rollAcc = atan2f(imu->acce.x - acc_x_off, imu->acce.z - acc_z_off);

// estimate pose from gyro and acce
pitch = alpha * (pitch +
(imu->gyro.x - gyro_x_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) +
(1.0 - alpha) * pitchAcc;

roll = alpha * (roll +
(imu->gyro.y - gyro_y_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC) -
(1.0 - alpha) * rollAcc;

// yaw cannot rely on acce.
yaw = yaw + (imu->gyro.z - gyro_z_off) * (float)(imu->timestamp - timestamp) / USEC_TO_SEC;
// limit yaw to -Pi to Pi
yaw = wrap<float>(yaw, -PI, PI);
}

// update timestamp
timestamp = imu->timestamp;
}
Expand Down
14 changes: 13 additions & 1 deletion shared/libraries/pose.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ class Pose {
*
**/
void PoseInit(void);

/** @description: naive calibration for all offsets
* read from IMU and compute the average for 100 times
* @param: num, IMU reads to compute the average offset
Expand All @@ -54,6 +54,11 @@ class Pose {
* @note : average 100 measure of z acceleration. Takes 1s.
**/
float GetGravity(void);

/** @description: set direction for gravity
* @param : 0 = x, 1 = y, 2 = z;
**/
void SetGravityDir(uint8_t);

/** @description: set offset (bias correction) for the 6 inputs
* @param: Offset (Bias) for the 3 axis of acce and gyro
Expand Down Expand Up @@ -117,6 +122,13 @@ class Pose {
// pose estimated by acce alone
float pitchAcc;
float rollAcc;
float yawAcc;

// direction of gravity
// 0 = x
// 1 = y
// 2 = z
uint8_t gravityDir;

}; // class Pose end

Expand Down

0 comments on commit cc6ee9a

Please sign in to comment.