74 lines
2.2 KiB
C++
74 lines
2.2 KiB
C++
#include "PID.hpp"
|
|
#include <memory>
|
|
#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]
|
|
};
|
|
|
|
struct PidSettings {
|
|
float p;
|
|
float i;
|
|
float d;
|
|
};
|
|
|
|
class FlightController {
|
|
public:
|
|
FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
|
|
PIDController *pid3, bool unlimitedBattery = false);
|
|
~FlightController();
|
|
ReportedDeviceState currentDeviceState() const;
|
|
void startTakeoff();
|
|
void startBoarding();
|
|
void startImuCalibration();
|
|
void tick();
|
|
[[deprecated]] void moveToFlightHeight(float newFlightHeight); //[cm]
|
|
[[deprecated]] void newFlightHeightStickPosition(int16_t newFlightHeightStickPostition);
|
|
[[deprecated]] void newYawStickPosition(int16_t newYawStickPostition);
|
|
[[deprecated]] void newPitchStickPosition(int16_t newPitchStickPostition);
|
|
void newRotor1Duty(uint32_t rotor1Duty);
|
|
void newRotor2Duty(uint32_t rotor2Duty);
|
|
void newRotor3Duty(uint32_t rotor3Duty);
|
|
void stopAllRotors();
|
|
void setPid1Params(float p, float i, float d);
|
|
void setPid2Params(float p, float i, float d);
|
|
void setPid3Params(float p, float i, float d);
|
|
PidSettings *pidSettings() const;
|
|
private:
|
|
void _updateBatteryCharge();
|
|
[[deprecated]] void _takeoff();
|
|
[[deprecated]] 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
|
|
}; |