Niklas Jensen
96 lines
3.1 KiB
C++
96 lines
3.1 KiB
C++
#pragma once
|
|
|
|
#include <utils/math_utils.h>
|
|
#include <peripherals/sensor.hpp>
|
|
#include <peripherals/drivers/hmc5883l.h>
|
|
|
|
struct MagnetometerMsg {
|
|
float rpy[3] {0, 0, 0};
|
|
float heading {-1};
|
|
bool success {false};
|
|
};
|
|
|
|
class Magnetometer : public SensorBase<MagnetometerMsg> {
|
|
public:
|
|
bool initialize(void* _arg) override {
|
|
#if FT_ENABLED(USE_ICM20948)
|
|
#if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
|
|
SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1);
|
|
_mag = (ICM_20948_SPI*)_arg;
|
|
#ifndef ICM20948_ALIVE
|
|
#define ICM20948_ALIVE
|
|
_imu->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode");
|
|
#endif
|
|
#else
|
|
_mag = (ICM_20948_I2C*)_arg;
|
|
#ifndef ICM20948_ALIVE
|
|
#define ICM20948_ALIVE
|
|
if (!_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
|
|
#endif
|
|
|
|
#endif
|
|
if (_mag->status != ICM_20948_Stat_Ok){ return false; }
|
|
|
|
_mag->startupMagnetometer();
|
|
if (_mag->status != ICM_20948_Stat_Ok){ return false; }
|
|
_msg.success = true;
|
|
#elif FT_ENABLED(USE_HMC5883)
|
|
_msg.success = _mag.begin();
|
|
#endif
|
|
return _msg.success;
|
|
}
|
|
|
|
bool update() override {
|
|
if (!_msg.success) return false;
|
|
#if FT_ENABLED(USE_ICM20948)
|
|
#ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
|
|
#define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
|
|
if (_imu->dataReady())
|
|
{
|
|
_imu->getAGMT();
|
|
} else {
|
|
return false;
|
|
}
|
|
#endif
|
|
_msg.rpy[0] = _mag->magX();
|
|
_msg.rpy[1] = _mag->magY();
|
|
_msg.rpy[2] = _mag->magZ();
|
|
|
|
#elif FT_ENABLED(USE_HMC5883)
|
|
sensors_event_t event;
|
|
bool updated = _mag.getEvent(&event);
|
|
if (!updated) return false;
|
|
_msg.rpy[0] = event.magnetic.x;
|
|
_msg.rpy[1] = event.magnetic.y;
|
|
_msg.rpy[2] = event.magnetic.z;
|
|
#endif
|
|
_msg.heading = atan2(_msg.rpy[1], _msg.rpy[0]); // atan2(y, x)
|
|
_msg.heading += declinationAngle;
|
|
if (_msg.heading < 0) _msg.heading += 2 * PI;
|
|
if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
|
|
_msg.heading *= 180 / M_PI;
|
|
return true;
|
|
}
|
|
|
|
float getMagX() { return _msg.rpy[0]; }
|
|
float getMagY() { return _msg.rpy[1]; }
|
|
float getMagZ() { return _msg.rpy[2]; }
|
|
float getHeading() { return _msg.heading; }
|
|
|
|
private:
|
|
|
|
#if FT_ENABLED(USE_ICM20948)
|
|
#if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
|
|
#define SPI_PORT SPI // TODO in periphearals_seetings.h
|
|
#define CS_PIN 2
|
|
ICM_20948_SPI* _mag;
|
|
#else
|
|
//#define WIRE_PORT Wire
|
|
ICM_20948_I2C* _mag;
|
|
#endif
|
|
#elif FT_ENABLED(USE_HMC5883)
|
|
Adafruit_HMC5883_Unified _mag {12345};
|
|
#endif
|
|
const float declinationAngle = 0.22;
|
|
};
|