在来自 IMU 的数组中存储许多对象及其数据以供进一步计算

如何解决在来自 IMU 的数组中存储许多对象及其数据以供进一步计算

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

作为使用 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

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

// Offsets applied to raw x/y/z mag values
float mag_offsets[6][3] = {
  { 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,// IMUs,21,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.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,};
// Used for calculating 'in between values' prior to passing to final mag array,sensor dependent
float deltamag[6][3] = {
  { 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,};

float accel[6][3] = {
  { 0.0F,};

float mag[6][3] = {
  { 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]);
}

虽然代码似乎按我预期的方式工作，但我相信这是存储此数据的错误方法...即在 getAccel,getGyro,getMag 数组中，或调用他们喜欢在eulerAngles .

我对此的预感是在初始测试期间我收到的一些传感器数据对它们应用了振荡错误，这让我觉得我正在从某处的内存中接收垃圾数据

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

那么问题是在数组中调用和存储如此多的对象（及其数据）以进行进一步计算的正确方法是什么？我想避免有超过一百个变量（当使用所有 16 个 IMU 和中间变量来执行所有适当的数学运算时。我为可能写得很糟糕的代码道歉，我的 c++/接线不是最好的。

解决方法

研究面向对象的编程。应用封装。根据对象而非相似性对数据进行分组 - 就像您对它们的看法一样。

使用标准库对象 - 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,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);
       }
   }
}