104 lines
2.9 KiB
C++
104 lines
2.9 KiB
C++
#include "FlightController.hpp"
|
|
|
|
FlightController::FlightController(Sensors *sensors, PIDController *pid1,
|
|
PIDController *pid2, PIDController *pid3, bool unlimitedBattery) {
|
|
assert(sensors != nullptr);
|
|
assert(pid1 != nullptr);
|
|
assert(pid2 != nullptr);
|
|
assert(pid3 != nullptr);
|
|
_sensors = sensors;
|
|
_unlimitedBattery = unlimitedBattery;
|
|
|
|
_status = DeviceStatus::Idle;
|
|
_updateBatteryCharge();
|
|
ESP_LOGI(_tag, "Flight controller initialized");
|
|
}
|
|
|
|
FlightController::~FlightController() {
|
|
delete _sensors;
|
|
delete _pid1;
|
|
delete _pid2;
|
|
delete _pid3;
|
|
}
|
|
|
|
void FlightController::tick() {
|
|
bool hasUpdates = _sensors->tick();
|
|
if (hasUpdates) {
|
|
_updateBatteryCharge();
|
|
}
|
|
switch (_status) {
|
|
case DeviceStatus::Idle: break;
|
|
case DeviceStatus::ChargeRequired: break;
|
|
case DeviceStatus::IsPreparingForTakeoff: break;
|
|
case DeviceStatus::IsBoarding:
|
|
/* TODO: implement boarding */
|
|
break;
|
|
case DeviceStatus::IsFlying:
|
|
/* TODO: implement flying */
|
|
break;
|
|
}
|
|
}
|
|
|
|
ReportedDeviceState FlightController::currentDeviceState() const {
|
|
return { .batteryCharge = _sensors->batteryCharge(),
|
|
.flightHeight = _sensors->flightHeight(),
|
|
.status = _status,
|
|
.mpuState = _sensors->mpuData() };
|
|
}
|
|
|
|
void FlightController::startTakeoff(OnMeasuringFinishedCb onTakeoffStarted) {
|
|
/* Start preparation for takeoff */
|
|
_updateBatteryCharge();
|
|
if (_status == DeviceStatus::ChargeRequired)
|
|
return;
|
|
|
|
_status = DeviceStatus::IsPreparingForTakeoff;
|
|
_sensors->onMeasuaringAltitudeFinished([this]() {
|
|
if (_status == DeviceStatus::IsPreparingForTakeoff) {
|
|
// Preparation for takeoff is done
|
|
_takeoff();
|
|
}
|
|
});
|
|
_sensors->measureBaseAltitudeAsync();
|
|
}
|
|
|
|
void FlightController::startBoarding() {
|
|
if (_status == DeviceStatus::IsFlying) {
|
|
_status = DeviceStatus::IsBoarding;
|
|
}
|
|
}
|
|
|
|
void FlightController::_updateBatteryCharge() {
|
|
if(_unlimitedBattery) return;
|
|
if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
|
|
if (_status == DeviceStatus::IsFlying or _status == DeviceStatus::IsBoarding) {
|
|
// TODO: board the device
|
|
_status = DeviceStatus::IsBoarding;
|
|
} else {
|
|
_status = DeviceStatus::ChargeRequired;
|
|
}
|
|
}
|
|
}
|
|
|
|
void FlightController::_takeoff() {
|
|
// TODO: implement takeoff
|
|
_status = DeviceStatus::IsFlying;
|
|
if(_onTakeoffStarted) {
|
|
_onTakeoffStarted();
|
|
}
|
|
}
|
|
|
|
void FlightController::_boarding() {
|
|
if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
|
|
// TODO: stop motors and setpoints
|
|
_status = DeviceStatus::Idle;
|
|
} else {
|
|
// TODO: implement boarding
|
|
}
|
|
}
|
|
|
|
void FlightController::startImuCalibration() {
|
|
_sensors->onMpuCalibrationFinished([this]() {
|
|
this->_status = DeviceStatus::Idle;
|
|
});
|
|
} |