agrobot_base/Firmware/Modulos/SensorIMUModel.h

#ifndef SensorIMUModel
#define SensorIMUModel

#include <MPU9250_asukiaaa.h>
#include <Adafruit_BMP280.h>
#include <MadgwickAHRS.h>
#include <Wire.h>
#include "SerialService.h"
#include "Pinout.h"

class SensorIMU {
  public:

    static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorIMU*>& lista, std::vector<uint8_t>& data, const String& Mod_ID);

    String Mod_ID;
    String _ID;
    int ID_Num;
    bool Iniciado = false;
    bool MpuIniciado = false;
    bool BmpIniciado = false;

    // Endereços I2C
    byte _EnderecoMPU; // 0x68
    byte _EnderecoBMP; // 0x76

    // MPU9250 (IMU 9 eixos)
    MPU9250_asukiaaa mpu;

    // BMP280 (Temperatura, Pressão, Altitude)
    Adafruit_BMP280 bmp;

    Madgwick filter;
    float filterHz = 250.0f;
    float filterBeta = 0.2f;
    int filterDelay = 1000.0f / filterHz;

    // Leituras principais
    float AccX = 0, AccY = 0, AccZ = 0;
    float GyroX = 0, GyroY = 0, GyroZ = 0;
    float MagX = 0, MagY = 0, MagZ = 0;
    float Temp = 0;
    float Pressao = 0;
    float Altitude = 0;
    float Roll = 0;
    float Pitch = 0;
    float Yaw = 0;

    SensorIMU(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    void Inicializar() {
      if (Iniciado) {
        PrintTela(_ID + " ja inicializado");
        return;
      }

      Wire.beginTransmission(_EnderecoMPU);
      byte errorMpu = Wire.endTransmission();
      MpuIniciado = errorMpu == 0;
      if (MpuIniciado) {
        mpu.setWire(&Wire);
        mpu.beginAccel();
        mpu.beginGyro();
        mpu.beginMag();

        filter.begin(filterHz);
        filter.setBeta(filterBeta);
      } else {
        PrintTela("MPU9250 nao encontrado no endereco " + String(_EnderecoMPU));
      }
      
      Wire.beginTransmission(_EnderecoBMP);
      byte errorBmp = Wire.endTransmission();
      bool BmpEncontrado = errorBmp == 0;
      if (BmpEncontrado) {
        BmpIniciado = bmp.begin(_EnderecoBMP);
        if (BmpIniciado) {
          bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
            Adafruit_BMP280::SAMPLING_X2,
            Adafruit_BMP280::SAMPLING_X16,
            Adafruit_BMP280::FILTER_X16,
            Adafruit_BMP280::STANDBY_MS_500);
        }
        else {
          PrintTela("Erro ao iniciar BMP280");
        }
      }
      else {
        PrintTela("BMP280 nao encontrado no endereco " + String(_EnderecoBMP));
      }

      Iniciado = MpuIniciado || BmpIniciado;
      if (Iniciado) {
        xTaskCreatePinnedToCore(&SensorIMU::IMUTaskWrapper, "IMUTask", 5000, this, 20, &IMUTaskHandle, tskNO_AFFINITY);
        PrintTela(_ID + " iniciado");
      }
    }

    void Desligar() {
      if (!Iniciado) {
        PrintTela(_ID + " nao esta inicializado");
        return;
      }

      // Parar a execução das tarefas
      if (IMUTaskHandle != NULL) {
        vTaskDelete(IMUTaskHandle);
        IMUTaskHandle = NULL;
      }

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void RequisitarDados() {
      /*Serial.print("Temp: "); Serial.println(Temp);
      Serial.print("Pressao: "); Serial.println(Pressao);
      Serial.print("Altitude: "); Serial.println(Altitude);
      Serial.print("Roll: "); Serial.println(Roll);
      Serial.print("Pitch: "); Serial.println(Pitch);
      Serial.print("Yaw: "); Serial.println(Yaw);
      Serial.println();*/
    }

    std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
      std::vector<uint8_t> data;
      data.push_back(ID_Num);
      data.push_back(static_cast<uint8_t>(posicao));
    
      switch (posicao) {
        case CanMessagePosicaoDados::Status: {
          data.push_back(Iniciado ? 1 : 0);
          break;
        }

        case CanMessagePosicaoDados::Dados1: { // Roll, Pitch, Yaw
          int16_t roll = Roll * 100;
          int16_t pitch = Pitch * 100;
          int16_t yaw = Yaw * 100;

          data.push_back(roll >> 8);  data.push_back(roll & 0xFF);
          data.push_back(pitch >> 8); data.push_back(pitch & 0xFF);
          data.push_back(yaw >> 8);   data.push_back(yaw & 0xFF);
          break;
        }

        case CanMessagePosicaoDados::Dados2: { // Temp, Pressão, Altitude
          int16_t temp = Temp * 100;
          uint16_t pressao = Pressao / 10;        // Ex: 100000 Pa → 10000 (precisão: 10 Pa)
          int16_t altitude = Altitude / 10;
    
          data.push_back(temp >> 8); data.push_back(temp & 0xFF);
          data.push_back(pressao >> 8); data.push_back(pressao & 0xFF);
          data.push_back(altitude >> 8); data.push_back(altitude & 0xFF);
          break;
        }
    
        case CanMessagePosicaoDados::Dados3: { // Acc
          int16_t accX = AccX * 100;
          int16_t accY = AccY * 100;
          int16_t accZ = AccZ * 100;
    
          data.push_back(accX >> 8); data.push_back(accX & 0xFF);
          data.push_back(accY >> 8); data.push_back(accY & 0xFF);
          data.push_back(accZ >> 8); data.push_back(accZ & 0xFF);
          break;
        }
    
        case CanMessagePosicaoDados::Dados4: { // Gyro
          int16_t gyroX = GyroX * 100;
          int16_t gyroY = GyroY * 100;
          int16_t gyroZ = GyroZ * 100;
    
          data.push_back(gyroX >> 8); data.push_back(gyroX & 0xFF);
          data.push_back(gyroY >> 8); data.push_back(gyroY & 0xFF);
          data.push_back(gyroZ >> 8); data.push_back(gyroZ & 0xFF);
          break;
        }
    
        case CanMessagePosicaoDados::Dados5: { // Mag
          int16_t magX = MagX * 100;
          int16_t magY = MagY * 100;
          int16_t magZ = MagZ * 100;
    
          data.push_back(magX >> 8); data.push_back(magX & 0xFF);
          data.push_back(magY >> 8); data.push_back(magY & 0xFF);
          data.push_back(magZ >> 8); data.push_back(magZ & 0xFF);
          break;
        }
    
      }
    
      return data;
    }


  private:
    TaskHandle_t IMUTaskHandle = NULL;
    static void IMUTaskWrapper(void *pvParameters) {
      SensorIMU *sensor = static_cast<SensorIMU*>(pvParameters);
      sensor->IMUTask();
    }

    void IMUTask() {
      while (1) {
        if (Iniciado) {
          AferirDados();
        }
        vTaskDelay(pdMS_TO_TICKS(filterDelay));
      }
    }

    void AferirDados() {
      if (!Iniciado) return;

      if (MpuIniciado) {
        mpu.accelUpdate();
        mpu.gyroUpdate();
        mpu.magUpdate();
  
        AccX = mpu.accelX();
        AccY = mpu.accelY();
        AccZ = mpu.accelZ();

        GyroX = mpu.gyroX();
        GyroY = mpu.gyroY();
        GyroZ = mpu.gyroZ();

        MagX = mpu.magX();
        MagY = mpu.magY();
        MagZ = mpu.magZ();

        if (true || MagX == 0 && MagY == 0 && MagZ == 0) {
          filter.updateIMU(GyroX, GyroY, GyroZ, AccX, AccY, AccZ);
        }
        else {
          filter.update(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ);
        }

        // Recupera os quaternions
        float q0 = filter.getQ0();
        float q1 = filter.getQ1();
        float q2 = filter.getQ2();
        float q3 = filter.getQ3();

        // Vetor "up"
        float upX = 2 * (q1 * q3 - q0 * q2);
        float upY = 2 * (q2 * q3 + q0 * q1);
        float upZ = 1 - 2 * (q1 * q1 + q2 * q2);

        // Calcula os ângulos corrigidos
        Roll = atan2(upY, upZ) * RAD_TO_DEG;
        Roll = (Roll > 0) ? Roll - 180 : Roll + 180;
        Pitch = -(atan2(-upX, sqrt(upY * upY + upZ * upZ)) * RAD_TO_DEG);
        Yaw = atan2(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3) * RAD_TO_DEG;
      }
      
      if (BmpIniciado) {
        Temp = bmp.readTemperature();
        Pressao = bmp.readPressure();
        Altitude = bmp.readAltitude();  
      }
    }
};


std::vector<uint8_t> SensorIMU::ConfigurarSensor(std::vector<SensorIMU*>& lista, std::vector<uint8_t>& data, const String& Mod_ID) {
  std::vector<uint8_t> status;
  if (data.size() < 3) return status;
  uint8_t idNum = data[1];
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[2];
  auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorIMU* s) { return s->ID_Num == idNum; });
  bool jaExiste = it != lista.end();
  SensorIMU* sensor;
  switch (posicao) {
    case CanMessagePosicaoDados::Config1: {
      if (data.size() < 7) return status;
      bool conectar = data[4] == 1;
      uint8_t enderecoMpu = data[5];
      uint8_t enderecoBmp = data[6];
      if (conectar) {
        if (!jaExiste) {
          sensor = new SensorIMU(Mod_ID, "sIMU_" + String(idNum));
        }
        else {
          sensor = *it;
        }
        if (!sensor->Iniciado) {
          sensor->ID_Num = idNum;
          sensor->_EnderecoMPU = enderecoMpu;
          sensor->_EnderecoBMP = enderecoBmp;
          sensor->Inicializar();
          if (!jaExiste) {
            lista.push_back(sensor);
            PrintTela("Sensor IMU adicionado via CAN: sIMU_" + String(idNum));
          }
        }
        status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
      } else {
        if (jaExiste) {
          sensor = *it;
          sensor->Desligar();
          status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          delete sensor;
          lista.erase(it);
          PrintTela("Sensor IMU removido via CAN: sIMU_" + String(idNum));
        }
      }
      break;
    }
  }
  return status;
}

#endif