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

Support IMU to work on any install orientation #51

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
+208 −22
Open

Support IMU to work on any install orientation #51

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
d167cc4
imu all gravity direction
AzulRadio Feb 22, 2022
1f8a632
check format
AzulRadio Feb 26, 2022
de18115
add negative gravity direction
AzulRadio May 20, 2022
d78b053
add gravity direction comment
AzulRadio May 20, 2022
f77f6bc
code format check
AzulRadio May 20, 2022
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
15 changes: 15 additions & 0 deletions examples/imu/poseEstimate.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,21 @@ 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;
const uint8_t X_NEG = 3;
// const uint8_t Y_NEG = 4;
// const uint8_t Z_NEG = 5;

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

// init params
float roll = 0;
float pitch = 0;
Expand Down
196 changes: 174 additions & 22 deletions shared/libraries/pose.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,21 @@

// 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;
static const uint8_t X_NEG = 3;
static const uint8_t Y_NEG = 4;
static const uint8_t Z_NEG = 5;

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 +56,7 @@ void Pose::PoseInit(void) {
timestamp = imu->timestamp;
roll = 0;
pitch = 0;
yaw = 0;
}

void Pose::Calibrate(void) {
Expand All @@ -58,6 +67,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 +76,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 +85,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 +94,65 @@ 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;
case X_NEG:
acc_x_off = 0;
break;
case Y_NEG:
acc_y_off = 0;
break;
case Z_NEG:
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;
case X_NEG:
acce += imu->acce.x;
break;
case Y_NEG:
acce += imu->acce.y;
break;
case Z_NEG:
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 || _gd == X_NEG || _gd == Y_NEG || _gd == Z_NEG) {
gravityDir = _gd;
}
}

void Pose::SetOffset(float _acc_x_off, float _acc_y_off, float _acc_z_off, float _gyro_x_off,
Expand All @@ -123,24 +179,120 @@ 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);
}
if (gravityDir == X_NEG) {
// 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_NEG) {
// 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_NEG) {
// 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
19 changes: 19 additions & 0 deletions shared/libraries/pose.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,18 @@ class Pose {
**/
float GetGravity(void);

/** @description: set direction for gravity
* @param : 0 = x, 1 = y, 2 = z; 3 = x_neg; 4 = y_neg, 5 = z_neg
* @note: Direction is based on the "R" marker on the A-type board.
* y z
* |/
* R -- x
* Gravity direction is when the axis is pointing to the ground
* at default postition.
* (Z is pointing inwards with respect to the screen.)
**/
void SetGravityDir(uint8_t);

/** @description: set offset (bias correction) for the 6 inputs
* @param: Offset (Bias) for the 3 axis of acce and gyro
* @note: The mean of all 6 axis except acc_z_off should be 0 after setting this.
Expand Down Expand Up @@ -117,6 +129,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