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

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

@ -141,6 +141,12 @@ public:
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
*/
@ -169,6 +175,7 @@ public:
// float estimatedPitchAngle = 0;
// vector<float> eulerAngles;
vector<float> gyroBias;
bool initialMeasurement = true;
/* We make Robot a friend to avoid all the setters and getters. */
friend class Robot;

16
src/robot.cpp
View file

@ -1,5 +1,6 @@
#include "robot.h"
#include <IRdecoder.h>
#include <limits.h>
void Robot::InitializeRobot(void)
{
@ -79,11 +80,22 @@ void Robot::HandleOrientationUpdate(void)
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__
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
}