249 lines
9.2 KiB
C++
249 lines
9.2 KiB
C++
#include <MPU9250_WE.h>
|
|
#include <Adafruit_BME280.h>
|
|
#include <Wire.h>
|
|
|
|
#define MPU9250_ADDR 0x68
|
|
|
|
MPU9250_WE MPU9250 = MPU9250_WE(MPU9250_ADDR);
|
|
|
|
|
|
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*/
|
|
|
|
//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 gravity_corrected_accel;
|
|
float absolute_z_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();
|
|
imu_data.gravity_corrected_accel = get_accel_without_gravity(imu_data.accel, imu_data.angles);
|
|
|
|
/*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();
|
|
|
|
Serial.println("Acceleration without Gravity in m/s/s (x,y,z):");
|
|
Serial.print(imu_data.gravity_corrected_accel.x); Serial.print(" ");
|
|
Serial.print(imu_data.gravity_corrected_accel.y); Serial.print(" ");
|
|
Serial.print(imu_data.gravity_corrected_accel.z); Serial.print(" ");
|
|
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();
|
|
Serial.println("************************************");
|
|
*/
|
|
|
|
//Serial.print(imu_data.gravity_corrected_accel.x); Serial.print(", ");
|
|
//Serial.print(imu_data.gravity_corrected_accel.y); Serial.print(", ");
|
|
Serial.print(imu_data.angles.z); Serial.print(", ");
|
|
Serial.print(imu_data.accel.z); Serial.print(", ");
|
|
Serial.print(imu_data.gravity_corrected_accel.z); Serial.println(", ");
|
|
//delay(1000);
|
|
}
|
|
|
|
|
|
float get_absolute_z_accel(xyzFloat accel_without_gravity) {
|
|
|
|
}
|
|
|
|
|
|
xyzFloat get_angles_correct() {
|
|
|
|
}
|
|
|
|
|
|
xyzFloat get_accel_without_gravity(xyzFloat uncorrected_accel, xyzFloat angles) {
|
|
IMUFloat uncorrected_data;
|
|
|
|
uncorrected_data.accel = uncorrected_accel;
|
|
uncorrected_data.angles = angles;
|
|
|
|
//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_to_zero(round_decimals(corrected_accel.x, 2), 0.1);
|
|
corrected_accel.y = uncorrected_data.accel.y - (net_gravity_accel*sin(uncorrected_data.angles.y*0.0174532));
|
|
corrected_accel.y = round_to_zero(round_decimals(corrected_accel.y, 2), 0.1);
|
|
corrected_accel.z = uncorrected_data.accel.z - (net_gravity_accel*sin(uncorrected_data.angles.z*0.0174532));
|
|
corrected_accel.z = round_to_zero(round_decimals(corrected_accel.z, 2), 0.1);
|
|
|
|
return corrected_accel;
|
|
}
|
|
|
|
xyzFloat get_median_accel(int num_samples, int millis_delay) {
|
|
// num_samples is the total samples to take with a delay of "millis_delay" milliseconds between them
|
|
|
|
xyzFloat array_values[num_samples];
|
|
|
|
for (int i = 0; i < num_samples; i++) {
|
|
array_values[i] = MPU9250.getAccValues();
|
|
delay(millis_delay);
|
|
}
|
|
return array_values[num_samples/2];
|
|
}
|
|
|
|
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 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));
|
|
}
|
|
|
|
float 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) {
|
|
return 0.0;
|
|
}
|
|
return num;
|
|
}
|