Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

IMU cleanup

This commit is contained in:
Stedd 2019-12-22 15:48:26 +01:00
parent 88bc161a84
commit c546c757c9
1 changed files with 21 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
Main/IMU.ino
View File

@ -5,12 +5,10 @@ const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
//IMU VARIABLES //IMU VARIABLES
int ax_, ay_, az_;
int ax, ay, az; int ax, ay, az;
int cx_, cy_, cz_; int cx, cy, cz;
int gx_, gy_, gz_;
int gx, gy, gz; int gx, gy, gz;
//float gt; float gt;
float acc_pitch; float acc_pitch;
float pitch_rate; float pitch_rate;
float dAngle, estAngle; float dAngle, estAngle;
@ -19,36 +17,32 @@ float pitch_prev = 0;
void readIMU() { void readIMU() {
ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() ); //Acceletometer
ay_ = IMU.accelerometer_y( IMU.readFromAccelerometer() ); ax = convertInt(IMU.accelerometer_x( IMU.readFromAccelerometer() ), acc_overflow_value);
az_ = IMU.accelerometer_z( IMU.readFromAccelerometer() ); ay = convertInt(IMU.accelerometer_y( IMU.readFromAccelerometer() ), acc_overflow_value);
az = convertInt(IMU.accelerometer_z( IMU.readFromAccelerometer() ), acc_overflow_value);
// cx = IMU.compass_x( IMU.readFromCompass() );
// cy = IMU.compass_y( IMU.readFromCompass() );
// cz = IMU.compass_z( IMU.readFromCompass() );
gx_ = IMU.gyro_x( IMU.readGyro() );
gy_ = IMU.gyro_y( IMU.readGyro() );
gz_ = IMU.gyro_z( IMU.readGyro() );
// gt = IMU.temp ( IMU.readGyro() );
//Convert accelerometer //Magnetometer
ax = convertInt(ax_, acc_overflow_value); cx = IMU.compass_x( IMU.readFromCompass() );
ay = convertInt(ay_, acc_overflow_value); cy = IMU.compass_y( IMU.readFromCompass() );
az = convertInt(az_, acc_overflow_value); cz = IMU.compass_z( IMU.readFromCompass() );
//Convert gyro // Gyrocope
// Gyroscope coordinate system // Coordinate system
// gx - Pitch rate // gx - Pitch rate
// gy - Roll rate // gy - Roll rate
// gz - Yaw rate // gz - Yaw rate
// Gyro is calibrated for +-2000deg/s // Gyro is calibrated for +-2000deg/s
// Conversion is happening in GY_85.h line 48-50 // Conversion is happening in GY_85.h line 48-50
gx = convertInt(gx_, gyro_overflow_value); gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value);
gy = convertInt(gy_, gyro_overflow_value); gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value);
gz = convertInt(gz_, gyro_overflow_value); gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value);
//Temperature sensor
gt = IMU.temp ( IMU.readGyro() );
// Pitch angle from accelerometer // Pitch angle from accelerometer
@ -83,16 +77,12 @@ void readIMU() {
int convertInt(int raw, int overflow_value_) { int convertInt(int raw, int overflow_value_) {
//For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK.
int conv_val;
if (raw > (overflow_value_ / 2)) { if (raw > (overflow_value_ / 2)) {
conv_val = (raw - overflow_value_); return (raw - overflow_value_);
} }
else { else {
conv_val = raw; return raw;
} }
return conv_val;
} }