99 lines
2.5 KiB
C++
99 lines
2.5 KiB
C++
//CONSTANTS
|
|
const float alpha = 0.95;
|
|
const int acc_overflow_value = 65535;
|
|
const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
|
|
|
|
|
|
//IMU VARIABLES
|
|
int ax_, ay_, az_;
|
|
int ax, ay, az;
|
|
int cx_, cy_, cz_;
|
|
int gx_, gy_, gz_;
|
|
int gx, gy, gz;
|
|
//float gt;
|
|
float acc_pitch;
|
|
float pitch_rate;
|
|
float dAngle, estAngle;
|
|
float pitch = 0;
|
|
float pitch_prev = 0;
|
|
|
|
|
|
void readIMU() {
|
|
ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() );
|
|
ay_ = IMU.accelerometer_y( IMU.readFromAccelerometer() );
|
|
az_ = IMU.accelerometer_z( IMU.readFromAccelerometer() );
|
|
|
|
// 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
|
|
ax = convertInt(ax_, acc_overflow_value);
|
|
ay = convertInt(ay_, acc_overflow_value);
|
|
az = convertInt(az_, acc_overflow_value);
|
|
|
|
|
|
//Convert gyro
|
|
// Gyroscope coordinate system
|
|
// gx - Pitch rate
|
|
// gy - Roll rate
|
|
// gz - Yaw rate
|
|
// Gyro is calibrated for +-2000deg/s
|
|
// Conversion is happening in GY_85.h line 48-50
|
|
gx = convertInt(gx_, gyro_overflow_value);
|
|
gy = convertInt(gy_, gyro_overflow_value);
|
|
gz = convertInt(gz_, gyro_overflow_value);
|
|
|
|
|
|
// Pitch angle from accelerometer
|
|
acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI;
|
|
|
|
|
|
//Pitch rate from gyroscope
|
|
pitch_rate = -gx;
|
|
|
|
|
|
//Complementary filter
|
|
dAngle = pitch_rate * dT * pow(10, -6);
|
|
pitch = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha);
|
|
pitch_prev = pitch;
|
|
|
|
|
|
//Serial plotter
|
|
// Serial.print ( "Pitch:" );
|
|
// Serial.print ( pitch );
|
|
// Serial.print (" ");
|
|
// Serial.print ( "Accelerometer_Pitch:" );
|
|
// Serial.print ( acc_pitch );
|
|
// Serial.print (" ");
|
|
// Serial.print ( "," );
|
|
// Serial.println ( gz );
|
|
// Serial.print ( "," );
|
|
// Serial.println ( gt );
|
|
// Serial.print ( "," );
|
|
// Serial.println ( acc_pitch);
|
|
}
|
|
|
|
|
|
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.
|
|
int conv_val;
|
|
|
|
if (raw > (overflow_value_ / 2)) {
|
|
conv_val = (raw - overflow_value_);
|
|
}
|
|
|
|
else {
|
|
conv_val = raw;
|
|
}
|
|
|
|
return conv_val;
|
|
|
|
}
|