Got/flight controller/clean servocontroller

src/flight-controller/README.md

@@ -28,12 +28,23 @@ Reading from the Pin P0 is a noisy measurement so a logical lowpass filter is im
 
 An charge plug icon is displayed when the battery needs charging, during the charging a battery icon is displayed.
 
-### Servo Motor & IMU (WonderKit Black board)
+## WonderKit Black board
+
+### Servo Motor & IMU 
 
 In order to apply actuation to the system, the MicroBit v2 is connected to a ServoMotor via its pins. The I2C Protocol (Inter-Integrated Circuit) is implemented as communication layer between the two modules using P2 (SDA) and P1 (SCL). Based on this serial-connection the Microbit can read and write actuation to the propellers. By writing to the Servo Motor registers, the servo motor will apply a PWM signal to the properllers. 
 
 The I2C functionality availiable in the sdk can be found [here](../../microbit-v2-sdk/libraries/codal-nrf52/source/NRF52I2C.cpp)
 
 ![I2C setup](../../img/i2c.png)
 
-![Motor setup](../../img/motor_registers.png)
+![Motor setup](../../img/motor_registers.png)
+
+### LED Display
+
+- If not able to setup I2C, "I2C" will be displayed on the screen.
+
+**Servo motor connection**
+- If MicroBit is able to read/write to servo motor, "M" is displayed on the screen.
+- - If *no connection* to servo motor, "NM" will be displayed on the screen.
+

src/flight-controller/WonderKit/I2C.cpp

@@ -0,0 +1,13 @@
+#include "I2C.h"
+#include "../flightController.h"
+
+MicroBitPin SDA = uBit.io.P2;
+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);
+    uBit.display.print("I2C");
+  }
+}

src/flight-controller/WonderKit/I2C.h

@@ -0,0 +1,5 @@
+#pragma once
+
+#include <MicroBit.h>
+
+void InitI2C();

src/flight-controller/WonderKit/servoController.cpp

@@ -0,0 +1,81 @@
+#include "servoController.h"
+#include "../flightController.h"
+#include "../utilities.h"
+#include "I2C.h"
+#include <MicroBit.h>
+
+const uint16_t SERVO_CONTROLLER_ADDR = 196; // 8-bit SLAVE ADDRESS
+
+// Servo controller registers
+const uint8_t SERVO_CONTROLLER_MODE1_REG = 0;
+const uint8_t SERVO_CONTROLLER_MODE2_REG = 1;
+const uint8_t SERVO_CONTROLLER_MODE3_REG = 8;
+// Servo controller register values
+// https://github.com/gomakekit/Airbit_V2/blob/master/custom.ts#L459
+const uint8_t SERVO_CONTROLLER_MODE1_VALUE = 128;
+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 int MAX_ACTUATION = 255;
+const int MIN_ACTUATION = 0;
+
+static void failSafe() {
+  SetState(State::PANIC);
+  uBit.display.print("NM"); // No Motor detected
+}
+
+static void WriteToServoController(uint8_t reg, uint8_t value) {
+  if (uBit.i2c.writeRegister(SERVO_CONTROLLER_ADDR, reg, value)) {
+    failSafe();
+  }
+}
+
+static void ReadServoController() {
+  uint8_t buf[2] = {0, 0};
+  if (uBit.i2c.read(SERVO_CONTROLLER_ADDR, buf, 2)) {
+    failSafe();
+  } else {
+    uBit.display.print("M"); // Motor detected
+    uBit.sleep(3000);
+  }
+}
+
+static void SetPropellerActuation(uint8_t motor, uint8_t actuation) {
+  actuation = Clamp(actuation, MIN_ACTUATION, MAX_ACTUATION);
+  if (uBit.i2c.writeRegister(SERVO_CONTROLLER_ADDR, motor, actuation)) {
+    failSafe();
+    return;
+  }
+
+  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};
+
+  for (int i = 0; i < NUM_MOTORS; ++i) {
+    SetPropellerActuation(motorRegs[i], motorValues[i]);
+  }
+}
+
+void InitServoController() {
+  // Configure the servo controller with default values.
+  WriteToServoController(SERVO_CONTROLLER_MODE1_REG,
+                         SERVO_CONTROLLER_MODE1_VALUE);
+  WriteToServoController(SERVO_CONTROLLER_MODE2_REG,
+                         SERVO_CONTROLLER_MODE2_VALUE);
+  WriteToServoController(SERVO_CONTROLLER_MODE3_REG,
+                         SERVO_CONTROLLER_MODE3_VALUE);
+  SetAllPropellerActuation(0, 0, 0, 0);
+  // Detect WonderKit powered.
+  ReadServoController();
+}

src/flight-controller/flightController.cpp

@@ -6,6 +6,14 @@ static FlightState flightState{
   motorActuation : {0, 0, 0, 0},
 };
 
+void InitFlightController() {
+  SetState(State::CALIBRATING);
+  InitBatteryInfo();
+  InitI2C(); // NB! I2C must be initialized before the servo controller, as the
+             // servo controller relies on I2C communication.
+  InitServoController();
+}
+
 const FlightState &GetFlightState() { return flightState; }
 
 void SetState(State state) { flightState.state = state; }

src/flight-controller/flightController.h

@@ -1,7 +1,9 @@
 #pragma once
+#include "WonderKit/I2C.h"
+#include "WonderKit/servoController.h"
 #include "battery.h"
+#include "pid.h"
 #include "receiver.h"
-#include "servoController.h"
 #include "ultrasonicSensor.h"
 #include "view.h"
 #include <MicroBit.h>
@@ -16,7 +18,7 @@
 
 #define MICROBIT_UBIT_AS_STATIC_OBJECT
 
-enum class State : uint8_t { CALIBRATING, CHARGING, DISARMED, ARMED, PANIC };
+enum class State : uint8_t { CALIBRATING, CHARGING, ARMED, DISARMED, PANIC };
 
 struct FlightState {
   int altitude;
@@ -35,4 +37,6 @@ const FlightState &GetFlightState();
 
 void SetState(State state);
 
+void InitFlightController();
+
 #endif

src/flight-controller/pid.cpp

@@ -0,0 +1,14 @@
+#include "pid.h"
+#include "flightController.h"
+
+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);
+  } else {
+    SetAllPropellerActuation(throttle, throttle, throttle, throttle);
+  }
+}

src/flight-controller/pid.h

@@ -0,0 +1,3 @@
+#pragma once
+
+void SetThrottle();

src/hand-controller/orientation.cpp

@@ -1,4 +1,5 @@
 #include "orientation.h"
+#include "../utilities.h"
 #include "handController.h"
 #include <MicroBit.h>
 
@@ -8,16 +9,9 @@ static int roll = 0;
 
 const int ANGULAR_THRESHOLD = 45;
 const int STEADY_STATE_THRESHOLD = 15;
-/*
- * Clamp the variable inside interval
- */
-static int clampThreshold(int value, int threshold) {
-  return std::min(std::max(value, -threshold),
-                  threshold); // implementing std::clamp
-}
 
 static int clampAngles(int value, int threshold) {
-  int clampedValue = clampThreshold(value, threshold);
+  int clampedValue = Clamp(value, -threshold, threshold);
 
   // Set steady state reference:
   if (clampedValue > -STEADY_STATE_THRESHOLD &&

src/main.cpp

@@ -13,20 +13,31 @@ static void initRadio() {
 }
 
 static void FlightController() {
-  SetState(State::CALIBRATING);
-  InitBatteryInfo();
-  InitServoController();
+  InitFlightController();
   if (GetFlightState().state == State::PANIC) {
     uBit.sleep(10000);
-    return;
-  } else {
-    SetState(State::DISARMED);
+    return; // MicroBit will freeze
   }
+  SetState(State::DISARMED);
 
   while (true) {
     SetBatteryInfo();
     FlushRadioBuffer();
+    if (IsDroneArmed()) {
+      SetState(State::ARMED);
+    } else {
+      SetState(State::DISARMED);
+    }
     UpdateView();
+
+    switch (GetFlightState().state) {
+    case State::ARMED:
+      SetThrottle();
+      break;
+    default:
+      SetAllPropellerActuation(0, 0, 0, 0);
+      break;
+    }
     uBit.sleep(100);
   }
 }

src/utilites.cpp

@@ -46,4 +46,8 @@ void DisplayPitchRoll(int roll, int pitch, Image &ledDisplay) {
 void DisplayYaw(int yaw, Image &ledDisplay) {
   uint16_t x = static_cast<uint16_t>(yaw / 30 + 2);
   ledDisplay.setPixelValue(x, 0, LED_ON);
+}
+
+int Clamp(int value, int min, int max) {
+  return (value < min) ? min : (value > max) ? max : value;
 }

src/utilities.h

@@ -7,4 +7,6 @@ void DisplayThrottle(int throttle, Image &ledDisplay);
 
 void DisplayPitchRoll(int roll, int pitch, Image &ledDisplay);
 
-void DisplayYaw(int yaw, Image &ledDisplay);
+void DisplayYaw(int yaw, Image &ledDisplay);
+
+int Clamp(int value, int min, int max);