ank/RBE2002
1
Fork 0
Code Issues Activity

imu impl

Browse source
This commit is contained in:
Andy Killorin 2024-10-30 14:48:52 -04:00
parent aeea35cce0
commit 86549f3093
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
3 changed files with 23 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
lib/LSM6/src/LSM6.cpp
View file

@ -29,7 +29,7 @@ LSM6::LSM6(void)
bool LSM6::checkForNewData(void) bool LSM6::checkForNewData(void)
{ {
bool retVal = false; bool retVal = false;
if(getStatus() & 0x01) if(getStatus() & 0x02)
{ {
read(); read();
@ -228,7 +228,7 @@ void LSM6::enableDefault(void)
{ {
// Set the gyro full scale and data rate // Set the gyro full scale and data rate
setFullScaleGyro(GYRO_FS245); setFullScaleGyro(GYRO_FS245);
setGyroDataOutputRate(ODR13); setGyroDataOutputRate(ODR104);
// Set the accelerometer full scale and data rate // Set the accelerometer full scale and data rate
setFullScaleAcc(ACC_FS2); setFullScaleAcc(ACC_FS2);

7
lib/LSM6/src/LSM6.h
View file

@ -141,6 +141,12 @@ public:
vector<float> updateGyroBias(void) vector<float> updateGyroBias(void)
{ {
if (initialMeasurement) {
gyroBias.z = g.z;
initialMeasurement = false;
}
float retain = (0.98);
gyroBias.z = g.z * (1- retain) + gyroBias.z * retain;
/** /**
* TODO: add a low-pass filter to update the bias * TODO: add a low-pass filter to update the bias
*/ */
@ -169,6 +175,7 @@ public:
// float estimatedPitchAngle = 0; // float estimatedPitchAngle = 0;
// vector<float> eulerAngles; // vector<float> eulerAngles;
vector<float> gyroBias; vector<float> gyroBias;
bool initialMeasurement = true;
/* We make Robot a friend to avoid all the setters and getters. */ /* We make Robot a friend to avoid all the setters and getters. */
friend class Robot; friend class Robot;

16
src/robot.cpp
View file

@ -1,5 +1,6 @@
#include "robot.h" #include "robot.h"
#include <IRdecoder.h> #include <IRdecoder.h>
#include <limits.h>
void Robot::InitializeRobot(void) void Robot::InitializeRobot(void)
{ {
@ -79,11 +80,22 @@ void Robot::HandleOrientationUpdate(void)
else // update orientation else // update orientation
{ {
// TODO: update the orientation float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
z_rot /= 104;
z_rot *= 245;
eulerAngles.z += z_rot;
} }
#ifdef __IMU_DEBUG__ #ifdef __IMU_DEBUG__
Serial.println(eulerAngles.z); Serial.print(imu.gyroBias.z);
Serial.print('\t');
Serial.print(eulerAngles.z);
Serial.print('\t');
//Serial.print(imu.g.x);
//Serial.print('\t');
//Serial.print(imu.g.y);
//Serial.print('\t');
Serial.println(imu.g.z);
#endif #endif
} }