70 lines
1.8 KiB
C++
70 lines
1.8 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 cx, cy, cz;
|
|
float gx, gy, gz;
|
|
float gt;
|
|
float acc_pitch;
|
|
float pitch_rate;
|
|
float pitch = 0;
|
|
float pitch_prev = 0;
|
|
|
|
|
|
void readIMU() {
|
|
// Serial.println("ReadingIMU");
|
|
//Acceletometer
|
|
int* accelerometerReadings = IMU.readFromAccelerometer();
|
|
ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value);
|
|
ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value);
|
|
az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value);
|
|
|
|
|
|
//Magnetometer
|
|
int* compassReadings = IMU.readFromCompass();
|
|
cx = IMU.compass_x(compassReadings);
|
|
cy = IMU.compass_y(compassReadings);
|
|
cz = IMU.compass_z(compassReadings);
|
|
|
|
|
|
// // Gyrocope
|
|
// float* gyroReadings = IMU.readGyro();
|
|
// gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate
|
|
// gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate
|
|
// gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate
|
|
|
|
|
|
// //Temperature sensor
|
|
// gt = IMU.temp(gyroReadings);
|
|
|
|
|
|
// 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
|
|
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
|
|
pitch_prev = 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.
|
|
|
|
if (raw > (overflow_value_ / 2)) {
|
|
return (raw - overflow_value_);
|
|
}
|
|
|
|
else {
|
|
return raw;
|
|
}
|
|
}
|