From b2733157ebb73054ef31048aa99ab3bb2738dd3f Mon Sep 17 00:00:00 2001 From: Geir Ola Tvinnereim Date: Sun, 19 Jul 2026 13:02:24 +0200 Subject: [PATCH 1/3] flight-controller: Make wonderkit folder Update servoController.cpp --- src/flight-controller/WonderKit/I2C.cpp | 13 +++ src/flight-controller/WonderKit/I2C.h | 5 ++ .../WonderKit/servoController.cpp | 81 +++++++++++++++++++ .../{ => WonderKit}/servoController.h | 0 src/flight-controller/servoController.cpp | 69 ---------------- 5 files changed, 99 insertions(+), 69 deletions(-) create mode 100644 src/flight-controller/WonderKit/I2C.cpp create mode 100644 src/flight-controller/WonderKit/I2C.h create mode 100644 src/flight-controller/WonderKit/servoController.cpp rename src/flight-controller/{ => WonderKit}/servoController.h (100%) delete mode 100644 src/flight-controller/servoController.cpp diff --git a/src/flight-controller/WonderKit/I2C.cpp b/src/flight-controller/WonderKit/I2C.cpp new file mode 100644 index 0000000..bdc9920 --- /dev/null +++ b/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"); + } +} \ No newline at end of file diff --git a/src/flight-controller/WonderKit/I2C.h b/src/flight-controller/WonderKit/I2C.h new file mode 100644 index 0000000..ea216ab --- /dev/null +++ b/src/flight-controller/WonderKit/I2C.h @@ -0,0 +1,5 @@ +#pragma once + +#include + +void InitI2C(); \ No newline at end of file diff --git a/src/flight-controller/WonderKit/servoController.cpp b/src/flight-controller/WonderKit/servoController.cpp new file mode 100644 index 0000000..394b33d --- /dev/null +++ b/src/flight-controller/WonderKit/servoController.cpp @@ -0,0 +1,81 @@ +#include "servoController.h" +#include "../flightController.h" +#include "../utilities.h" +#include "I2C.h" +#include + +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(); +} \ No newline at end of file diff --git a/src/flight-controller/servoController.h b/src/flight-controller/WonderKit/servoController.h similarity index 100% rename from src/flight-controller/servoController.h rename to src/flight-controller/WonderKit/servoController.h diff --git a/src/flight-controller/servoController.cpp b/src/flight-controller/servoController.cpp deleted file mode 100644 index e33d874..0000000 --- a/src/flight-controller/servoController.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include "servoController.h" -#include "flightController.h" -#include - -// I2C -const uint16_t SERVO_CONTROLLER_ADDR = 196; // 8-bit SLAVE ADDRESS -MicroBitPin SDA = uBit.io.P2; -MicroBitPin SCL = uBit.io.P1; - -// Servo Motors -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; - -static void FailSafe() { - SetState(State::PANIC); - uBit.display.scroll("I2C"); -} - -static void WriteToServoController(uint8_t reg, uint8_t value) { - uint8_t buf[2] = {reg, value}; - if (uBit.i2c.write(SERVO_CONTROLLER_ADDR, buf, 2)) { - FailSafe(); - } -} - -static void ReadServoController(uint8_t reg, uint8_t value) { - uint8_t buf[2] = {reg, value}; - if (uBit.i2c.write(SERVO_CONTROLLER_ADDR, buf, 2)) { - FailSafe(); - } -} - -static void SetPropellerActuation(uint8_t motor, uint8_t actuation) { - uint8_t buf[2] = {motor, actuation}; - if (uBit.i2c.write(SERVO_CONTROLLER_ADDR, buf, 2)) { - SetState(State::PANIC); - uBit.display.scroll("Motor Error"); - } -} - -void SetAllPropellerActuation(uint8_t cw1_v, uint8_t cw2_v, uint8_t ccw1_v, - uint8_t ccw2_v) { - SetPropellerActuation(MOTOR_CW1_REG, cw1_v); - SetPropellerActuation(MOTOR_CW2_REG, cw2_v); - SetPropellerActuation(MOTOR_CCW1_REG, ccw1_v); - SetPropellerActuation(MOTOR_CCW2_REG, ccw2_v); -} - -void InitServoController() { - // Redirect I2C to use P1 and P2. - if (uBit.i2c.redirect(uBit.io.P2, uBit.io.P1)) { - FailSafe(); - } - - // Configure the servo controller with default values. - WriteToServoController(0, 128); - WriteToServoController(1, 5); - WriteToServoController(8, 170); - SetAllPropellerActuation(0, 0, 0, 0); - uint8_t buf[2] = {0, 0}; - if (uBit.i2c.read(SERVO_CONTROLLER_ADDR, buf, 2)) { - FailSafe(); - } else { - uBit.display.print("M"); - uBit.sleep(3000); - } -} \ No newline at end of file From c2b547fb6903cddb35666163b3fdded134657e9a Mon Sep 17 00:00:00 2001 From: Geir Ola Tvinnereim Date: Sun, 19 Jul 2026 13:03:02 +0200 Subject: [PATCH 2/3] src: Moving clamp to utilities Update utilites.cpp --- src/hand-controller/orientation.cpp | 10 ++-------- src/utilites.cpp | 4 ++++ src/utilities.h | 4 +++- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/hand-controller/orientation.cpp b/src/hand-controller/orientation.cpp index 01c916a..9451a78 100644 --- a/src/hand-controller/orientation.cpp +++ b/src/hand-controller/orientation.cpp @@ -1,5 +1,6 @@ #include "orientation.h" #include "handController.h" +#include "../utilities.h" #include // orientation variables: @@ -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 && diff --git a/src/utilites.cpp b/src/utilites.cpp index 56c6fb2..ffdf47d 100644 --- a/src/utilites.cpp +++ b/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(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; } \ No newline at end of file diff --git a/src/utilities.h b/src/utilities.h index 18d581e..a55ef41 100644 --- a/src/utilities.h +++ b/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); \ No newline at end of file +void DisplayYaw(int yaw, Image &ledDisplay); + +int Clamp(int value, int min, int max); \ No newline at end of file From 0a36da8398413d6f2417bf55e7673d447bf66343 Mon Sep 17 00:00:00 2001 From: Geir Ola Tvinnereim Date: Sun, 19 Jul 2026 13:15:21 +0200 Subject: [PATCH 3/3] flight-controller: apply throttle in armed mode --- src/flight-controller/README.md | 15 ++++++++++++-- src/flight-controller/flightController.cpp | 8 ++++++++ src/flight-controller/flightController.h | 8 ++++++-- src/flight-controller/pid.cpp | 14 +++++++++++++ src/flight-controller/pid.h | 3 +++ src/hand-controller/orientation.cpp | 2 +- src/main.cpp | 23 ++++++++++++++++------ 7 files changed, 62 insertions(+), 11 deletions(-) create mode 100644 src/flight-controller/pid.cpp create mode 100644 src/flight-controller/pid.h diff --git a/src/flight-controller/README.md b/src/flight-controller/README.md index 5449381..0b93dc6 100644 --- a/src/flight-controller/README.md +++ b/src/flight-controller/README.md @@ -28,7 +28,9 @@ 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. @@ -36,4 +38,13 @@ The I2C functionality availiable in the sdk can be found [here](../../microbit-v ![I2C setup](../../img/i2c.png) -![Motor setup](../../img/motor_registers.png) \ No newline at end of file +![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. + diff --git a/src/flight-controller/flightController.cpp b/src/flight-controller/flightController.cpp index 23813be..ce6306c 100644 --- a/src/flight-controller/flightController.cpp +++ b/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; } \ No newline at end of file diff --git a/src/flight-controller/flightController.h b/src/flight-controller/flightController.h index 85e02e7..43ac274 100644 --- a/src/flight-controller/flightController.h +++ b/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 @@ -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 \ No newline at end of file diff --git a/src/flight-controller/pid.cpp b/src/flight-controller/pid.cpp new file mode 100644 index 0000000..1103704 --- /dev/null +++ b/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); + } +} \ No newline at end of file diff --git a/src/flight-controller/pid.h b/src/flight-controller/pid.h new file mode 100644 index 0000000..3e70193 --- /dev/null +++ b/src/flight-controller/pid.h @@ -0,0 +1,3 @@ +#pragma once + +void SetThrottle(); \ No newline at end of file diff --git a/src/hand-controller/orientation.cpp b/src/hand-controller/orientation.cpp index 9451a78..4fec86f 100644 --- a/src/hand-controller/orientation.cpp +++ b/src/hand-controller/orientation.cpp @@ -1,6 +1,6 @@ #include "orientation.h" -#include "handController.h" #include "../utilities.h" +#include "handController.h" #include // orientation variables: diff --git a/src/main.cpp b/src/main.cpp index 9a27852..3e9d60a 100644 --- a/src/main.cpp +++ b/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); } }