119 lines
3.4 KiB
C++
119 lines
3.4 KiB
C++
// This is a personal academic project. Dear PVS-Studio, please check it.
|
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
|
|
|
#include "Sensors.hpp"
|
|
|
|
Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
|
|
BatteryController *battery, volatile bool loop_on_fail) {
|
|
assert(barometer != nullptr);
|
|
assert(mpu != nullptr);
|
|
assert(filter != nullptr);
|
|
assert(battery != nullptr);
|
|
|
|
_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) const {
|
|
/* Returns flight height from barometer immediatly */
|
|
return _barometer->flightHeight();
|
|
}
|
|
|
|
float Sensors::flightHeight(void) const {
|
|
/* Returns flight height from cache immediatly */
|
|
if (_flightHeightFromBarometer == 0.f) {
|
|
ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
|
|
return rawFlightHeight();
|
|
} else {
|
|
return _flightHeightFromBarometer;
|
|
}
|
|
}
|
|
|
|
MpuData Sensors::mpuData() const {
|
|
if (_mpuData.ax == 0.f and _mpuData.gravity == 0.f) {
|
|
ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
|
|
}
|
|
return _mpuData;
|
|
}
|
|
|
|
bool Sensors::tick(void) {
|
|
/* Super loop iteraion */
|
|
/* Return true on update */
|
|
_barometer->tick();
|
|
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(),
|
|
.gravity = _mpu->gravityZ(),
|
|
.zInertial = _mpu->accZInertial() };
|
|
return true;
|
|
} else if (err) {
|
|
ESP_LOGE(_tag, "Failed to get DMP data!");
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int Sensors::batteryCharge(void) const {
|
|
return _battery->percent();
|
|
}
|
|
|
|
void Sensors::onMpuCalibrationFinished(OnCalibrationFinishedCb callback) {
|
|
_mpu->onCalibrationFinished(callback);
|
|
}
|
|
|
|
void Sensors::startMpuCalibration() {
|
|
_mpu->startCalibration();
|
|
}
|
|
|
|
void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) {
|
|
_barometer->onMeasuaringHeightFinished(callback);
|
|
} |