将多个对象及其数据存储在IMUs的数组中以供进一步计算

Question

我目前有一个机器人项目，它使用许多(16) IMU的MPU9250，特别是在SPI下运行的。

作为使用大胆飞行库的六个传感器的简化示例

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

为了使用这些感应器，它们都必须被校准，并且在使用过程中有磁性的、硬的和软的偏移，除此之外，我还必须使用陀螺仪和accel。校准算法这意味着，对于每个传感器，我必须从每个IMU调用9个不同的数据点，并应用一些数学方法，因此我设置了一些数组，用于在值与最终值和偏移量之间存储：

// 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 },
};

然后，在循环本身中，我调用每个对象并获取传感器读数：

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

尽管代码似乎按照我预期的方式工作，但我确信这是将这个data...namely存储在getAccel, getGyro, getMag数组中或像在eulerAngles中那样调用它们的错误方法。

我对此的预感是，在初步测试过程中，我收到的一些传感器数据在应用到它们时出现了一个振荡错误，这让我觉得我从内存中接收到了垃圾数据。

...I本来会使用for循环，但由于每个对象名称都是单独的，并且没有索引，我不确定最佳实践，也不确定调用和处理如此大的数据集的最快方法。我找到了一个相似问题，但不幸的是，我太笨了，无法将它应用到我的情况中。

因此，问题是，在数组中调用和存储这么多对象(及其数据)以进行进一步计算的适当方法是什么？我想避免有超过100个变量(当使用所有16个IMUs和中间变量来执行所有适当的数学。我很抱歉我的代码写得很糟糕，我的c++/接线并不是最好的。

Answer 1

研究面向对象的编程。应用封装。分组数据取决于对象，而不是相似之处--就像你对它们的思考一样。

使用标准库对象- std::array。节省内存，允许优化-尽可能应用const，可能时使用constexpr。研究代码指南和风格指南-比如https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#S-philosophy和https://google.github.io/styleguide/cppguide.html。

假设在伪代码中，可以将所有变量封装在一个对象中，并使用成员函数计算相关内容：

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