Better routing

Logic/FlightController.cpp

@@ -1,13 +1,16 @@
 #include "FlightController.hpp"
 
-FlightController::FlightController(Sensors *sensors, PIDController *pid1,
-                                   PIDController *pid2, PIDController *pid3, bool unlimitedBattery) {
+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;
+    _pid1 = pid1;
+    _pid2 = pid2;
+    _pid3 = pid3;
 
     _status = DeviceStatus::Idle;
     _updateBatteryCharge();
@@ -30,11 +33,17 @@ void FlightController::tick() {
     case DeviceStatus::Idle: break;
     case DeviceStatus::ChargeRequired: break;
     case DeviceStatus::IsPreparingForTakeoff: break;
+    case DeviceStatus::IsImuCalibration: break;
     case DeviceStatus::IsBoarding:
         /* TODO: implement boarding */
         break;
     case DeviceStatus::IsFlying:
         /* TODO: implement flying */
+        if (hasUpdates) {
+            _pid1->tick();
+            _pid2->tick();
+            _pid3->tick();
+        }
         break;
     }
 }
@@ -46,10 +55,10 @@ ReportedDeviceState FlightController::currentDeviceState() const {
              .mpuState = _sensors->mpuData() };
 }
 
-void FlightController::startTakeoff(OnMeasuringFinishedCb onTakeoffStarted) {
+void FlightController::startTakeoff() {
     /* Start preparation for takeoff */
     _updateBatteryCharge();
-    if (_status == DeviceStatus::ChargeRequired)
+    if (_status != DeviceStatus::Idle)
         return;
 
     _status = DeviceStatus::IsPreparingForTakeoff;
@@ -69,11 +78,11 @@ void FlightController::startBoarding() {
 }
 
 void FlightController::_updateBatteryCharge() {
-    if(_unlimitedBattery) return;
+    if (_unlimitedBattery)
+        return;
     if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
-        if (_status == DeviceStatus::IsFlying or _status == DeviceStatus::IsBoarding) {
-            // TODO: board the device
-            _status = DeviceStatus::IsBoarding;
+        if (_status == DeviceStatus::IsFlying) {
+            startBoarding();
         } else {
             _status = DeviceStatus::ChargeRequired;
         }
@@ -83,7 +92,7 @@ void FlightController::_updateBatteryCharge() {
 void FlightController::_takeoff() {
     // TODO: implement takeoff
     _status = DeviceStatus::IsFlying;
-    if(_onTakeoffStarted) {
+    if (_onTakeoffStarted) {
         _onTakeoffStarted();
     }
 }
@@ -92,13 +101,91 @@ void FlightController::_boarding() {
     if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
         // TODO: stop motors and setpoints
         _status = DeviceStatus::Idle;
+
     } else {
         // TODO: implement boarding
     }
 }
 
 void FlightController::startImuCalibration() {
+    if (_status != DeviceStatus::Idle)
+        return;
     _sensors->onMpuCalibrationFinished([this]() {
         this->_status = DeviceStatus::Idle;
     });
+}
+
+void FlightController::moveToFlightHeight(float newFlightHeight) {
+    // TODO: implement setting flight height
+}
+
+void FlightController::newFlightHeightStickPosition(int16_t newFlightHeightStickPostition) {
+    // TODO: implement stick input
+}
+
+void FlightController::newYawStickPosition(int16_t newYawStickPostition) {
+    // TODO: implement stick input
+}
+
+void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
+    // TODO: implement stick input
+}
+
+void FlightController::newRotor1Duty(uint32_t rotor1Duty) {
+    if (_status == DeviceStatus::Idle) {
+        _pid1->setRotorDuty(rotor1Duty);
+    }
+}
+
+void FlightController::newRotor2Duty(uint32_t rotor2Duty) {
+    if (_status == DeviceStatus::Idle) {
+        _pid2->setRotorDuty(rotor2Duty);
+    }
+}
+
+void FlightController::newRotor3Duty(uint32_t rotor3Duty) {
+    if (_status == DeviceStatus::Idle) {
+        _pid3->setRotorDuty(rotor3Duty);
+    }
+}
+
+void FlightController::stopAllRotors() {
+    _pid1->setRotorDuty(0);
+    _pid2->setRotorDuty(0);
+    _pid3->setRotorDuty(0);
+}
+
+void FlightController::setPid1Params(float p, float i, float d) {
+    if (_status == DeviceStatus::Idle) {
+        _pid1->Kp = p;
+        _pid1->Ki = i;
+        _pid1->Kd = d;
+        _pid1->save();
+    }
+}
+
+void FlightController::setPid2Params(float p, float i, float d) {
+    if (_status == DeviceStatus::Idle) {
+        _pid2->Kp = p;
+        _pid2->Ki = i;
+        _pid2->Kd = d;
+        _pid1->save();
+    }
+}
+
+void FlightController::setPid3Params(float p, float i, float d) {
+    if (_status == DeviceStatus::Idle) {
+        _pid3->Kp = p;
+        _pid3->Ki = i;
+        _pid3->Kd = d;
+        _pid1->save();
+    }
+}
+
+PidSettings *FlightController::pidSettings() const {
+    PidSettings pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
+    PidSettings pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
+    PidSettings pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
+    PidSettings settings[] = { pid1, pid2, pid3 };
+    return settings;
 }