ank/RBE2002
1
Fork 0
Code Issues Activity

complimentary filter

Browse source
This commit is contained in:
Andy Killorin 2024-11-11 13:33:22 -05:00
parent 86549f3093
commit b50d69a2ae
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 28 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -31,7 +31,13 @@ bool LSM6::checkForNewData(void)
bool retVal = false;
if(getStatus() & 0x02)
{
read();
readGyro();
retVal = true;
}
if(getStatus() & 0x01)
{
readAcc();
retVal = true;
}
@ -232,7 +238,7 @@ void LSM6::enableDefault(void)
// Set the accelerometer full scale and data rate
setFullScaleAcc(ACC_FS2);
setAccDataOutputRate(ODR13);
setAccDataOutputRate(ODR104);
// Auto-increment
writeReg(CTRL3_C, 0x04);

22
src/robot.cpp
View file

@ -81,11 +81,29 @@ void Robot::HandleOrientationUpdate(void)
else // update orientation
{
float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
z_rot /= 104;
z_rot *= 245;
z_rot /= 104.0;
z_rot *= 245.0;
eulerAngles.z += z_rot;
}
float g_lsb = 0.061 / 1000.;
float acc_x = asin(imu.a.x * g_lsb) * 180 / PI;
if (isnan(acc_x)) {
acc_x = 90; // pointing straight down will never occur
}
float gyrodelt_x = (float)imu.g.x / (float)INT_MAX;
gyrodelt_x *= 245.0 / 104.0;
float kappa = 0.1;
eulerAngles.x = (1-kappa) * (eulerAngles.x + gyrodelt_x) + kappa * acc_x;
float acc_y = asin(imu.a.y * g_lsb) * 180 / PI;
if (isnan(acc_y)) {
acc_y = 90; // pointing straight down will never occur
}
float gyrodelt_y = (float)imu.g.y / (float)INT_MAX;
gyrodelt_y *= 245.0 / 104.0;
eulerAngles.y = (1-kappa) * (eulerAngles.y + gyrodelt_y) + kappa * acc_y;
#ifdef __IMU_DEBUG__
Serial.print(imu.gyroBias.z);
Serial.print('\t');