95 lines
2.6 KiB
C++
95 lines
2.6 KiB
C++
#include "Sensors.hpp"
|
|
|
|
Sensors::Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail) {
|
|
_barometer = &barometer;
|
|
_mpu = &mpu;
|
|
_2d_filter = &filter;
|
|
_battery = &battery;
|
|
|
|
ESP_LOGI(_tag, "Initialize BMP280...");
|
|
if (_barometer->initialize()) {
|
|
ESP_LOGI(_tag, "BMP280 initialized");
|
|
} else {
|
|
ESP_LOGI(_tag, "BMP280 initialization failed!");
|
|
while (loop_on_fail)
|
|
;
|
|
}
|
|
|
|
ESP_LOGI(_tag, "Initialize MPU6050...");
|
|
if (_mpu->initialize()) {
|
|
ESP_LOGI(_tag, "MPU6050 initialized");
|
|
} else {
|
|
ESP_LOGI(_tag, "MPU6050 initialization failed!");
|
|
while (loop_on_fail)
|
|
;
|
|
}
|
|
|
|
_battery->initialize();
|
|
ESP_LOGI(_tag, "BatteryController initialized");
|
|
ESP_LOGI(_tag, "Sensors initialized");
|
|
}
|
|
|
|
Sensors::~Sensors() {
|
|
delete _barometer;
|
|
delete _mpu;
|
|
delete _2d_filter;
|
|
delete _battery;
|
|
}
|
|
|
|
void Sensors::measureBaseAltitudeSync(void) {
|
|
_barometer->measureBaseAltitudeSync();
|
|
}
|
|
|
|
void Sensors::measureBaseAltitudeAsync(void) {
|
|
_barometer->measureBaseAltitudeAsync();
|
|
}
|
|
|
|
float Sensors::rawFlightHeight(void) {
|
|
/* Returns flight height from barometer immediatly */
|
|
return _barometer->flightHeight();
|
|
}
|
|
|
|
float Sensors::flightHeight(void) {
|
|
/* Returns flight height from cache immediatly */
|
|
if (_flightHeightFromBarometer == NAN) {
|
|
ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
|
|
return rawFlightHeight();
|
|
} else {
|
|
return _flightHeightFromBarometer;
|
|
}
|
|
}
|
|
|
|
MpuData Sensors::mpuData() {
|
|
if (_mpuData.ax == NAN and _mpuData.gravityZ == NAN) {
|
|
ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
|
|
}
|
|
return _mpuData;
|
|
}
|
|
|
|
bool Sensors::tick(void) {
|
|
/* Super loop iteraion */
|
|
/* Return true on update */
|
|
bool err;
|
|
bool isMpuDataReady = _mpu->tick(err);
|
|
if (isMpuDataReady and !err) {
|
|
_2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(), _flightHeightFromBarometer, _zVelocityAltitude);
|
|
_mpuData = { .ax = _mpu->ax(),
|
|
.ay = _mpu->ay(),
|
|
.az = _mpu->az(),
|
|
.gx = _mpu->gx(),
|
|
.gy = _mpu->gy(),
|
|
.gz = _mpu->gz(),
|
|
.yaw = _mpu->yaw(),
|
|
.pitch = _mpu->pitch(),
|
|
.roll = _mpu->roll(),
|
|
.gravityZ = _mpu->gravityZ() };
|
|
return true;
|
|
} else if (err) {
|
|
ESP_LOGE(_tag, "Failed to get DMP data!");
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int Sensors::batteryCharge(void) {
|
|
return _battery->percent();
|
|
} |