0

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

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

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

虽然代码似乎按我预期的方式工作，但我相信这是存储这些数据的错误方法......即在getAccel, getGyro, getMag数组中，或者像在eulerAngles.

我对此的预感是在初始测试期间，我收到的一些传感器数据有一个振荡错误应用于它们，这让我认为我正在从某个地方的内存中接收垃圾数据

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

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

4

1 回答 1

2

研究面向对象的程序设计。应用封装。根据对象而不是相似性对数据进行分组 - 就像您考虑它们一样。

使用标准库对象 - 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);
       }
   }
}
于 2021-07-25T12:20:49.157 回答