Merge pull request #52 from gotvinne/got/flight-controller/add-state-machine

Flight-controller: Add state machine

Author: gotvinne

img/state-machine.png

@@ -1,6 +1,8 @@
 # Flight controller
 Reimplement the javascript logic found [here](https://github.com/gomakekit/Airbit_V2/blob/master/custom.ts).
 
+![State machine](../../img/state-machine.png)
+
 ### Ultrasonic sensor (external hardware HC-SR04)
 
 The microbit PCB is equiped with a barometer which can be used to calculate the altitude of the quadcopter. However, due to the properllers impacting the sounding pressure, this measurement is not reliable. To circumvent this problem, and retieving a reliable measurement, an external ultrasonic sensor can be connected to the microbit.

src/flight-controller/README.md

@@ -7,7 +7,7 @@ MicroBitPin SCL = uBit.io.P1;
 void InitI2C() {
   // Redirect I2C to use P1 and P2.
   if (uBit.i2c.redirect(uBit.io.P2, uBit.io.P1)) {
-    SetState(State::PANIC);
+    SetState(State::ERROR);
     uBit.display.print("I2C");
   }
 }

src/flight-controller/WonderKit/I2C.cpp

@@ -17,16 +17,20 @@ const uint8_t SERVO_CONTROLLER_MODE2_VALUE = 5;
 const uint8_t SERVO_CONTROLLER_MODE3_VALUE = 170;
 
 // Servo Motors
-const int NUM_MOTORS = 4;
 const uint8_t MOTOR_CW1_REG = 4;
 const uint8_t MOTOR_CCW1_REG = 2;
 const uint8_t MOTOR_CW2_REG = 3;
 const uint8_t MOTOR_CCW2_REG = 5;
+const uint8_t motorRegs[] = {MOTOR_CW1_REG, MOTOR_CW2_REG, MOTOR_CCW1_REG,
+                             MOTOR_CCW2_REG};
+
 const int MAX_ACTUATION = 255;
 const int MIN_ACTUATION = 0;
 
+static uint8_t lastPropellerActuation[NUM_MOTORS] = {0, 0, 0, 0};
+
 static void failSafe() {
-  SetState(State::PANIC);
+  SetState(State::ERROR);
   uBit.display.print("NM"); // No Motor detected
 }
 
@@ -36,7 +40,7 @@ static void WriteToServoController(uint8_t reg, uint8_t value) {
   }
 }
 
-static void ReadServoController() {
+static void readServoController() {
   uint8_t buf[2] = {0, 0};
   if (uBit.i2c.read(SERVO_CONTROLLER_ADDR, buf, 2)) {
     failSafe();
@@ -46,7 +50,7 @@ static void ReadServoController() {
   }
 }
 
-static void SetPropellerActuation(uint8_t motor, uint8_t actuation) {
+static void WritePropellerActuation(uint8_t motor, uint8_t actuation) {
   actuation = Clamp(actuation, MIN_ACTUATION, MAX_ACTUATION);
   if (uBit.i2c.writeRegister(SERVO_CONTROLLER_ADDR, motor, actuation)) {
     failSafe();
@@ -56,14 +60,15 @@ static void SetPropellerActuation(uint8_t motor, uint8_t actuation) {
   uBit.sleep(10);
 }
 
-void SetAllPropellerActuation(uint8_t cw1_v, uint8_t cw2_v, uint8_t ccw1_v,
-                              uint8_t ccw2_v) {
-  const uint8_t motorRegs[] = {MOTOR_CW1_REG, MOTOR_CW2_REG, MOTOR_CCW1_REG,
-                               MOTOR_CCW2_REG};
-  const uint8_t motorValues[] = {cw1_v, cw2_v, ccw1_v, ccw2_v};
-
+void SetPropellerActuation() {
+  const uint8_t *motorValues = GetFlightState().propellerActuation;
   for (int i = 0; i < NUM_MOTORS; ++i) {
-    SetPropellerActuation(motorRegs[i], motorValues[i]);
+    if (motorValues[i] == lastPropellerActuation[i]) {
+      continue; // Skip writing to the servo controller if the actuation value
+                // hasn't changed
+    }
+    WritePropellerActuation(motorRegs[i], motorValues[i]);
+    lastPropellerActuation[i] = motorValues[i];
   }
 }
 
@@ -75,7 +80,8 @@ void InitServoController() {
                          SERVO_CONTROLLER_MODE2_VALUE);
   WriteToServoController(SERVO_CONTROLLER_MODE3_REG,
                          SERVO_CONTROLLER_MODE3_VALUE);
-  SetAllPropellerActuation(0, 0, 0, 0);
+  SetPropellerActuation(); // Set all motors to 0 actuation due to initial
+                           // state.
   // Detect WonderKit powered.
-  ReadServoController();
+  readServoController();
 }

src/flight-controller/WonderKit/servoController.cpp

@@ -3,5 +3,4 @@
 
 void InitServoController();
 
-void SetAllPropellerActuation(uint8_t cw1_v, uint8_t cw2_v, uint8_t ccw1_v,
-                              uint8_t ccw2_v);
+void SetPropellerActuation();

src/flight-controller/WonderKit/servoController.h

@@ -1,13 +1,13 @@
 #include "flightController.h"
+#include "../utilities.h"
 
 static FlightState flightState{
   altitude : 0,
   state : State::CALIBRATING,
-  motorActuation : {0, 0, 0, 0},
+  propellerActuation : {0, 0, 0, 0}, // cw1_v, cw2_v, ccw1_v, ccw2_v
 };
 
 void InitFlightController() {
-  SetState(State::CALIBRATING);
   InitBatteryInfo();
   InitI2C(); // NB! I2C must be initialized before the servo controller, as the
              // servo controller relies on I2C communication.
@@ -16,4 +16,34 @@ void InitFlightController() {
 
 const FlightState &GetFlightState() { return flightState; }
 
-void SetState(State state) { flightState.state = state; }
+void SetState(State state) { flightState.state = state; }
+
+void UpdatePropellerActuation(MotorIndex motorIndex, uint8_t actuation) {
+  flightState.propellerActuation[static_cast<uint8_t>(motorIndex)] = actuation;
+}
+
+void UpdatePropellerActuationEqual(uint8_t actuation) {
+  for (int i = 0; i < NUM_MOTORS; ++i) {
+    UpdatePropellerActuation(static_cast<MotorIndex>(i), actuation);
+  }
+  SetPropellerActuation();
+}
+
+void CheckFlightState() {
+  if (GetFlightState().state == State::ARMED) {
+    SetState(IsDroneArmed() ? State::ARMED : State::IDLE);
+    return;
+  }
+  if (GetBatteryState().isCharging) { // If the battery is charging, set the
+                                      // state to CHARGING again
+    SetState(State::CHARGING);
+    return;
+  }
+  if (GetBatteryState().batteryLevel == BatteryLevel::EMPTY ||
+      GetBatteryState().batteryLevel == BatteryLevel::LOW) {
+    SetState(State::LOWBATTERY);
+    return;
+  } else {
+    SetState(IsDroneArmed() ? State::ARMED : State::IDLE);
+  }
+}

src/flight-controller/flightController.cpp

@@ -7,6 +7,7 @@
 #include "ultrasonicSensor.h"
 #include "view.h"
 #include <MicroBit.h>
+#include <cstdint>
 
 // FlightController.h is the umbrella header for the flight controller module,
 // which includes all the necessary headers for the flight controller
@@ -18,12 +19,28 @@
 
 #define MICROBIT_UBIT_AS_STATIC_OBJECT
 
-enum class State : uint8_t { CALIBRATING, CHARGING, ARMED, DISARMED, PANIC };
+const int NUM_MOTORS = 4;
+
+enum class State : uint8_t {
+  CALIBRATING,
+  CHARGING,
+  LOWBATTERY,
+  ARMED,
+  IDLE,
+  ERROR,
+};
+
+enum class MotorIndex : uint8_t {
+  CW1,  // Upper left motor
+  CW2,  // Lower right motor
+  CCW1, // Upper right motor
+  CCW2  // Lower left motor
+};
 
 struct FlightState {
   int altitude;
   State state;
-  uint8_t motorActuation[4];
+  uint8_t propellerActuation[NUM_MOTORS];
 };
 
 #ifdef MICROBIT_UBIT_AS_STATIC_OBJECT
@@ -39,4 +56,10 @@ void SetState(State state);
 
 void InitFlightController();
 
+void UpdatePropellerActuation(MotorIndex motorIndex, uint8_t actuation);
+
+void UpdatePropellerActuationEqual(uint8_t actuation);
+
+void CheckFlightState();
+
 #endif

src/flight-controller/flightController.h

@@ -6,9 +6,8 @@ const uint8_t ARMED_THROTTLE = 5;
 void SetThrottle() {
   uint8_t throttle = GetDroneThrottle() * 2.55;
   if (throttle == 0) {
-    SetAllPropellerActuation(ARMED_THROTTLE, ARMED_THROTTLE, ARMED_THROTTLE,
-                             ARMED_THROTTLE);
+    UpdatePropellerActuationEqual(ARMED_THROTTLE);
   } else {
-    SetAllPropellerActuation(throttle, throttle, throttle, throttle);
+    UpdatePropellerActuationEqual(throttle);
   }
-}
+}

src/flight-controller/pid.cpp

@@ -8,47 +8,39 @@
 // MicroBit display
 const uint16_t LED_DISPLAY_SIZE = 5;
 const int LED_ON = 255;
+const int CHARGING_COLUMN = 4;
 const Image ICON_BATTERY_NEEDS_CHARGING =
     Image("0,255,255,255,0\n255,0,255,0,255\n255,255,255,255,255\n0,255,0,255,"
           "0\n0,255,0,255,0\n");
 const Image ICON_BATTERY_CHARGING =
     Image("0,0,255,0,0\n0,255,255,255,0\n0,255,255,255,0\n0,255,255,255,0\n0,"
           "255,255,255,0\n");
 
-static void addChargingColumn(int column, Image &ledDisplay) {
-  if (column < 0 || column >= LED_DISPLAY_SIZE) {
-    return;
-  }
+static void addChargingColumn(Image &ledDisplay) {
   int y = static_cast<int>(GetBatteryState().batteryLevel);
   for (int i = 4; i >= y; i--) {
-    ledDisplay.setPixelValue(column, i, LED_ON);
-  }
-}
-
-static void viewBatteryLevel(Image &ledDisplay) {
-  const BatteryState &batteryState = GetBatteryState();
-  if (batteryState.isCharging) {
-    ledDisplay = ICON_BATTERY_CHARGING;
-    return;
+    ledDisplay.setPixelValue(CHARGING_COLUMN, i, LED_ON);
   }
-
-  if (batteryState.batteryLevel == BatteryLevel::EMPTY ||
-      batteryState.batteryLevel == BatteryLevel::LOW) {
-    ledDisplay = ICON_BATTERY_NEEDS_CHARGING;
-    return;
-  }
-
-  addChargingColumn(4, ledDisplay);
 }
 
-void UpdateView() {
+void UpdateViewIdle() {
   uBit.display.clear();
 
   Image ledDisplay = Image(LED_DISPLAY_SIZE, LED_DISPLAY_SIZE);
-  viewBatteryLevel(ledDisplay);
+  addChargingColumn(ledDisplay);
   DisplayArmed(IsDroneArmed(), ledDisplay);
   DisplayThrottle(GetDroneThrottle(), ledDisplay);
   DisplayPitchRoll(GetDroneRoll(), GetDronePitch(), ledDisplay);
   DisplayYaw(GetDroneYaw(), ledDisplay);
   uBit.display.print(ledDisplay);
 }
+
+void UpdateViewCharging() {
+  uBit.display.clear();
+  uBit.display.print(ICON_BATTERY_CHARGING);
+}
+
+void UpdateViewBatteryLow() {
+  uBit.display.clear();
+  uBit.display.print(ICON_BATTERY_NEEDS_CHARGING);
+}

src/flight-controller/view.cpp

@@ -1,3 +1,5 @@
 #pragma once
 
-void UpdateView();
+void UpdateViewIdle();
+void UpdateViewCharging();
+void UpdateViewBatteryLow();