mpuclass

#include <Arduino.h>
#include <array>
#include "MPU9250.h"
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
//#include <driver/adc.h>
#include <Madgwick.h>
#include <TaskScheduler.h>



class euler {
    // maybe be more verbose
    using axisvals = std::array<float, 3>;

private:
    // apply constness to save RAM memory
   // const std::array<float, 2> mag_field_strength =  {{ 41.85F, 40.6F }};
   MPU9250 mpu;
   Madgwick filter;
   const std::array<float, 3> mag_offsets;
   const std::array<std::array<float, 3> , 3> mag_softiron_matrix;
   const std::array<float, 3> gyro_offsets{};

public:
   euler(int gpio, const std::array<float, 3>& mag_offsets, const std::array<std::array<float, 3> , 3>& mag_softiron_matrix) :
      mpu(SPI, gpio),
      mag_offsets{mag_offsets},
      mag_softiron_matrix{mag_softiron_matrix} {
   }

   void setup() {
     // do some setuping stuff
     filter.begin(250);
     mpu.setAccelRange(MPU9250::ACCEL_RANGE_2G);
     mpu.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
     mpu.calibrateAccel();
     mpu.calibrateGyro();
  }

   axisvals calculate_stuff() {
      mpu.readSensor();
      // use const as much as possible
      const std::array<float, 3> accel = {
         mpu.getAccelX_mss(),
         mpu.getAccelY_mss(),
         mpu.getAccelZ_mss(),
      };
      const std::array<float, 3> gyro = {
         mpu.getGyroX_rads(),
         mpu.getGyroY_rads(),
         mpu.getGyroZ_rads(),
      };
      const std::array<float, 3> magneto = {
         mpu.getMagX_uT(),
         mpu.getMagY_uT(),
         mpu.getMagZ_uT(),
      };
      filter.update(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magneto[0], magneto[1], magneto[2]);
      return {filter.getRoll(), filter.getPitch(),  filter.getYaw()};
   }
};
euler mup(21, {10.44, 34.76, -49.86}, {{ 0.994, -0.023, -0.008 },{ -0.023, 1.027, 0.013 },{ -0.008, 0.013, 0.980 }});

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