I currently have a robotics project which is using many (16) IMU's specifically the MPU9250 running under SPI.
As a reduced example of six sensors using the Bolder flight library
int cs[6] = {21, 25, 26, 27, 32, 14}; //chipselects
MPU9250 IMU0(SPI, 21); // Header P5
MPU9250 IMU1(SPI, 25); // Header P6
MPU9250 IMU2(SPI, 26); // Header P7
MPU9250 IMU3(SPI, 27); // Header P9
MPU9250 IMU4(SPI, 32); // Header P10
MPU9250 IMU5(SPI, 12); // Header P11
To use these sensors they all have to be calibrated and have magnetic hard and soft offsets applied to them live during use, on top of that, I also have to apply gyroscopic and accel. calibration algorithms. Which means, for each sensor, I have to call 9 different data points from each IMU and apply some maths, so I set up some arrays for storing in between values and final values and offsets:
// Offsets applied to raw x/y/z mag values
float mag_offsets[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 10.44F, 34.76F, -49.86F },
{ 8.62F, 20.41F, -12.65F },
{ -3.05F, 19.75F, -8.55F },
};
// Soft iron error compensation matrix
float mag_softiron_matrix[6][3][3] = {
// IMUs 27, 14, 32
{{ 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }},
{{ 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }},
{{ 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }},
// IMUs, 21, 25, 26
{{ 1.036F, 0.017F, -0.001F }, { 0.017F, 0.954F, -0.028F }, { -0.001F, 0.028F, 1.013F }},
{{ 1.031F, 0.013F, -0.024F }, { 0.013F, 0.897F, 0.054F }, { -0.024F, 0.054F, 1.085F }},
{{ 1.057F, 0.034F, 0.017F }, { 0.034F, 0.967F, 0.038F }, { 0.017F, 0.038F, 0.981F }},
};
float mag_field_strength[3] = {38.52F, 37.24F , 38.58F };
// Offsets applied to compensate for gyro zero-drift error for x/y/z, sensor dependent
float gyro_zero_offsets[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
// Used for calculating 'in between values' prior to passing to final mag array, sensor dependent
float deltamag[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
// Following array names should always be constant and final values to be given to Magdwick filters, sensor agnostic.
float gyro[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
float accel[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
float mag[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
Then in the loop itself I call each object and get the sensors readings:
void loop(){
IMU0.readSensor();
IMU1.readSensor();
IMU2.readSensor();
IMU3.readSensor();
IMU4.readSensor();
IMU5.readSensor();
// update accel, gyro, mag arrays
float getAccel[6][3] = {
{ IMU0.getAccelX_mss(), IMU0.getAccelY_mss(), IMU0.getAccelZ_mss() },
{ IMU1.getAccelX_mss(), IMU1.getAccelY_mss(), IMU1.getAccelZ_mss() },
{ IMU2.getAccelX_mss(), IMU2.getAccelY_mss(), IMU2.getAccelZ_mss() },
{ IMU3.getAccelX_mss(), IMU3.getAccelY_mss(), IMU3.getAccelZ_mss() },
{ IMU4.getAccelX_mss(), IMU4.getAccelY_mss(), IMU4.getAccelZ_mss() },
{ IMU5.getAccelX_mss(), IMU5.getAccelY_mss(), IMU5.getAccelZ_mss() },
};
float getGyro[6][3] = {
{ IMU0.getGyroX_rads(), IMU0.getGyroY_rads(), IMU0.getGyroZ_rads() },
{ IMU1.getGyroX_rads(), IMU1.getGyroY_rads(), IMU1.getGyroZ_rads() },
{ IMU2.getGyroX_rads(), IMU2.getGyroY_rads(), IMU2.getGyroZ_rads() },
{ IMU3.getGyroX_rads(), IMU3.getGyroY_rads(), IMU3.getGyroZ_rads() },
{ IMU4.getGyroX_rads(), IMU4.getGyroY_rads(), IMU4.getGyroZ_rads() },
{ IMU5.getGyroX_rads(), IMU5.getGyroY_rads(), IMU5.getGyroZ_rads() },
};
float getMag[6][3] = {
{ IMU0.getMagX_uT(), IMU0.getMagY_uT(), IMU0.getMagZ_uT() },
{ IMU1.getMagX_uT(), IMU1.getMagY_uT(), IMU1.getMagZ_uT() },
{ IMU2.getMagX_uT(), IMU2.getMagY_uT(), IMU2.getMagZ_uT() },
{ IMU3.getMagX_uT(), IMU3.getMagY_uT(), IMU3.getMagZ_uT() },
{ IMU4.getMagX_uT(), IMU4.getMagY_uT(), IMU4.getMagZ_uT() },
{ IMU5.getMagX_uT(), IMU5.getMagY_uT(), IMU5.getMagZ_uT() },
};
// Apply magnetic offsets
for (int j = 0; j < 6; j++) {
for (int i = 0; i < 4; i++) {
deltamag[j][i] = getMag[j][i] - mag_offsets[i][j];
}
}
// Apply magnetic softiron offsets
for (int k = 0; k < 6; k++) {
for (int j = 0; j < 6; j++) {
for (int i = 0; i < 4; i++) {
mag[j][i] = deltamag[j][0] * mag_softiron_matrix[k][0][0] + deltamag[j][1] * mag_softiron_matrix[k][0][1] + deltamag[j][2] * mag_softiron_matrix[k][0][2];
}
}
}
// Apply gyroscope offsets
for (int j = 0; j < 6; j++) {
for (int i = 0; i < 4; i++) {
gyro[j][i] = getGyro[j][i] - gyro_zero_offsets[j][i];
}
}
// Update Madgwick filters
filter0.update(gyro[0][0], gyro[0][1], gyro[0][2], accel[0][0], accel[0][1], accel[0][2], mag[0][0], mag[0][1], -1 * mag[0][2]);
filter1.update(gyro[1][0], gyro[1][1], gyro[1][2], accel[1][0], accel[1][1], accel[1][2], mag[1][0], mag[1][1], -1 * mag[1][2]);
filter2.update(gyro[2][0], gyro[2][1], gyro[2][2], accel[2][0], accel[2][1], accel[2][2], mag[2][0], mag[2][1], -1 * mag[2][2]);
filter3.update(gyro[3][0], gyro[3][1], gyro[3][2], accel[3][0], accel[3][1], accel[3][2], mag[3][0], mag[3][1], -1 * mag[3][2]);
filter4.update(gyro[4][0], gyro[4][1], gyro[4][2], accel[4][0], accel[4][1], accel[4][2], mag[4][0], mag[4][1], -1 * mag[4][2]);
filter5.update(gyro[5][0], gyro[5][1], gyro[5][2], accel[5][0], accel[5][1], accel[5][2], mag[5][0], mag[5][1], -1 * mag[5][2]);
// Call All Euler Angle Rotations around {X,Y,Z} or {gamma, delta, epsilon}
float eulerAngles[6][3] = {
{filter0.getRoll(), filter0.getPitch(), filter0.getYaw()},
{filter1.getRoll(), filter1.getPitch(), filter1.getYaw()},
{filter2.getRoll(), filter2.getPitch(), filter2.getYaw()},
{filter3.getRoll(), filter3.getPitch(), filter3.getYaw()},
{filter4.getRoll(), filter4.getPitch(), filter4.getYaw()},
{filter5.getRoll(), filter5.getPitch(), filter5.getYaw()},
};
Serial.print(eulerAngles[0][0]);
Serial.print(eulerAngles[0][1]);
Serial.print(eulerAngles[0][2]);
}
Though the code seems to work the way I expected it, I am confident this is the wrong method to store this data...namely in the getAccel, getGyro, getMag arrays, or to call them like in eulerAngles .
My hunch on this was during initial testing some sensors data that I receive are have an oscillating error being applied to them, which makes me think i'm receiving junk data from the memory somewhere
...I would have used a for loop, but since each object name is individual and do not have indices, I am unsure the best practice, nor the fastest way to call and work with such a large data set. I have found a similar question, though I'm unfortunately too dumb to apply it to my situation.
So the question is what is the proper method to call and store so many objects (and their data) in arrays for further calculations? I would like to avoid having over a hundred variables (when using all 16 IMUs and in-between variables to carry out all the appropriate maths. My apologies for probably terribly written code, my c++/Wiring is not the best.
Research object-oriented programming. Apply encapsulation. Group data depending on object, not similarities - just like you think about them.
Use standard library objects - std::array. Save memory, allow optimization - apply const whenever possible, use constexpr when possible. Research code guidelines and style guides - like https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#S-philosophy and https://google.github.io/styleguide/cppguide.html .
Let's say in pseudocode, you could encapsulate all variables within one object and use member function to calculate the relevant stuff:
class MyStuff { // pick more meaningfull name
// maybe be more verbose
using axisvals = std::array<float, 3>;
private:
// apply constness to save RAM memory
static const std::array<float, 3> mag_field_strength = { 38.52F, 37.24F , 38.58F };
MPU9250 mpu;
FILTER filter;
const std::array<float, 3> mag_offsets;
const std::array<std::array<float, 3> , 6> mag_softiron_matrix;
std::array<float, 3> gyros{}; // maybe some internal state?
public:
MyStuff(int gpionum,
const std::array<float, 3>& mag_offsets,
const std::array<std::array<float, 3> , 6> mag_softiron_matrix) :
mpu{SPI, gpionum},
filter{some, params, for, filter, constructor},
mag_offsets{mag_offsets},
mag_softiron_matrix{mag_softiron_matrix} {
}
void setup() {
// do some setuping stuff
}
axisvals calculate_stuff() {
mpu.readSensor();
// use const as much as possible
const std::array<float, 3> guro = {
something * mpu.getGyroX_rads(),
something * mpu.getGyroY_rads(),
something * mpu.getGyroZ_rads(),
};
// ...
filter.update(
gyro[0], gyro[1], gyro[2],
accel[0], accel[1], accel[0][2],
mag[0], mag[1], -1 * mag[2]);
// ...
return {filter.getRoll(), filter.getPitch(), filter.getYaw()};
}
};
std::array<MyStuff, 6> imus = {
{ 21, {10.44F, 34.76F, -49.86F}, {{1.036F, 0.017F, -0.001F }, {...}, {...} }, // Header P5
{25, {....} {{...},{..}{...} }, // Header P6
// etc....
};
void setup() {
for (auto&& imu : imus) {
imu.setup();
}
}
void loop() {
for (auto&& imu : imus) {
const auto&vals = imu.calculate_stuff();
for (auto&& v : vals) {
Serial.print(v);
}
}
}