101 lines
4.5 KiB
C++
101 lines
4.5 KiB
C++
#include "mpu6050.hpp"
|
|
|
|
measurement mpu_offset;
|
|
|
|
void setup_mpu_6050_registers() {
|
|
Wire.begin();
|
|
//Activate the MPU-6050
|
|
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
|
Wire.write(0x6B); //Send the requested starting register
|
|
Wire.write(0x00); //Set the requested starting register
|
|
Wire.endTransmission();
|
|
//Configure the accelerometer (+/-8g)
|
|
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
|
Wire.write(0x1C); //Send the requested starting register
|
|
Wire.write(0x10); //Set the requested starting register
|
|
Wire.endTransmission();
|
|
//Configure the gyro (500dps full scale)
|
|
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
|
Wire.write(0x1B); //Send the requested starting register
|
|
//Wire.write(0x08); //Set the requested starting register 500°
|
|
Wire.write(0x10); //Set the requested starting register 250°
|
|
Wire.endTransmission();
|
|
//TODO Configure digital low pass filter
|
|
/*Wire.beginTransmission(0x68);
|
|
Wire.write(0x1A);
|
|
Wire.write(0x04);
|
|
Wire.endTransmission();*/
|
|
}
|
|
|
|
void read_mpu_6050_data_with_offset_substraction(measurement &measurement) {
|
|
read_mpu_6050_data(measurement);
|
|
//TODO: check if necessary
|
|
substract_gyroscope_offset(measurement);
|
|
//substract_accelerometer_offset(measurement);
|
|
}
|
|
|
|
void read_mpu_6050_data(measurement &measurement) { //Subroutine for reading the raw gyro and accelerometer data
|
|
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
|
Wire.write(0x3B); //Send the requested starting register
|
|
Wire.endTransmission(); //End the transmission
|
|
Wire.requestFrom(0x68, 14); //Request 14 bytes from the MPU-6050
|
|
while (Wire.available() < 14); //Wait until all the bytes are received
|
|
measurement.acceleration.x = Wire.read() << 8 | Wire.read();
|
|
measurement.acceleration.y = Wire.read() << 8 | Wire.read();
|
|
measurement.acceleration.z = Wire.read() << 8 | Wire.read();
|
|
measurement.temperature = Wire.read() << 8 | Wire.read();
|
|
measurement.gyroscope.x = Wire.read() << 8 | Wire.read();
|
|
measurement.gyroscope.y = Wire.read() << 8 | Wire.read();
|
|
measurement.gyroscope.z = Wire.read() << 8 | Wire.read();
|
|
}
|
|
|
|
void substract_gyroscope_offset(measurement &measurement) {
|
|
measurement.gyroscope.x -= mpu_offset.gyroscope.x;
|
|
measurement.gyroscope.y -= mpu_offset.gyroscope.y;
|
|
measurement.gyroscope.z -= mpu_offset.gyroscope.z;
|
|
}
|
|
|
|
void substract_accelerometer_offset(measurement &measurement) {
|
|
measurement.acceleration.x -= mpu_offset.acceleration.x;
|
|
measurement.acceleration.y -= mpu_offset.acceleration.y;
|
|
measurement.acceleration.z -= mpu_offset.acceleration.z;
|
|
}
|
|
|
|
void calculate_mpu_offsets_as_average() {
|
|
mpu_offset.acceleration.x = 0;
|
|
mpu_offset.acceleration.y = 0;
|
|
mpu_offset.acceleration.z = 0;
|
|
measurement measurement;
|
|
for (int cal_int = 0; cal_int < 1000; cal_int++) { //Read the raw acc and gyro data from the MPU-6050 for 1000 times
|
|
read_mpu_6050_data(measurement);
|
|
mpu_offset.gyroscope.x += measurement.gyroscope.x; //Add the gyro x offset to the gyro_x_cal variable
|
|
mpu_offset.gyroscope.y += measurement.gyroscope.y; //Add the gyro y offset to the gyro_y_cal variable
|
|
mpu_offset.gyroscope.z += measurement.gyroscope.z; //Add the gyro z offset to the gyro_z_cal variable
|
|
mpu_offset.acceleration.x += measurement.acceleration.x;
|
|
mpu_offset.acceleration.y += measurement.acceleration.y;
|
|
mpu_offset.acceleration.z += measurement.acceleration.z;
|
|
delay(3); //Delay 3us to have 250Hz for-loop
|
|
}
|
|
|
|
// get average offset
|
|
mpu_offset.gyroscope.x /= 1000;
|
|
mpu_offset.gyroscope.y /= 1000;
|
|
mpu_offset.gyroscope.z /= 1000;
|
|
mpu_offset.acceleration.x /= 1000;
|
|
mpu_offset.acceleration.y /= 1000;
|
|
mpu_offset.acceleration.z /= 1000;
|
|
}
|
|
|
|
measurement get_mpu_offset() {
|
|
return mpu_offset;
|
|
}
|
|
|
|
void mpu_measurement_with_precission(measurement_double_precission &measurement_dp, measurement const measurement) {
|
|
measurement_dp.acceleration.x = measurement.acceleration.x * ACCELEROMETER_CONST;
|
|
measurement_dp.acceleration.y = measurement.acceleration.y * ACCELEROMETER_CONST;
|
|
measurement_dp.acceleration.z = measurement.acceleration.z * ACCELEROMETER_CONST;
|
|
|
|
measurement_dp.gyroscope.x = measurement.gyroscope.x * GYRO_CONST;
|
|
measurement_dp.gyroscope.y = measurement.gyroscope.y * GYRO_CONST;
|
|
measurement_dp.gyroscope.z = measurement.gyroscope.z * GYRO_CONST;
|
|
}
|