From d167cc4d9198f84cac106ee7856c8195e39b9814 Mon Sep 17 00:00:00 2001 From: youhy Date: Mon, 21 Feb 2022 20:48:21 -0800 Subject: [PATCH] imu all gravity direction --- examples/imu/poseEstimate.cc | 11 ++- shared/libraries/pose.cc | 136 ++++++++++++++++++++++++++++------- shared/libraries/pose.h | 14 +++- 3 files changed, 132 insertions(+), 29 deletions(-) diff --git a/examples/imu/poseEstimate.cc b/examples/imu/poseEstimate.cc index 70f7ec7..f0e078b 100644 --- a/examples/imu/poseEstimate.cc +++ b/examples/imu/poseEstimate.cc @@ -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; diff --git a/shared/libraries/pose.cc b/shared/libraries/pose.cc index c4d0f29..3d279d9 100644 --- a/shared/libraries/pose.cc +++ b/shared/libraries/pose.cc @@ -29,6 +29,9 @@ // 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 { @@ -36,6 +39,8 @@ 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 @@ -48,6 +53,7 @@ void Pose::PoseInit(void) { timestamp = imu->timestamp; roll = 0; pitch = 0; + yaw = 0; } void Pose::Calibrate(void) { @@ -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; @@ -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; @@ -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. @@ -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, @@ -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) { @@ -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(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(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(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(yaw, -PI, PI); + } + // update timestamp timestamp = imu->timestamp; } diff --git a/shared/libraries/pose.h b/shared/libraries/pose.h index bd7d6c1..39f2ac3 100644 --- a/shared/libraries/pose.h +++ b/shared/libraries/pose.h @@ -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 @@ -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 @@ -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