Fix

To prevent i term windup.

.github/workflows/build.yml

@@ -23,7 +23,9 @@ jobs:
       with:
         name: firmware-binary
         path: flix/build
-    - name: Build firmware without Wi-Fi
+    - name: Build firmware for ESP32-S3
+      run: make BOARD=esp32:esp32:esp32s3
+    - name: Build firmware with WiFi disabled
       run: sed -i 's/^#define WIFI_ENABLED 1$/#define WIFI_ENABLED 0/' flix/flix.ino && make
     - name: Check c_cpp_properties.json
       run: tools/check_c_cpp_properties.py
@@ -53,15 +55,25 @@ jobs:
       run: python3 tools/check_c_cpp_properties.py
 
   build_simulator:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-latest
+    container:
+      image: ubuntu:20.04
     steps:
+    - name: Install dependencies
+      run: |
+        apt-get update
+        DEBIAN_FRONTEND=noninteractive apt-get install -y curl wget build-essential cmake g++ pkg-config gnupg2 lsb-release sudo
     - name: Install Arduino CLI
       uses: arduino/setup-arduino-cli@v1.1.1
     - uses: actions/checkout@v4
     - name: Install Gazebo
-      run: curl -sSL http://get.gazebosim.org | sh
+      run: |
+        sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+        wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+        sudo apt-get update
+        sudo apt-get install -y gazebo11 libgazebo11-dev
     - name: Install SDL2
-      run: sudo apt-get install libsdl2-dev
+      run: sudo apt-get install -y libsdl2-dev
     - name: Build simulator
       run: make build_simulator
     - uses: actions/upload-artifact@v4

.vscode/c_cpp_properties.json

@@ -5,31 +5,20 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
+				"${workspaceFolder}/tools/**",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
 				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
 				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
 				"~/Arduino/libraries/**",
-				"/usr/include/**"
+				"/usr/include/gazebo-11/",
+				"/usr/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h"
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -51,32 +40,19 @@
 			"name": "Mac",
 			"includePath": [
 				"${workspaceFolder}/flix",
-				// "${workspaceFolder}/gazebo",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
 				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/include/**",
 				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**",
-				"/opt/homebrew/include/**"
+				"/opt/homebrew/include/gazebo-11/",
+				"/opt/homebrew/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h"
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -100,6 +76,7 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
+				"${workspaceFolder}/tools/**",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
@@ -110,20 +87,7 @@
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h"
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",

.vscode/settings.json

@@ -1,5 +1,6 @@
 {
 	"C_Cpp.intelliSenseEngineFallback": "enabled",
+	"C_Cpp.errorSquiggles": "disabled",
 	"files.associations": {
 		"*.sdf": "xml",
 		"*.ino": "cpp",

README.md

@@ -87,7 +87,7 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 |Tape, double-sided tape||||
 
 *² — barometer is not used for now.*<br>
-*³ — change `MPU9250` to `ICM20948` in `imu.ino` file if using ICM-20948 board.*<br>
+*³ — change `MPU9250` to `ICM20948` or `MPU6050` in `imu.ino` file for using the appropriate boards.*<br>
 *³⁻¹ — MPU-6050 supports I²C interface only (not recommended). To use it change IMU declaration to `MPU6050 IMU(Wire)`.*<br>
 *⁴ — this frame is optimized for GY-91 board, if using other, the board mount holes positions should be modified.*<br>
 *⁵ — you also may use any transmitter-receiver pair with SBUS interface.*

docs/firmware.md

@@ -8,7 +8,7 @@ The firmware is a regular Arduino sketch, and it follows the classic Arduino one
 
 The main loop is running at 1000 Hz. All the dataflow goes through global variables (for simplicity):
 
-* `t` *(double)* — current step time, *s*.
+* `t` *(float)* — current step time, *s*.
 * `dt` *(float)* — time delta between the current and previous steps, *s*.
 * `gyro` *(Vector)* — data from the gyroscope, *rad/s*.
 * `acc` *(Vector)* — acceleration data from the accelerometer, *m/s<sup>2</sup>*.

docs/troubleshooting.md

@@ -15,7 +15,7 @@ Do the following:
 * **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
 * **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
 * **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
-* **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere in the `imu.ino` file.
+* **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
 * **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
 * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
 * **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.

docs/usage.md

@@ -13,7 +13,7 @@ cd flix
 
 ### Ubuntu
 
-The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you have a newer version, consider using a virtual machine.
+The latest version of Ubuntu supported by Gazebo 11 simulator is 20.04. If you have a newer version, consider using a virtual machine.
 
 1. Install Arduino CLI:
 
@@ -24,7 +24,10 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you h
 2. Install Gazebo 11:
 
    ```bash
-   curl -sSL http://get.gazebosim.org | sh
+   sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+   wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+   sudo apt-get update
+   sudo apt-get install -y gazebo11 libgazebo11-dev
    ```
 
    Set up your Gazebo environment variables:

flix/cli.cpp

@@ -3,16 +3,16 @@
 
 // Implementation of command line interface
 
+#include <Arduino.h>
+#include "flix.h"
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern const int ACRO, STAB, AUTO;
-extern float loopRate, dt;
+extern float t, dt, loopRate;
-extern double t;
 extern uint16_t channels[16];
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
 extern int mode;
 extern bool armed;
 
@@ -60,7 +60,7 @@ void print(const char* format, ...) {
 }
 
 void pause(float duration) {
-	double start = t;
+	float start = t;
 	while (t - start < duration) {
 		step();
 		handleInput();
@@ -71,7 +71,7 @@ void pause(float duration) {
 	}
 }
 
-void doCommand(String str, bool echo = false) {
+void doCommand(String str, bool echo) {
 	// parse command
 	String command, arg0, arg1;
 	splitString(str, command, arg0, arg1);

flix/config.h

@@ -0,0 +1,55 @@
+// Wi-Fi
+#define WIFI_ENABLED 1
+#define WIFI_SSID "flix"
+#define WIFI_PASSWORD "flixwifi"
+#define WIFI_UDP_PORT 14550
+#define WIFI_UDP_REMOTE_PORT 14550
+#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
+
+// Motors
+#define MOTOR_0_PIN 12 // rear left
+#define MOTOR_1_PIN 13 // rear right
+#define MOTOR_2_PIN 14 // front right
+#define MOTOR_3_PIN 15 // front left
+#define PWM_FREQUENCY 78000
+#define PWM_RESOLUTION 10
+#define PWM_STOP 0
+#define PWM_MIN 0
+#define PWM_MAX 1000000 / PWM_FREQUENCY
+
+// Control
+#define PITCHRATE_P 0.05
+#define PITCHRATE_I 0.2
+#define PITCHRATE_D 0.001
+#define PITCHRATE_I_LIM 0.3
+#define ROLLRATE_P PITCHRATE_P
+#define ROLLRATE_I PITCHRATE_I
+#define ROLLRATE_D PITCHRATE_D
+#define ROLLRATE_I_LIM PITCHRATE_I_LIM
+#define YAWRATE_P 0.3
+#define YAWRATE_I 0.0
+#define YAWRATE_D 0.0
+#define YAWRATE_I_LIM 0.3
+#define ROLL_P 6
+#define ROLL_I 0
+#define ROLL_D 0
+#define PITCH_P ROLL_P
+#define PITCH_I ROLL_I
+#define PITCH_D ROLL_D
+#define YAW_P 3
+#define PITCHRATE_MAX radians(360)
+#define ROLLRATE_MAX radians(360)
+#define YAWRATE_MAX radians(300)
+#define TILT_MAX radians(30)
+#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
+
+// Estimation
+#define WEIGHT_ACC 0.003
+#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
+
+// MAVLink
+#define SYSTEM_ID 1
+
+// Safety
+#define RC_LOSS_TIMEOUT 1
+#define DESCEND_TIME 10

flix/control.cpp

@@ -3,41 +3,26 @@
 
 // Flight control
 
+#include "config.h"
 #include "vector.h"
 #include "quaternion.h"
 #include "pid.h"
 #include "lpf.h"
 #include "util.h"
 
-#define ARMED_THRUST 0.1 // thrust to indicate armed state
+extern const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
-#define PITCHRATE_P 0.05
+extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-#define PITCHRATE_I 0.2
-#define PITCHRATE_D 0.001
-#define PITCHRATE_I_LIM 0.3
-#define ROLLRATE_P PITCHRATE_P
-#define ROLLRATE_I PITCHRATE_I
-#define ROLLRATE_D PITCHRATE_D
-#define ROLLRATE_I_LIM PITCHRATE_I_LIM
-#define YAWRATE_P 0.3
-#define YAWRATE_I 0.0
-#define YAWRATE_D 0.0
-#define YAWRATE_I_LIM 0.3
-#define ROLL_P 6
-#define ROLL_I 0
-#define ROLL_D 0
-#define PITCH_P ROLL_P
-#define PITCH_I ROLL_I
-#define PITCH_D ROLL_D
-#define YAW_P 3
-#define PITCHRATE_MAX radians(360)
-#define ROLLRATE_MAX radians(360)
-#define YAWRATE_MAX radians(300)
-#define TILT_MAX radians(30)
-#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
 int mode = STAB;
 bool armed = false;
+float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
+float controlTime;
+
+Quaternion attitudeTarget;
+Vector ratesTarget;
+Vector ratesExtra; // feedforward rates
+Vector torqueTarget;
+float thrustTarget;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
 PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -48,15 +33,6 @@ PID yawPID(YAW_P, 0, 0);
 Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
 float tiltMax = TILT_MAX;
 
-Quaternion attitudeTarget;
-Vector ratesTarget;
-Vector ratesExtra; // feedforward rates
-Vector torqueTarget;
-float thrustTarget;
-
-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
-
 void control() {
 	interpretControls();
 	failsafe();
@@ -80,7 +56,7 @@ void interpretControls() {
 
 	if (mode == STAB) {
 		float yawTarget = attitudeTarget.getYaw();
-		if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or pilot commands yaw rate
+		if (!armed || invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target
 		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
 		ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
 	}
@@ -100,12 +76,7 @@ void interpretControls() {
 }
 
 void controlAttitude() {
-	if (!armed || attitudeTarget.invalid()) { // skip attitude control
+	if (!armed || attitudeTarget.invalid() || thrustTarget < 0.1) return; // skip attitude control
-		rollPID.reset();
-		pitchPID.reset();
-		yawPID.reset();
-		return;
-	}
 
 	const Vector up(0, 0, 1);
 	Vector upActual = Quaternion::rotateVector(up, attitude);
@@ -113,28 +84,23 @@ void controlAttitude() {
 
 	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
-	ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;
+	ratesTarget.x = rollPID.update(error.x) + ratesExtra.x;
-	ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y;
+	ratesTarget.y = pitchPID.update(error.y) + ratesExtra.y;
 
 	float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
-	ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
+	ratesTarget.z = yawPID.update(yawError) + ratesExtra.z;
 }
 
 
 void controlRates() {
-	if (!armed || ratesTarget.invalid()) { // skip rates control
+	if (!armed || ratesTarget.invalid() || thrustTarget < 0.1) return; // skip rates control
-		rollRatePID.reset();
-		pitchRatePID.reset();
-		yawRatePID.reset();
-		return;
-	}
 
 	Vector error = ratesTarget - rates;
 
 	// Calculate desired torque, where 0 - no torque, 1 - maximum possible torque
-	torqueTarget.x = rollRatePID.update(error.x, dt);
+	torqueTarget.x = rollRatePID.update(error.x);
-	torqueTarget.y = pitchRatePID.update(error.y, dt);
+	torqueTarget.y = pitchRatePID.update(error.y);
-	torqueTarget.z = yawRatePID.update(error.z, dt);
+	torqueTarget.z = yawRatePID.update(error.z);
 }
 
 void controlTorque() {
@@ -145,12 +111,11 @@ void controlTorque() {
 		return;
 	}
 
-	if (thrustTarget < 0.05) {
+	if (thrustTarget < 0.1) {
-		// minimal thrust to indicate armed state
+		motors[0] = 0.1; // idle thrust
-		motors[0] = ARMED_THRUST;
+		motors[1] = 0.1;
-		motors[1] = ARMED_THRUST;
+		motors[2] = 0.1;
-		motors[2] = ARMED_THRUST;
+		motors[3] = 0.1;
-		motors[3] = ARMED_THRUST;
 		return;
 	}
 

flix/estimate.cpp

@@ -3,13 +3,16 @@
 
 // Attitude estimation from gyro and accelerometer
 
+#include "config.h"
+#include "flix.h"
 #include "quaternion.h"
 #include "vector.h"
 #include "lpf.h"
 #include "util.h"
 
-#define WEIGHT_ACC 0.003
+Vector rates; // filtered angular rates, rad/s
-#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
+Quaternion attitude; // estimated attitude
+bool landed; // are we landed and stationary
 
 void estimate() {
 	applyGyro();

flix/flix.h

@@ -0,0 +1,92 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// All-in-one header file
+
+#pragma once
+
+#include <Arduino.h>
+#include "vector.h"
+#include "quaternion.h"
+
+// The most used global variables:
+extern float t; // current step time, s
+extern float dt; // time delta from previous step, s
+extern Vector gyro; // gyroscope data
+extern Vector acc; // accelerometer data, m/s²
+extern Vector rates; // filtered angular rates, rad/s
+extern Quaternion attitude; // estimated attitude
+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode; // pilot inputs, range [-1, 1]
+extern float controlTime; // inputs last update time
+extern int mode;
+extern bool armed;
+extern Vector ratesTarget, ratesExtra, torqueTarget;
+extern Quaternion attitudeTarget;
+extern float thrustTarget;
+extern bool landed; // are we landed and stationary
+extern float motors[4]; // normalized motors thrust in range [0..1]
+
+void print(const char* format, ...);
+void pause(float duration);
+void doCommand(String str, bool echo = false);
+void handleInput();
+void control();
+void interpretControls();
+void controlAttitude();
+void controlRates();
+void controlTorque();
+const char *getModeName();
+void estimate();
+void applyGyro();
+void applyAcc();
+void setupIMU();
+void configureIMU();
+void readIMU();
+void rotateIMU(Vector& data);
+void calibrateGyroOnce();
+void calibrateAccel();
+void calibrateAccelOnce();
+void printIMUCalibration();
+void printIMUInfo();
+void setupLED();
+void setLED(bool on);
+void blinkLED();
+void prepareLogData();
+void logData();
+void dumpLog();
+void processMavlink();
+void sendMavlink();
+void sendMessage(const void *msg);
+void receiveMavlink();
+void handleMavlink(const void *_msg);
+void mavlinkPrint(const char* str);
+void sendMavlinkPrint();
+void setupMotors();
+int getDutyCycle(float value);
+void sendMotors();
+bool motorsActive();
+void testMotor(int n);
+void setupParameters();
+int parametersCount();
+const char *getParameterName(int index);
+float getParameter(int index);
+float getParameter(const char *name);
+bool setParameter(const char *name, const float value);
+void syncParameters();
+void printParameters();
+void resetParameters();
+void setupRC();
+bool readRC();
+void normalizeRC();
+void calibrateRC();
+void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str);
+void printRCCalibration();
+void failsafe();
+void rcLossFailsafe();
+void descend();
+void autoFailsafe();
+void step();
+void computeLoopRate();
+void setupWiFi();
+void sendWiFi(const uint8_t *buf, int len);
+int receiveWiFi(uint8_t *buf, int len);

flix/flix.ino

@@ -3,26 +3,14 @@
 
 // Main firmware file
 
+#include "config.h"
 #include "vector.h"
 #include "quaternion.h"
 #include "util.h"
-
+#include "flix.h"
-#define SERIAL_BAUDRATE 115200
-#define WIFI_ENABLED 1
-
-double t = NAN; // current step time, s
-float dt; // time delta from previous step, s
-float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
-float controlMode = NAN;
-Vector gyro; // gyroscope data
-Vector acc; // accelerometer data, m/s/s
-Vector rates; // filtered angular rates, rad/s
-Quaternion attitude; // estimated attitude
-bool landed; // are we landed and stationary
-float motors[4]; // normalized motors thrust in range [0..1]
 
 void setup() {
-	Serial.begin(SERIAL_BAUDRATE);
+	Serial.begin(115200);
 	print("Initializing flix\n");
 	disableBrownOut();
 	setupParameters();

flix/imu.cpp

@@ -4,34 +4,36 @@
 // Work with the IMU sensor
 
 #include <SPI.h>
-#include <MPU9250.h>
+#include <FlixPeriph.h>
+#include "vector.h"
 #include "lpf.h"
 #include "util.h"
 
-MPU9250 IMU(SPI);
+MPU9250 imu(SPI);
 
-Vector accBias;
+Vector gyro, gyroBias;
-Vector accScale(1, 1, 1);
+Vector acc, accBias, accScale(1, 1, 1);
-Vector gyroBias;
+
+extern float loopRate;
 
 void setupIMU() {
 	print("Setup IMU\n");
-	IMU.begin();
+	imu.begin();
 	configureIMU();
 }
 
 void configureIMU() {
-	IMU.setAccelRange(IMU.ACCEL_RANGE_4G);
+	imu.setAccelRange(imu.ACCEL_RANGE_4G);
-	IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
+	imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
-	IMU.setDLPF(IMU.DLPF_MAX);
+	imu.setDLPF(imu.DLPF_MAX);
-	IMU.setRate(IMU.RATE_1KHZ_APPROX);
+	imu.setRate(imu.RATE_1KHZ_APPROX);
-	IMU.setupInterrupt();
+	imu.setupInterrupt();
 }
 
 void readIMU() {
-	IMU.waitForData();
+	imu.waitForData();
-	IMU.getGyro(gyro.x, gyro.y, gyro.z);
+	imu.getGyro(gyro.x, gyro.y, gyro.z);
-	IMU.getAccel(acc.x, acc.y, acc.z);
+	imu.getAccel(acc.x, acc.y, acc.z);
 	calibrateGyroOnce();
 	// apply scale and bias
 	acc = (acc - accBias) / accScale;
@@ -49,9 +51,8 @@ void rotateIMU(Vector& data) {
 }
 
 void calibrateGyroOnce() {
-	static float landedTime = 0;
+	static Delay landedDelay(2);
-	landedTime = landed ? landedTime + dt : 0;
+	if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
-	if (landedTime < 2) return; // calibrate only if definitely stationary
 
 	static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
 	gyroBias = gyroCalibrationFilter.update(gyro);
@@ -59,7 +60,7 @@ void calibrateGyroOnce() {
 
 void calibrateAccel() {
 	print("Calibrating accelerometer\n");
-	IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode
+	imu.setAccelRange(imu.ACCEL_RANGE_2G); // the most sensitive mode
 
 	print("1/6 Place level [8 sec]\n");
 	pause(8);
@@ -93,9 +94,9 @@ void calibrateAccelOnce() {
 	// Compute the average of the accelerometer readings
 	acc = Vector(0, 0, 0);
 	for (int i = 0; i < samples; i++) {
-		IMU.waitForData();
+		imu.waitForData();
 		Vector sample;
-		IMU.getAccel(sample.x, sample.y, sample.z);
+		imu.getAccel(sample.x, sample.y, sample.z);
 		acc = acc + sample;
 	}
 	acc = acc / samples;
@@ -119,15 +120,16 @@ void printIMUCalibration() {
 }
 
 void printIMUInfo() {
-	IMU.status() ? print("status: ERROR %d\n", IMU.status()) : print("status: OK\n");
+	imu.status() ? print("status: ERROR %d\n", imu.status()) : print("status: OK\n");
-	print("model: %s\n", IMU.getModel());
+	print("model: %s\n", imu.getModel());
-	print("who am I: 0x%02X\n", IMU.whoAmI());
+	print("who am I: 0x%02X\n", imu.whoAmI());
 	print("rate: %.0f\n", loopRate);
 	print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
 	print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
+	imu.waitForData();
 	Vector rawGyro, rawAcc;
-	IMU.getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
+	imu.getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
-	IMU.getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
+	imu.getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
 	print("raw gyro: %f %f %f\n", rawGyro.x, rawGyro.y, rawGyro.z);
 	print("raw acc: %f %f %f\n", rawAcc.x, rawAcc.y, rawAcc.z);
 }

flix/led.cpp

@@ -3,6 +3,8 @@
 
 // Board's LED control
 
+#include <Arduino.h>
+
 #define BLINK_PERIOD 500000
 
 #ifndef LED_BUILTIN

flix/log.cpp

@@ -3,6 +3,7 @@
 
 // In-RAM logging
 
+#include "flix.h"
 #include "vector.h"
 
 #define LOG_RATE 100
@@ -10,7 +11,6 @@
 #define LOG_PERIOD 1.0 / LOG_RATE
 #define LOG_SIZE LOG_DURATION * LOG_RATE
 
-float tFloat;
 Vector attitudeEuler;
 Vector attitudeTargetEuler;
 
@@ -20,7 +20,7 @@ struct LogEntry {
 };
 
 LogEntry logEntries[] = {
-	{"t", &tFloat},
+	{"t", &t},
 	{"rates.x", &rates.x},
 	{"rates.y", &rates.y},
 	{"rates.z", &rates.z},
@@ -40,7 +40,6 @@ const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
 float logBuffer[LOG_SIZE][logColumns];
 
 void prepareLogData() {
-	tFloat = t;
 	attitudeEuler = attitude.toEuler();
 	attitudeTargetEuler = attitudeTarget.toEuler();
 }
@@ -48,7 +47,7 @@ void prepareLogData() {
 void logData() {
 	if (!armed) return;
 	static int logPointer = 0;
-	static double logTime = 0;
+	static float logTime = 0;
 	if (t - logTime < LOG_PERIOD) return;
 	logTime = t;
 

flix/lpf.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include <Arduino.h>
+
 template <typename T> // Using template to make the filter usable for scalar and vector values
 class LowPassFilter {
 public:

flix/mavlink.cpp

@@ -3,19 +3,24 @@
 
 // MAVLink communication
 
+#include <Arduino.h>
+#include "config.h"
+#include "flix.h"
+
 #if WIFI_ENABLED
 
 #include <MAVLink.h>
 
-#define SYSTEM_ID 1
 #define PERIOD_SLOW 1.0
 #define PERIOD_FAST 0.1
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
 
 String mavlinkPrintBuffer;
 
-extern double controlTime;
+extern uint16_t channels[16];
+extern float controlTime;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
+extern const int STAB, AUTO;
 
 void processMavlink() {
 	sendMavlink();
@@ -25,8 +30,8 @@ void processMavlink() {
 void sendMavlink() {
 	sendMavlinkPrint();
 
-	static double lastSlow = 0;
+	static float lastSlow = 0;
-	static double lastFast = 0;
+	static float lastFast = 0;
 
 	mavlink_message_t msg;
 	uint32_t time = t * 1000;

flix/motors.cpp

@@ -4,24 +4,16 @@
 // Motors output control using MOSFETs
 // In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
 
+#include <Arduino.h>
+#include "config.h"
+#include "flix.h"
 #include "util.h"
 
-#define MOTOR_0_PIN 12 // rear left
+float motors[4]; // normalized motors thrust in range [0..1]
-#define MOTOR_1_PIN 13 // rear right
+extern const int MOTOR_REAR_LEFT = 0;
-#define MOTOR_2_PIN 14 // front right
+extern const int MOTOR_REAR_RIGHT = 1;
-#define MOTOR_3_PIN 15 // front left
+extern const int MOTOR_FRONT_RIGHT = 2;
-
+extern const int MOTOR_FRONT_LEFT = 3;
-#define PWM_FREQUENCY 78000
-#define PWM_RESOLUTION 10
-#define PWM_STOP 0
-#define PWM_MIN 0
-#define PWM_MAX 1000000 / PWM_FREQUENCY
-
-// Motors array indexes:
-const int MOTOR_REAR_LEFT = 0;
-const int MOTOR_REAR_RIGHT = 1;
-const int MOTOR_FRONT_RIGHT = 2;
-const int MOTOR_FRONT_LEFT = 3;
 
 void setupMotors() {
 	print("Setup Motors\n");

flix/parameters.cpp

@@ -4,11 +4,17 @@
 // Parameters storage in flash memory
 
 #include <Preferences.h>
+#include "flix.h"
+#include "pid.h"
 
 extern float channelZero[16];
 extern float channelMax[16];
 extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
-extern float mavlinkControlScale;
+extern float tiltMax;
+extern PID rollPID, pitchPID, yawPID;
+extern PID rollRatePID, pitchRatePID, yawRatePID;
+extern Vector maxRate;
+extern Vector accBias, accScale;
 
 Preferences storage;
 
@@ -119,7 +125,7 @@ bool setParameter(const char *name, const float value) {
 }
 
 void syncParameters() {
-	static double lastSync = 0;
+	static float lastSync = 0;
 	if (t - lastSync < 1) return; // sync once per second
 	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
 	lastSync = t;

flix/pid.h

@@ -5,44 +5,50 @@
 
 #pragma once
 
+#include "Arduino.h"
+#include "flix.h"
 #include "lpf.h"
 
 class PID {
 public:
-	float p = 0;
+	float p, i, d;
-	float i = 0;
+	float windup;
-	float d = 0;
+	float dtMax;
-	float windup = 0;
 
 	float derivative = 0;
 	float integral = 0;
 
 	LowPassFilter<float> lpf; // low pass filter for derivative term
 
-	PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
+	PID(float p, float i, float d, float windup = 0, float dAlpha = 1, float dtMax = 0.1) :
+		p(p), i(i), d(d), windup(windup), lpf(dAlpha), dtMax(dtMax) {}
 
-	float update(float error, float dt) {
+	float update(float error) {
-		integral += error * dt;
+		float dt = t - prevTime;
 
-		if (isfinite(prevError) && dt > 0) {
+		if (dt > 0 && dt < dtMax) {
-			// calculate derivative if both dt and prevError are valid
+			integral += error * dt;
-			derivative = (error - prevError) / dt;
+			derivative = lpf.update((error - prevError) / dt); // compute derivative and apply low-pass filter
-
+		} else {
-			// apply low pass filter to derivative
+			integral = 0;
-			derivative = lpf.update(derivative);
+			derivative = 0;
 		}
 
 		prevError = error;
+		prevTime = t;
 
 		return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
 	}
 
 	void reset() {
 		prevError = NAN;
+		prevTime = NAN;
 		integral = 0;
 		derivative = 0;
+		lpf.reset();
 	}
 
 private:
 	float prevError = NAN;
+	float prevTime = NAN;
 };

flix/quaternion.h

@@ -5,6 +5,7 @@
 
 #pragma once
 
+#include <Arduino.h>
 #include "vector.h"
 
 class Quaternion : public Printable {
@@ -45,7 +46,7 @@ public:
 			cx * cy * sz - sx * sy * cz);
 	}
 
-	static Quaternion fromBetweenVectors(Vector u, Vector v) {
+	static Quaternion fromBetweenVectors(const Vector& u, const Vector& v) {
 		float dot = u.x * v.x + u.y * v.y + u.z * v.z;
 		float w1 = u.y * v.z - u.z * v.y;
 		float w2 = u.z * v.x - u.x * v.z;

flix/rc.cpp

@@ -6,10 +6,9 @@
 #include <SBUS.h>
 #include "util.h"
 
-SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
+SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
 uint16_t channels[16]; // raw rc channels
-double controlTime; // time of the last controls update
 float channelZero[16]; // calibration zero values
 float channelMax[16]; // calibration max values
 
@@ -18,12 +17,12 @@ float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel =
 
 void setupRC() {
 	print("Setup RC\n");
-	RC.begin();
+	rc.begin();
 }
 
 bool readRC() {
-	if (RC.read()) {
+	if (rc.read()) {
-		SBUSData data = RC.data();
+		SBUSData data = rc.data();
 		for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
 		normalizeRC();
 		controlTime = t;

flix/safety.cpp

@@ -3,11 +3,11 @@
 
 // Fail-safe functions
 
-#define RC_LOSS_TIMEOUT 0.5
+#include "config.h"
-#define DESCEND_TIME 3.0 // time to descend from full throttle to zero
+#include "flix.h"
 
-extern double controlTime;
+extern float controlTime;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw;
+extern const int AUTO, STAB;
 
 void failsafe() {
 	rcLossFailsafe();
@@ -16,7 +16,7 @@ void failsafe() {
 
 // RC loss failsafe
 void rcLossFailsafe() {
-	if (mode == AUTO) return;
+	if (controlTime == 0) return; // no RC at all
 	if (!armed) return;
 	if (t - controlTime > RC_LOSS_TIMEOUT) {
 		descend();
@@ -25,14 +25,12 @@ void rcLossFailsafe() {
 
 // Smooth descend on RC lost
 void descend() {
-	mode = STAB;
+	mode = AUTO;
-	controlRoll = 0;
+	attitudeTarget = Quaternion();
-	controlPitch = 0;
+	thrustTarget -= dt / DESCEND_TIME;
-	controlYaw = 0;
+	if (thrustTarget < 0) {
-	controlThrottle -= dt / DESCEND_TIME;
+		thrustTarget = 0;
-	if (controlThrottle < 0) {
 		armed = false;
-		controlThrottle = 0;
 	}
 }
 

flix/time.cpp

@@ -3,10 +3,15 @@
 
 // Time related functions
 
+#include "Arduino.h"
+#include "flix.h"
+
+float t = NAN; // current step time, s
+float dt; // time delta from previous step, s
 float loopRate; // Hz
 
 void step() {
-	double now = micros() / 1000000.0;
+	float now = micros() / 1000000.0;
 	dt = now - t;
 	t = now;
 
@@ -18,7 +23,7 @@ void step() {
 }
 
 void computeLoopRate() {
-	static double windowStart = 0;
+	static float windowStart = 0;
 	static uint32_t rate = 0;
 	rate++;
 	if (t - windowStart >= 1) { // 1 second window

flix/util.h

@@ -8,27 +8,28 @@
 #include <math.h>
 #include <soc/soc.h>
 #include <soc/rtc_cntl_reg.h>
+#include "flix.h"
 
 const float ONE_G = 9.80665;
 
-float mapf(long x, long in_min, long in_max, float out_min, float out_max) {
+inline float mapf(long x, long in_min, long in_max, float out_min, float out_max) {
 	return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
 }
 
-float mapff(float x, float in_min, float in_max, float out_min, float out_max) {
+inline float mapff(float x, float in_min, float in_max, float out_min, float out_max) {
 	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-bool invalid(float x) {
+inline bool invalid(float x) {
 	return !isfinite(x);
 }
 
-bool valid(float x) {
+inline bool valid(float x) {
 	return isfinite(x);
 }
 
 // Wrap angle to [-PI, PI)
-float wrapAngle(float angle) {
+inline float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);
 	if (angle > PI) {
 		angle -= 2 * PI;
@@ -39,12 +40,12 @@ float wrapAngle(float angle) {
 }
 
 // Disable reset on low voltage
-void disableBrownOut() {
+inline void disableBrownOut() {
 	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
 }
 
 // Trim and split string by spaces
-void splitString(String& str, String& token0, String& token1, String& token2) {
+inline void splitString(String& str, String& token0, String& token1, String& token2) {
 	str.trim();
 	char chars[str.length() + 1];
 	str.toCharArray(chars, str.length() + 1);
@@ -52,3 +53,23 @@ void splitString(String& str, String& token0, String& token1, String& token2) {
 	token1 = strtok(NULL, " "); // String(NULL) creates empty string
 	token2 = strtok(NULL, "");
 }
+
+// Delay filter for boolean signals - ensures the signal is on for at least 'delay' seconds
+class Delay {
+public:
+	float delay;
+	float start = NAN;
+
+	Delay(float delay) : delay(delay) {}
+
+	bool update(bool on) {
+		if (!on) {
+			start = NAN;
+			return false;
+		}
+		if (isnan(start)) {
+			start = t;
+		}
+		return t - start >= delay;
+	}
+};

flix/vector.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include <Arduino.h>
+
 class Vector : public Printable {
 public:
 	float x, y, z;
@@ -124,5 +126,5 @@ public:
 	}
 };
 
-Vector operator * (const float a, const Vector& b) { return b * a; }
+inline Vector operator * (const float a, const Vector& b) { return b * a; }
-Vector operator + (const float a, const Vector& b) { return b + a; }
+inline Vector operator + (const float a, const Vector& b) { return b + a; }

flix/wifi.cpp

@@ -3,18 +3,15 @@
 
 // Wi-Fi support
 
+#include "config.h"
+#include "flix.h"
+
 #if WIFI_ENABLED
 
 #include <WiFi.h>
 #include <WiFiAP.h>
 #include <WiFiUdp.h>
 
-#define WIFI_SSID "flix"
-#define WIFI_PASSWORD "flixwifi"
-#define WIFI_UDP_PORT 14550
-#define WIFI_UDP_REMOTE_PORT 14550
-#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
-
 WiFiUDP udp;
 
 void setupWiFi() {

gazebo/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 project(flix_gazebo)
 
-# === gazebo plugin
+# Gazebo plugin
 find_package(gazebo REQUIRED)
 find_package(SDL2 REQUIRED)
 include_directories(${GAZEBO_INCLUDE_DIRS})

gazebo/flix.h

@@ -12,7 +12,7 @@
 
 #define WIFI_ENABLED 1
 
-double t = NAN;
+float t = NAN;
 float dt;
 float motors[4];
 float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;

gazebo/simulator.cpp

@@ -21,7 +21,7 @@
 #include "cli.ino"
 #include "control.ino"
 #include "estimate.ino"
-#include "failsafe.ino"
+#include "safety.ino"
 #include "log.ino"
 #include "lpf.h"
 #include "mavlink.ino"
@@ -59,6 +59,7 @@ public:
 
 	void OnReset() {
 		attitude = Quaternion(); // reset estimated attitude
+		armed = false;
 		__resetTime += __micros;
 		gzmsg << "Flix plugin reset" << endl;
 	}

tools/check_c_cpp_properties.py

@@ -49,6 +49,8 @@ for configuration in props['configurations']:
     print('Check configuration', configuration['name'])
 
     for include_path in configuration.get('includePath', []):
+        if include_path.startswith('/opt/') or include_path.startswith('/usr/'): # don't check non-Arduino libs
+            continue
         check_path(include_path)
 
     for forced_include in configuration.get('forcedInclude', []):