295 lines
11 KiB
C++
295 lines
11 KiB
C++
#include <MPU9250_WE.h>
|
|
#include <Adafruit_BME280.h>
|
|
#include <Wire.h>
|
|
|
|
#define MPU9250_ADDR 0x68
|
|
|
|
MPU9250_WE MPU9250 = MPU9250_WE(MPU9250_ADDR);
|
|
|
|
#define SEALEVELPRESSURE_HPA (1019.7) // Change to current reported reading
|
|
|
|
Adafruit_BME280 altimeter;
|
|
|
|
float net_gravity_accel;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
Wire.begin();
|
|
|
|
if (!MPU9250.init()) {
|
|
Serial.println("MPU9520 is not responding");
|
|
} else {
|
|
Serial.println("MPU9520 is connected");
|
|
}
|
|
|
|
if (!MPU9250.initMagnetometer()) {
|
|
Serial.println("Magnetometer is not responding");
|
|
} else {
|
|
Serial.println("Magnetometer is connected");
|
|
}
|
|
|
|
/*******************************************************************************************************/
|
|
/*MPU9250 General Settings*/
|
|
//MPU9250.autoOffsets(); // Lay the sensor on a level surface and call this function
|
|
|
|
/* Sample rate divider divides the output rate of the gyroscope and accelerometer.
|
|
* Sample rate = Internal sample rate / (1 + divider)
|
|
* It can only be applied if the corresponding DLPF is enabled and 0<DLPF<7!
|
|
* Divider is a number 0...255
|
|
*/
|
|
MPU9250.setSampleRateDivider(5);
|
|
/*End MPU9250 General Settings*/
|
|
/*******************************************************************************************************/
|
|
/*Accelerometer Settings*/
|
|
MPU9250.enableAccDLPF(true); // Enable the digital low pass filter (DLPF) for the accelerometer
|
|
/* Digital low pass filter (DLPF) for the accelerometer, if enabled
|
|
* MPU9250_DPLF_0, MPU9250_DPLF_2, ...... MPU9250_DPLF_7
|
|
* DLPF Bandwidth [Hz] Delay [ms] Output rate [kHz]
|
|
* 0 460 1.94 1
|
|
* 1 184 5.80 1
|
|
* 2 92 7.80 1
|
|
* 3 41 11.80 1
|
|
* 4 20 19.80 1
|
|
* 5 10 35.70 1
|
|
* 6 5 66.96 1
|
|
* 7 460 1.94 1
|
|
*/
|
|
MPU9250.setAccDLPF(MPU9250_DLPF_6); // Set the DLPF for the accelerometer
|
|
/*Accelerometer Range
|
|
* MPU9250_ACC_RANGE_2G 2 g (default)
|
|
* MPU9250_ACC_RANGE_4G 4 g
|
|
* MPU9250_ACC_RANGE_8G 8 g
|
|
* MPU9250_ACC_RANGE_16G 16 g
|
|
*/
|
|
MPU9250.setAccRange(MPU9250_ACC_RANGE_4G);
|
|
/* You can enable or disable the axes for gyroscope and/or accelerometer measurements.
|
|
* By default all axes are enabled. Parameters are:
|
|
* MPU9250_ENABLE_XYZ //all axes are enabled (default)
|
|
* MPU9250_ENABLE_XY0 // X, Y enabled, Z disabled
|
|
* MPU9250_ENABLE_X0Z
|
|
* MPU9250_ENABLE_X00
|
|
* MPU9250_ENABLE_0YZ
|
|
* MPU9250_ENABLE_0Y0
|
|
* MPU9250_ENABLE_00Z
|
|
* MPU9250_ENABLE_000 // all axes disabled
|
|
*/
|
|
//MPU9250.enableAccAxes(MPU9250_ENABLE_XYZ);
|
|
/*End Accelerometer Settings*/
|
|
/*******************************************************************************************************/
|
|
/*Gyroscope Settings*/
|
|
MPU9250.enableGyrDLPF(); // Enable the digital low pass filter (DLPF) for the gyroscope
|
|
/* Digital Low Pass Filter for the gyroscope must be enabled to choose the level.
|
|
* MPU9250_DPLF_0, MPU9250_DPLF_2, ...... MPU9250_DPLF_7
|
|
*
|
|
* DLPF Bandwidth [Hz] Delay [ms] Output Rate [kHz]
|
|
* 0 250 0.97 8
|
|
* 1 184 2.9 1
|
|
* 2 92 3.9 1
|
|
* 3 41 5.9 1
|
|
* 4 20 9.9 1
|
|
* 5 10 17.85 1
|
|
* 6 5 33.48 1
|
|
* 7 3600 0.17 8
|
|
*
|
|
* You achieve lowest noise using level 6
|
|
*/
|
|
MPU9250.setGyrDLPF(MPU9250_DLPF_6); // Set the DLPF for the gyroscope
|
|
/*Gyroscope Range
|
|
* MPU9250_GYRO_RANGE_250 250 degrees per second (default)
|
|
* MPU9250_GYRO_RANGE_500 500 degrees per second
|
|
* MPU9250_GYRO_RANGE_1000 1000 degrees per second
|
|
* MPU9250_GYRO_RANGE_2000 2000 degrees per second
|
|
*/
|
|
MPU9250.setGyrRange(MPU9250_GYRO_RANGE_250);
|
|
/* You can enable or disable the axes for gyroscope and/or accelerometer measurements.
|
|
* By default all axes are enabled. Parameters are:
|
|
* MPU9250_ENABLE_XYZ //all axes are enabled (default)
|
|
* MPU9250_ENABLE_XY0 // X, Y enabled, Z disabled
|
|
* MPU9250_ENABLE_X0Z
|
|
* MPU9250_ENABLE_X00
|
|
* MPU9250_ENABLE_0YZ
|
|
* MPU9250_ENABLE_0Y0
|
|
* MPU9250_ENABLE_00Z
|
|
* MPU9250_ENABLE_000 // all axes disabled
|
|
*/
|
|
//MPU9250.enableGyrAxes(MPU9250_ENABLE_XYZ);
|
|
/*End Gyroscope Settings*/
|
|
/*******************************************************************************************************/
|
|
/*Magnetometer Settings*/
|
|
/*
|
|
* AK8963_PWR_DOWN
|
|
* AK8963_CONT_MODE_8HZ default
|
|
* AK8963_CONT_MODE_100HZ
|
|
* AK8963_FUSE_ROM_ACC_MODE
|
|
*/
|
|
MPU9250.setMagOpMode(AK8963_CONT_MODE_100HZ);
|
|
/*End Magnetometer Settings*/
|
|
|
|
altimeter.begin(0x76);
|
|
|
|
//Serial.println("Getting the Net Gravitational Acceleration");
|
|
//Serial.println("Keep the Accelerometer Still!");
|
|
//delay(1000);
|
|
net_gravity_accel = get_net_gravity_accel();
|
|
//Serial.println(net_gravity_accel);
|
|
}
|
|
|
|
struct IMUFloat {
|
|
xyzFloat accel;
|
|
xyzFloat gyro;
|
|
xyzFloat mag;
|
|
xyzFloat angles;
|
|
};
|
|
|
|
void loop() {
|
|
IMUFloat imu_data;
|
|
imu_data.accel = get_median_accel(10, 2); // Get the median of 10 measurements with 2ms between them
|
|
imu_data.gyro = MPU9250.getGyrValues(); // Automatically applyies the autoOffsets() calculated earlier and relates raw values to degreees per second
|
|
imu_data.mag = MPU9250.getMagValues();
|
|
imu_data.angles = MPU9250.getAngles();
|
|
|
|
Serial.println("Acceleration in m/s/s (x,y,z):");
|
|
Serial.print(imu_data.accel.x); Serial.print(" ");
|
|
Serial.print(imu_data.accel.y); Serial.print(" ");
|
|
Serial.print(imu_data.accel.z); Serial.print(" ");
|
|
Serial.println("");
|
|
|
|
get_accel_without_gravity(imu_data.accel);
|
|
|
|
Serial.println("Rotational Velocity in deg/s (x,y,z):");
|
|
Serial.print(imu_data.gyro.x); Serial.print(" ");
|
|
Serial.print(imu_data.gyro.y); Serial.print(" ");
|
|
Serial.print(imu_data.gyro.z); Serial.print(" ");
|
|
Serial.println("");
|
|
|
|
Serial.println("Magnetometer data in µTesla (x,y,z):");
|
|
Serial.print(imu_data.mag.x); Serial.print(" ");
|
|
Serial.print(imu_data.mag.y); Serial.print(" ");
|
|
Serial.print(imu_data.mag.z); Serial.print(" ");
|
|
Serial.println();
|
|
Serial.print(" Heading: "); Serial.println(get_mag_heading(imu_data.mag));
|
|
Serial.println("");
|
|
|
|
Serial.println("Angles in deg (x,y,z):");
|
|
Serial.print(imu_data.angles.x); Serial.print(" ");
|
|
Serial.print(imu_data.angles.y); Serial.print(" ");
|
|
Serial.print(imu_data.angles.z); Serial.print(" ");
|
|
Serial.println("");
|
|
|
|
Serial.println("Angles from pitch/roll in deg (x,y):");
|
|
Serial.print(MPU9250.getPitch()); Serial.print(" ");
|
|
Serial.print(MPU9250.getRoll()); Serial.print(" ");
|
|
Serial.println("");
|
|
|
|
Serial.println("Altimeter Data:");
|
|
Serial.print(" Pressure Alt: "); Serial.println(altimeter.readAltitude(1013.25)*3.28084);
|
|
Serial.print(" Indicated Alt: "); Serial.println(altimeter.readAltitude(SEALEVELPRESSURE_HPA)*3.28084);
|
|
Serial.print(" Pressure: "); Serial.print(altimeter.readPressure() / 100.0F);
|
|
Serial.println("");
|
|
|
|
Serial.println("************************************");
|
|
delay(1000);
|
|
}
|
|
|
|
|
|
xyzFloat get_accel_without_gravity(xyzFloat uncorrected_accel) {
|
|
IMUFloat uncorrected_data;
|
|
|
|
uncorrected_data.accel = uncorrected_accel;
|
|
uncorrected_data.angles = MPU9250.getAngles();
|
|
|
|
/*if (uncorrected_data.angles.x > 60) {
|
|
uncorrected_data.angles.x = MPU9250.getPitch();
|
|
}
|
|
if (uncorrected_data.angles.y > 60) {
|
|
uncorrected_data.angles.y = MPU9250.getRoll();
|
|
}*/
|
|
|
|
//float net_gravity_accel = sqrt(uncorrected_data.accel.x*uncorrected_data.accel.x + uncorrected_data.accel.y*uncorrected_data.accel.y + uncorrected_data.accel.z*uncorrected_data.accel.z);
|
|
|
|
xyzFloat corrected_accel;
|
|
corrected_accel.x = uncorrected_data.accel.x - (net_gravity_accel*sin(uncorrected_data.angles.x*0.0174532));
|
|
corrected_accel.x = round_decimals(corrected_accel.x, 2);
|
|
corrected_accel.y = uncorrected_data.accel.y - (net_gravity_accel*sin(uncorrected_data.angles.y*0.0174532));
|
|
corrected_accel.y = round_decimals(corrected_accel.y, 2);
|
|
corrected_accel.z = uncorrected_data.accel.z - (net_gravity_accel*sin(uncorrected_data.angles.z*0.0174532));
|
|
corrected_accel.z = round_decimals(corrected_accel.z, 2);
|
|
|
|
Serial.println("Acceleration without Gravity in m/s/s (x,y,z):");
|
|
Serial.print(corrected_accel.x); Serial.print(" ");
|
|
Serial.print(corrected_accel.y); Serial.print(" ");
|
|
Serial.print(corrected_accel.z); Serial.print(" ");
|
|
Serial.println("");
|
|
|
|
return corrected_accel;
|
|
}
|
|
|
|
|
|
xyzFloat get_average_accel(int num_samples, int millis_delay) {
|
|
/*
|
|
* This function helps reduced the inaccuracy of the accelerometer by averaging readings
|
|
* It returns an xyzFloat struct as defined in the MPU9250 library
|
|
* num_samples is the number of readings from the accelerometer to take
|
|
* millis_delay is the delay in between readings
|
|
*/
|
|
|
|
xyzFloat reading; // Temporary struct to store each reading
|
|
float x_sum, y_sum, z_sum; // Floats to tally up the total value of the readings to then divide by the total number of readings
|
|
|
|
for (int i = 0; i < num_samples; i++) {
|
|
reading = MPU9250.getAccValues();
|
|
x_sum += reading.x;
|
|
y_sum += reading.y;
|
|
z_sum += reading.z;
|
|
|
|
delay(millis_delay);
|
|
}
|
|
|
|
xyzFloat average_readings; // xyzFloat struct to store and return the averages
|
|
|
|
average_readings.x = x_sum / num_samples;
|
|
average_readings.y = y_sum / num_samples;
|
|
average_readings.z = z_sum / num_samples;
|
|
|
|
return average_readings;
|
|
}
|
|
|
|
|
|
float get_net_gravity_accel() {
|
|
xyzFloat accel = get_median_accel(50, 20); // Get the median of 50 measurements with 20ms between them
|
|
|
|
return sqrt(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
|
|
}
|
|
|
|
|
|
float get_mag_heading(xyzFloat mag_readings) {
|
|
return atan2(mag_readings.y*0.01, mag_readings.x*0.01)*180/3.14169;
|
|
}
|
|
|
|
|
|
float round_decimals(float num, int places) {
|
|
// num is the original number to be rounded
|
|
// places is the number of figures after the decimal place to keep
|
|
|
|
float new_num = num * pow(10, places);
|
|
new_num += (0.5 * pow(10, -1*(places-1)));
|
|
|
|
return float(int(new_num) / pow(10, places));
|
|
}
|
|
|
|
|
|
void round_to_zero(float *num, float *margin) {
|
|
// num is the original number to be rounded to zero if the number is within an absolute distance "margin" away from zero
|
|
|
|
if (*num >= -1*(*margin) && *num <= *margin) {
|
|
*num = 0.0;
|
|
}
|
|
|
|
/*How to call it
|
|
float number = 0.07;
|
|
float margin = 0.05;
|
|
round_to_zero(&number, &margin);
|
|
Serial.println(number);
|
|
*/
|
|
}
|