Storing many objects and their data within arrays from IMUs for further calculation

Question

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.

You should research member function pointers. Why isn't this a loop? `but since each object name is individual and do not have indices` so put objects in an array? Why isn't it an array then? — KamilCuk, Jul 25 '21 at 11:15
it is in an array? is that the correct method...what I already did? also it is in a loop? I will have a look at member function pointers, thanks for the suggestion. — DrMrstheMonarch, Jul 25 '21 at 11:20
Whyt isn't `MPU9250 IMU0(SPI, 21); // Header P5 MPU9250 IMU1(SPI, 25); ...` just `MPU9250 IMU[6] = { ....}`? Do you have control over this code? Is each `MPU9250` + `filter` a separate entity unrelated to others? The calculations seems to care of one MPU at a time. Maybe you should instead create a `class` where you put _common logic_ in a function to encompass repeated code patterns? — KamilCuk, Jul 25 '21 at 11:20
'Is each MPU9250 + filter a separate entity unrelated to others? The calculations seems to care of one MPU at a time' Yes there are 16 different sensors, with their own filters and offsets, this code just has 6 as an example. — DrMrstheMonarch, Jul 25 '21 at 11:24
@KamilCuk a class sounds like a good idea. Though I would have assumed for loops with the arrays would be enough? Or is the idea that I give each object within that class the imu and have it apply those offsets internally each time? like IMU.readsensor()? Sorry for the dumbness on my part. I'm mostly a hack at embedded programming — DrMrstheMonarch, Jul 25 '21 at 11:29

KamilCuk · Accepted Answer · 2021-07-25T16:27:10.317

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);
       }
   }
}

Thank you for providing this initial steps and advice...I have managed to write a working class...however for some reason I cannot initalize the object within my program. Here is my current version of your code https://pastebin.com/5Vgj2X3n thanks again! — DrMrstheMonarch, Aug 01 '21 at 16:47
Hm, with this design you'll have to tell compiler what type the subjects are. `euler mup(21, {10.44, 34.76, -49.86}, { std::array{ 0.994, -0.023, -0.008 }, std::array{ -0.023, 1.027, 0.013 }, std::array{ -0.008, 0.013, 0.980 } });`. or `euler mup(21, {10.44, 34.76, -49.86}, std::array , 3>{{ { 0.994, -0.023, -0.008 }, { -0.023, 1.027, 0.013 }, { -0.008, 0.013, 0.980 }, }});`. The above was just an example, you might want to use different design, with better object lifetimes and read about std::initailizer_list — KamilCuk, Aug 01 '21 at 17:02
I will try those suggestions thanks!, If you have a better design idea I'd definitely try it...But I've spent all day trying to get this to work, a different design would probably be more advanced for my current ability. Regardless appreciate your help! — DrMrstheMonarch, Aug 01 '21 at 17:09

Storing many objects and their data within arrays from IMUs for further calculation

1 Answers1