54 lines
1.5 KiB
C++
54 lines
1.5 KiB
C++
#include "PID.hpp"
|
|
#include "Sensors/Sensors.hpp"
|
|
|
|
enum DeviceStatus {
|
|
Idle = 0,
|
|
IsPreparingForTakeoff,
|
|
IsFlying,
|
|
IsBoarding,
|
|
IsImuCalibration,
|
|
ChargeRequired
|
|
};
|
|
|
|
struct ReportedDeviceState {
|
|
int batteryCharge;
|
|
float flightHeight;
|
|
DeviceStatus status;
|
|
MpuData mpuState;
|
|
};
|
|
|
|
struct FlightParams {
|
|
float height; // [cm]
|
|
float yaw; // [degrees]
|
|
float pitch; // [degrees]
|
|
};
|
|
|
|
class FlightController {
|
|
public:
|
|
FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2, PIDController *pid3, bool unlimitedBattery = false);
|
|
~FlightController();
|
|
ReportedDeviceState currentDeviceState() const;
|
|
void startTakeoff(OnMeasuringFinishedCb onTakeoffStarted);
|
|
void startBoarding();
|
|
void startImuCalibration();
|
|
void tick();
|
|
private:
|
|
void _updateBatteryCharge();
|
|
void _takeoff();
|
|
void _boarding();
|
|
|
|
Sensors *_sensors = nullptr;
|
|
PIDController *_pid1 = nullptr;
|
|
PIDController *_pid2 = nullptr;
|
|
PIDController *_pid3 = nullptr;
|
|
DeviceStatus _status;
|
|
OnMeasuringFinishedCb _onTakeoffStarted = nullptr;
|
|
|
|
static constexpr const char *_tag = "FlightController";
|
|
static constexpr uint8_t _batteryCriticalCharge = 15; // [%]
|
|
static constexpr float _defaultFlightHeight = 30.f; // [cm]
|
|
static constexpr float _defaultHorizontAngle = 90.f; // [degrees]
|
|
static constexpr float _defaultStopMotorHeight = 5; // [cm]
|
|
|
|
bool _unlimitedBattery = false; // for debug purposes
|
|
}; |