Initial commit

.editorconfig

@@ -0,0 +1,11 @@
+root = true
+
+[*]
+end_of_line = lf
+insert_final_newline = true
+
+[*.{ino,cpp,c,h,hpp,sdf,world}]
+charset = utf-8
+indent_style = tab
+tab_width = 4
+trim_trailing_whitespace = true

.github/workflows/build.yml

@@ -0,0 +1,30 @@
+name: Build
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v3
+    - name: Install Arduino CLI
+      uses: arduino/setup-arduino-cli@v1.1.1
+    - name: Install dependencies
+      run: make dependencies
+    - name: Build firmware
+      run: make
+
+  build_simulator:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v3
+    - name: Install Gazebo
+      run: curl -sSL http://get.gazebosim.org | sh
+    - name: Install SDL2
+      run: sudo apt-get install libsdl2-dev
+    - name: Build simulator
+      run: make build_simulator

.gitignore

@@ -0,0 +1,4 @@
+*.hex
+*.elf
+gazebo/build/
+tools/log/

Makefile

@@ -0,0 +1,40 @@
+SKETCH = flix
+BOARD := esp32:esp32:d1_mini32
+FILE = arduino.avr.nano
+PORT := /dev/cu.usbserial-01E2D770
+
+build:
+# arduino-cli compile --fqbn $(BOARD) --build-path $(SKETCH)/build --build-cache-path $(SKETCH)/cache $(SKETCH)
+	arduino-cli compile --fqbn $(BOARD) $(SKETCH)
+
+upload: build
+	arduino-cli upload --fqbn $(BOARD) -p $(PORT) $(SKETCH)
+
+monitor:
+	arduino-cli monitor -p $(PORT) -c baudrate=115200
+
+dependencies:
+	arduino-cli core update-index
+	arduino-cli core install esp32:esp32
+	arduino-cli lib install 'Bolder Flight Systems SBUS'@1.0.1
+	arduino-cli lib install --git-url https://github.com/okalachev/MPU9250.git
+
+gazebo/build cmake: gazebo/CMakeLists.txt
+	mkdir -p gazebo/build
+	cd gazebo/build && cmake ..
+
+build_simulator: gazebo/build
+	make -C gazebo/build
+
+simulator: build_simulator
+	GAZEBO_MODEL_PATH=$$GAZEBO_MODEL_PATH:${CURDIR}/gazebo/models \
+	GAZEBO_PLUGIN_PATH=$$GAZEBO_PLUGIN_PATH:${CURDIR}/gazebo/build \
+	gazebo --verbose ${CURDIR}/gazebo/flix.world
+
+grab_log:
+	tools/grab_log.py
+
+clean:
+	rm -rf gazebo/plugin/build $(SKETCH)/build $(SKETCH)/cache
+
+.PHONY: build upload monitor upload_and_monitor dependencies cmake build_simulator simulator grab_log clean

README.md

@@ -0,0 +1,38 @@
+# flix
+
+**flix** (*flight + X*) — making an open source ESP32-based quadcopter from scratch.
+
+## Features
+
+* Very simple and clear source code.
+* Acro and Stabilized flight using remote control.
+* Precise simulation using Gazebo.
+* In-RAM logging.
+* Command line interface through USB port.
+* Wi-Fi support.
+* ESCs with reverse support.
+* *Textbook and videos for students on writing a flight controller\*.*
+* *MAVLink support\*.*
+* *Position control and autonomous flights using external camera\**.
+
+*\* — planned.*
+
+## Version 0
+
+### Components
+
+|Component|Type|Image|Quantity|
+|-|-|-|-|
+|ESP32 Mini|Microcontroller board|<img src="docs/img/esp32.jpg" width=180>|1|
+|GY-91|IMU+barometer board|<img src="docs/img/gy-91.jpg" width=180>|1|
+|K100|Quadcopter frame|<img src="docs/img/frame.jpg" width=180>|1|
+|8520 3.7V brushed motor|Motor|<img src="docs/img/motor.jpeg" width=180>|4|
+|Hubsan 55 mm| Propeller|<img src="docs/img/prop.jpg" width=180>|4|
+|2.7A 1S Dual Way Micro Brush ESC|Motor ESC|<img src="docs/img/esc.jpg" width=180>|4|
+|KINGKONG TINY X8|RC transmitter|<img src="docs/img/tx.jpg" width=180>|1|
+|DF500 (SBUS)|RC receiver|<img src="docs/img/rx.jpg" width=180>|1|
+||SBUS inverter|<img src="docs/img/inv.jpg" width=180>||
+|3.7 Li-Po 850 MaH 60C|Battery|||
+||Battery charger|<img src="docs/img/charger.jpg" width=180>||
+||Wires, connectors, tape, ...||
+||3D-printed frame parts||

arduino-cli.yaml

@@ -0,0 +1,5 @@
+board_manager:
+  additional_urls:
+    - https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json
+library:
+  enable_unsafe_install: true

flix/cli.ino

@@ -0,0 +1,170 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include <MPU9250.h>
+#include "pid.hpp"
+
+static String command;
+static String value;
+static bool parsingCommand = true;
+
+extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
+extern MPU9250 IMU;
+
+const char* motd =
+"\nWelcome to\n"
+" _______  __       __  ___   ___\n"
+"|   ____||  |     |  | \\  \\ /  /\n"
+"|  |__   |  |     |  |  \\  V  /\n"
+"|   __|  |  |     |  |   >   <\n"
+"|  |     |  `----.|  |  /  .  \\\n"
+"|__|     |_______||__| /__/ \\__\\\n\n"
+"Commands:\n\n"
+"show - show all parameters\n"
+"<name> <value> - set parameter\n"
+"ps - show pitch/roll/yaw\n"
+"psq - show attitude quaternion\n"
+"imu - show IMU data\n"
+"rc - show RC data\n"
+"mot - show motor data\n"
+"log - dump in-RAM log\n"
+"cg - calibrate gyro\n"
+"fullmot <n> - test motor on all signals\n"
+"wifi - start wi-fi access point\n\n";
+
+bool showMotd = true;
+
+static const struct Param {
+	const char* name;
+	float* value;
+	float* value2;
+} params[] = {
+	{"rp", &rollRatePID.p, &pitchRatePID.p},
+	{"ri", &rollRatePID.i, &pitchRatePID.i},
+	{"rd", &rollRatePID.d, &pitchRatePID.d},
+
+	{"ap", &rollPID.p, &pitchPID.p},
+	{"ai", &rollPID.i, &pitchPID.i},
+	{"ad", &rollPID.d, &pitchPID.d},
+
+	{"yp", &yawRatePID.p, nullptr},
+	{"yi", &yawRatePID.i, nullptr},
+	{"yd", &yawRatePID.d, nullptr},
+
+	{"ss", &stepsPerSecond, nullptr},
+	// {"m", &mode, nullptr},
+};
+
+static void doCommand()
+{
+	if (command == "show") {
+		showTable();
+	} else if (command == "ps") {
+		Vector a = attitude.toEulerZYX();
+		Serial.println("roll: " + String(a.x * RAD_TO_DEG, 2) +
+			" pitch: " + String(a.y * RAD_TO_DEG, 2) +
+			" yaw: " + String(a.z * RAD_TO_DEG, 2));
+	} else if (command == "psq") {
+		Serial.println("qx: " + String(attitude.x) +
+			" qy: " + String(attitude.y) +
+			" qz: " + String(attitude.z) +
+			" qw: " + String(attitude.w));
+	} else if (command == "imu") {
+		Serial.println("gyro bias " + String(IMU.getGyroBiasX_rads()) + " "
+			+ String(IMU.getGyroBiasY_rads()) + " "
+			+ String(IMU.getGyroBiasZ_rads()));
+	} else if (command == "rc") {
+		Serial.println("RAW throttle " + String(channels[RC_CHANNEL_THROTTLE]) +
+			" yaw " + String(channels[RC_CHANNEL_YAW]) +
+			" pitch " + String(channels[RC_CHANNEL_PITCH]) +
+			" roll " + String(channels[RC_CHANNEL_ROLL]) +
+			" aux " + String(channels[RC_CHANNEL_AUX]) +
+			" mode " + String(channels[RC_CHANNEL_MODE]));
+		Serial.println("CONTROL throttle " + String(controls[RC_CHANNEL_THROTTLE]) +
+			" yaw " + String(controls[RC_CHANNEL_YAW]) +
+			" pitch " + String(controls[RC_CHANNEL_PITCH]) +
+			" roll " + String(controls[RC_CHANNEL_ROLL]) +
+			" aux " + String(controls[RC_CHANNEL_AUX]) +
+			" mode " + String(controls[RC_CHANNEL_MODE]));
+	} else if (command == "mot") {
+		Serial.println("MOTOR front-right " + String(motors[MOTOR_FRONT_RIGHT]) +
+			" front-left " + String(motors[MOTOR_FRONT_LEFT]) +
+			" rear-right " + String(motors[MOTOR_REAR_RIGHT]) +
+			" rear-left " + String(motors[MOTOR_REAR_LEFT]));
+	} else if (command == "log") {
+		dumpLog();
+	} else if (command == "cg") {
+		calibrateGyro();
+	} else if (command == "mfr") {
+		cliTestMotor(MOTOR_FRONT_RIGHT);
+	} else if (command == "mfl") {
+		cliTestMotor(MOTOR_FRONT_LEFT);
+	} else if (command == "mrr") {
+		cliTestMotor(MOTOR_REAR_RIGHT);
+	} else if (command == "mrl") {
+		cliTestMotor(MOTOR_REAR_LEFT);
+	} else if (command == "fullmot") {
+		fullMotorTest(value.toInt());
+	} else {
+		float val = value.toFloat();
+		if (!isfinite(val)) {
+			Serial.println("Invalid value: " + value);
+		}
+
+		for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
+			if (command == params[i].name) {
+				*params[i].value = val;
+				if (params[i].value2 != nullptr) *params[i].value2 = val;
+				Serial.print(command);
+				Serial.print(" = ");
+				Serial.println(val, 4);
+				return;
+			}
+		}
+		Serial.println("Invalid command: '" + command + "'");
+	}
+}
+
+static void showTable()
+{
+	for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
+		Serial.print(params[i].name);
+		Serial.print(" ");
+		Serial.println(*params[i].value, 5);
+	}
+}
+
+static void cliTestMotor(uint8_t n)
+{
+	Serial.println("Testing motor " + String(n));
+	motors[n] = 1;
+	sendMotors();
+	delay(5000);
+	motors[n] = 0;
+	sendMotors();
+	Serial.println("Done");
+}
+
+void parseInput()
+{
+	if (showMotd) {
+		Serial.println(motd);
+		showMotd = false;
+	}
+
+	while (Serial.available()) {
+		char c = Serial.read();
+		if (c == '\n') {
+			parsingCommand = true;
+			if (!command.isEmpty()) {
+				doCommand();
+			}
+			command.clear();
+			value.clear();
+		} else if (c == ' ') {
+			parsingCommand = false;
+		} else {
+			(parsingCommand ? command : value) += c;
+		}
+	}
+}

flix/control.ino

@@ -0,0 +1,267 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#pragma diag_suppress 144, 513
+
+#include "pid.hpp"
+#include "vector.hpp"
+#include "quaternion.hpp"
+
+#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 4.5
+#define ROLL_I 0
+#define ROLL_D 0
+#define PITCH_P ROLL_P // 8
+#define PITCH_I ROLL_I
+#define PITCH_D ROLL_D
+#define YAW_P 3
+#define PITCHRATE_MAX 360 * DEG_TO_RAD
+#define ROLLRATE_MAX 360 * DEG_TO_RAD
+#define YAWRATE_MAX 360 * DEG_TO_RAD
+#define MAX_TILT 30 * DEG_TO_RAD
+
+enum { MANUAL, ACRO, STAB } mode = STAB;
+bool armed = false;
+
+PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM);
+PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM);
+PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
+PID rollPID(ROLL_P, ROLL_I, ROLL_D);
+PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
+PID yawPID(YAW_P, 0, 0);
+
+Vector ratesFiltered;
+
+Quaternion attitudeTarget;
+Vector ratesTarget;
+Vector torqueTarget;
+float thrustTarget;
+ // TODO: ugly
+float yawTarget = NAN;
+bool controlYaw = false;
+
+void control()
+{
+	interpretRC();
+	if (mode == STAB) {
+		controlAttitude();
+		// controlAttitudeAlter();
+	}
+
+	if (mode == MANUAL) {
+		controlManual();
+	} else {
+		controlRate();
+	}
+}
+
+
+void interpretRC()
+{
+	if (controls[RC_CHANNEL_MODE] < 0.25) {
+		mode = MANUAL;
+	} else if (controls[RC_CHANNEL_MODE] < 0.75) {
+		mode = ACRO;
+	} else {
+		mode = STAB;
+	}
+
+	armed = controls[RC_CHANNEL_THROTTLE] >= 0.1 && controls[RC_CHANNEL_AUX] >= 0.5;
+
+	controlYaw = armed && mode == STAB && controls[RC_CHANNEL_YAW] == 0;
+	if (!controlYaw) {
+		yawTarget = attitude.getYaw();
+	}
+
+	if (mode == ACRO) {
+		ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
+		ratesTarget.y = -controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; // up pitch stick means tilt clockwise in frd
+		ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
+
+	} else if (mode == STAB) {
+		attitudeTarget = Quaternion::fromEulerZYX(
+			controls[RC_CHANNEL_ROLL] * MAX_TILT,
+			-controls[RC_CHANNEL_PITCH] * MAX_TILT,
+			yawTarget); // attitude.getYaw());
+		ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
+	}
+}
+
+static void controlAttitude()
+{
+	if (!armed) {
+		rollPID.reset();
+		pitchPID.reset();
+		yawPID.reset();
+		return;
+	}
+
+	const Vector up(0, 0, -1);
+	Vector upActual = attitude.rotate(up);
+	Vector upTarget = attitudeTarget.rotate(up);
+
+	float angle = Vector::angleBetweenVectors(upTarget, upActual);
+	if (!isfinite(angle)) {
+		// not enough precision to calculate
+		Serial.println("angle is nan, skip");
+		return;
+	}
+
+	Vector ratesTargetDir = Vector::angularRatesBetweenVectors(upTarget, upActual);
+	ratesTargetDir.normalize();
+
+	if (!ratesTargetDir.finite()) {
+		Serial.println("ratesTargetDir is nan, skip");
+		// std::cout << "angle is nan" << std::endl;
+		ratesTarget = Vector(0, 0, 0);
+		return;
+	}
+
+	ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt);
+	ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt);
+
+	if (controlYaw) {
+		ratesTarget.z = yawPID.update(yawTarget - attitude.getYaw(), dt); // WARNING:
+	}
+
+	if (!ratesTarget.finite()) {
+		Serial.print("ratesTarget: "); Serial.println(ratesTarget);
+		Serial.print("ratesTargetDir: "); Serial.println(ratesTargetDir);
+		Serial.print("attitudeTarget: "); Serial.println(attitudeTarget);
+		Serial.print("attitude: "); Serial.println(attitude);
+		Serial.print("upActual: "); Serial.println(upActual);
+		Serial.print("upTarget: "); Serial.println(upTarget);
+		Serial.print("angle: "); Serial.println(angle);
+		Serial.print("dt: "); Serial.println(dt);
+	}
+
+	// std::cout << "rsp: " << ratesTarget.x << " " << ratesTarget.y << std::endl;
+}
+
+static void controlAttitudeAlter()
+{
+	if (!armed) {
+		rollPID.reset();
+		pitchPID.reset();
+		yawPID.reset();
+		return;
+	}
+	Vector target = attitudeTarget.toEulerZYX();
+	Vector att = attitude.toEulerZYX();
+
+	ratesTarget.x = rollPID.update(target.x - att.x, dt);
+	ratesTarget.y = pitchPID.update(target.y - att.y, dt);
+
+	if (controlYaw) {
+		ratesTarget.z = yawPID.update(target.z - att.z, dt); // WARNING:
+	}
+}
+
+// passthrough mode
+static void controlManual()
+{
+	if (controls[RC_CHANNEL_THROTTLE] < 0.1) {
+		memset(motors, 0, sizeof(motors));
+		return;
+	}
+
+	thrustTarget = controls[RC_CHANNEL_THROTTLE]; // collective thrust
+
+	torqueTarget = ratesTarget * 0.01;
+
+	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
+	motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
+	motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
+	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
+
+	if (!isfinite(motors[0]) || !isfinite(motors[1]) || !isfinite(motors[2]) || !isfinite(motors[3])) {
+		Serial.println("motors are nan");
+	}
+
+	motors[0] = constrain(motors[0], 0, 1);
+	motors[1] = constrain(motors[1], 0, 1);
+	motors[2] = constrain(motors[2], 0, 1);
+	motors[3] = constrain(motors[3], 0, 1);
+}
+
+static void controlRate()
+{
+	if (!armed) { // TODO: too rough
+		memset(motors, 0, sizeof(motors));
+		rollRatePID.reset();
+		pitchRatePID.reset();
+		yawRatePID.reset();
+		return;
+	}
+
+	// collective thrust is throttle * 4
+	thrustTarget = controls[RC_CHANNEL_THROTTLE]; // WARNING:
+
+	ratesFiltered = rates * 0.8 + ratesFiltered * 0.2; // cut-off frequency 40 Hz
+
+	torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
+	torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
+	torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt);
+
+	if (!torqueTarget.finite()) {
+		Serial.print("torqueTarget: "); Serial.println(torqueTarget);
+		Serial.print("ratesTarget: "); Serial.println(ratesTarget);
+		Serial.print("rates: "); Serial.println(rates);
+		Serial.print("dt: "); Serial.println(dt);
+	}
+
+	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
+	motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
+	motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
+	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
+
+	//indicateSaturation();
+	// desaturate(motors[0], motors[1], motors[2], motors[3]);
+
+	// constrain and reverse, multiple by -1 if reversed
+	motors[0] = constrain(motors[0], 0, 1);
+	motors[1] = constrain(motors[1], 0, 1);
+	motors[2] = constrain(motors[2], 0, 1);
+	motors[3] = constrain(motors[3], 0, 1);
+}
+
+void desaturate(float& a, float& b, float& c, float& d)
+{
+	float m = max(max(a, b), max(c, d));
+	if (m > 1) {
+		float diff = m - 1;
+		a -= diff;
+		b -= diff;
+		c -= diff;
+		d -= diff;
+	}
+	m = min(min(a, b), min(c, d));
+	if (m < 0) {
+		a -= m;
+		b -= m;
+		c -= m;
+		d -= m;
+	}
+}
+
+static bool motorsSaturation = false;
+
+static inline void indicateSaturation() {
+	bool sat = motors[0] > 1 || motors[1] > 1 || motors[2] > 1 || motors[3] > 1 ||
+	           motors[0] < 0 || motors[1] < 0 || motors[2] < 0 || motors[3] < 0;
+	if (motorsSaturation != sat) {
+		motorsSaturation = sat;
+		setLED(motorsSaturation);
+	}
+}

flix/estimate.ino

@@ -0,0 +1,64 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include "quaternion.hpp"
+#include "vector.hpp"
+
+#define ONE_G 9.807f
+#define ACC_MIN 0.9f
+#define ACC_MAX 1.1f
+#define WEIGHT_ACC 0.5f
+
+void estimate()
+{
+	if (dt == 0) {
+		return;
+	}
+
+	// applying gyro
+	attitude *= Quaternion::fromAngularRates(rates * dt);
+	attitude.normalize();
+
+	// test should we apply acc
+	float accNorm = acc.norm();
+	if (accNorm < ACC_MIN * ONE_G || accNorm > ACC_MAX * ONE_G) {
+		// use acc only when we're not accelerating
+		return;
+	}
+
+	Vector up = attitude.rotate(Vector(0, 0, -1));
+	Vector accCorrDirection = Vector::angularRatesBetweenVectors(acc, up);
+	accCorrDirection.normalize();
+
+	if (!accCorrDirection.finite()) {
+		return;
+	}
+
+	Vector accCorr = accCorrDirection * Vector::angleBetweenVectors(up, acc) * dt * WEIGHT_ACC;
+
+	if (!accCorr.finite()) {
+		return; // TODO
+	}
+
+	attitude *= Quaternion::fromAngularRates(accCorr);
+	attitude.normalize();
+
+	if (!attitude.finite()) {
+		Serial.print("dt "); Serial.println(dt, 15);
+		Serial.print("up "); Serial.println(up);
+		Serial.print("acc "); Serial.println(acc);
+		Serial.print("acc norm "); Serial.println(acc.norm());
+		Serial.print("upp norm "); Serial.println(up.norm());
+		Serial.print("acc dot up "); Serial.println(Vector::dot(up, acc), 15);
+		Serial.print("acc cor ang "); Serial.println(Vector::angleBetweenVectors(up, acc), 15);
+		Serial.print("acc corr dir "); Serial.println(accCorrDirection);
+		Serial.print("acc cor "); Serial.println(accCorr);
+		Serial.print("att "); Serial.println(attitude);
+	}
+}
+
+void signalizeHorizontality()
+{
+	float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
+	setLED(angle < 15 * DEG_TO_RAD);
+}

flix/flix.ino

@@ -0,0 +1,74 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include "vector.hpp"
+#include "quaternion.hpp"
+
+#define SERIAL_BAUDRATE 115200
+
+#define WIFI_ENABLED 0
+
+#define RC_CHANNELS 6
+#define RC_CHANNEL_THROTTLE 2
+#define RC_CHANNEL_YAW 3
+#define RC_CHANNEL_PITCH 1
+#define RC_CHANNEL_ROLL 0
+#define RC_CHANNEL_AUX 4
+#define RC_CHANNEL_MODE 5
+
+#define MOTOR_REAR_LEFT 0
+#define MOTOR_FRONT_LEFT 3
+#define MOTOR_FRONT_RIGHT 2
+#define MOTOR_REAR_RIGHT 1
+
+uint32_t startTime; // system startup time
+uint32_t stepTime; // current step time
+uint32_t steps; // total steps count
+float stepsPerSecond; // steps per last second
+float dt; // time delta from previous step
+uint16_t channels[16]; // raw rc channels
+float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
+uint32_t rcFailSafe, rcLostFrame;
+float motors[4]; // normalized motors thrust in range [-1..1]
+Vector rates; // angular rates, rad/s
+Vector acc; // accelerometer data, m/s/s
+Quaternion attitude; // estimated attitude
+bool calibrating; // flag we're calibrating
+
+void setupDebug();
+void lowPowerMode();
+
+void setup()
+{
+	Serial.begin(SERIAL_BAUDRATE);
+	Serial.println("Initializing flix");
+	setupTime();
+	setupLED();
+	setupMotors();
+	setLED(true);
+#if WIFI_ENABLED == 1
+	setupWiFi();
+#endif
+	setupIMU();
+	setupRC();
+
+	setLED(false);
+	Serial.println("Initializing complete");
+}
+
+void loop()
+{
+	if (!readIMU()) return;
+
+	step();
+	readRC();
+	estimate();
+	control();
+	sendMotors();
+	parseInput();
+#if WIFI_ENABLED == 1
+	sendMavlink();
+#endif
+	logData();
+	signalizeHorizontality();
+}

flix/imu.ino

@@ -0,0 +1,112 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include <MPU9250.h>
+
+#define IMU_CS_PIN 4 // chip-select pin for IMU SPI connection
+
+MPU9250 IMU(SPI, IMU_CS_PIN);
+
+void setupIMU()
+{
+	Serial.println("Setup IMU");
+
+	auto status = IMU.begin();
+	if (status < 0) {
+		while (true) {
+			Serial.print("IMU begin error: "); Serial.println(status);
+			delay(1000);
+		}
+	}
+
+	calibrating = true;
+	calibrateGyro();
+	// loadGyroCal();
+	// calibrateAccel();
+	loadAccelCal();
+
+	IMU.setSrd(0); // set sample rate to 1000 Hz
+	// NOTE: very important, without the above the rate would be terrible 50 Hz
+
+	calibrating = false;
+}
+
+bool readIMU()
+{
+	if (IMU.readSensor() < 0) {
+		Serial.println("IMU read error"); // TODO:
+		return false;
+	}
+
+	auto lastRates = rates;
+
+	rates.x = IMU.getGyroX_rads();
+	rates.y = IMU.getGyroY_rads();
+	rates.z = IMU.getGyroZ_rads();
+	acc.x = IMU.getAccelX_mss();
+	acc.y = IMU.getAccelY_mss();
+	acc.z = IMU.getAccelZ_mss();
+
+	return rates != lastRates;
+}
+
+static void calibrateGyro()
+{
+	Serial.println("Calibrating gyro, stand still");
+	delay(500);
+	int status = IMU.calibrateGyro();
+	Serial.println("Calibration status: " + String(status));
+	Serial.print("Gyro bias: ");
+	Serial.print(IMU.getGyroBiasX_rads(), 10); Serial.print(" ");
+	Serial.print(IMU.getGyroBiasY_rads(), 10); Serial.print(" ");
+	Serial.println(IMU.getGyroBiasZ_rads(), 10);
+	IMU.setSrd(0);
+}
+
+static void calibrateAccel()
+{
+	Serial.println("Cal accel: place level"); delay(3000);
+	IMU.calibrateAccel();
+	Serial.println("Cal accel: place nose up"); delay(3000);
+	IMU.calibrateAccel();
+	Serial.println("Cal accel: place nose down"); delay(3000);
+	IMU.calibrateAccel();
+	Serial.println("Cal accel: place on right side"); delay(3000);
+	IMU.calibrateAccel();
+	Serial.println("Cal accel: place on left side"); delay(3000);
+	IMU.calibrateAccel();
+	Serial.println("Cal accel: upside down"); delay(300);
+	IMU.calibrateAccel();
+	Serial.print("Accel bias: ");
+	Serial.print(IMU.getAccelBiasX_mss(), 10); Serial.print(" ");
+	Serial.print(IMU.getAccelBiasY_mss(), 10); Serial.print(" ");
+	Serial.println(IMU.getAccelBiasZ_mss(), 10);
+	Serial.print("Accel scale: ");
+	Serial.print(IMU.getAccelScaleFactorX(), 10); Serial.print(" ");
+	Serial.print(IMU.getAccelScaleFactorY(), 10); Serial.print(" ");
+	Serial.println(IMU.getAccelScaleFactorZ(), 10);
+}
+
+static void loadAccelCal()
+{
+	IMU.setAccelCalX(-0.0048542023, 1.0008112192);
+	IMU.setAccelCalY(0.0521845818, 0.9985780716);
+	IMU.setAccelCalZ(0.5754694939, 1.0045746565);
+}
+
+static void loadGyroCal()
+{
+	IMU.setGyroBiasX_rads(-0.0175771303);
+	IMU.setGyroBiasY_rads(-0.0298212003);
+	IMU.setGyroBiasZ_rads(0.0148300380);
+}
+
+// Accel bias: 0.0463809967 0.0463809967 0.1486964226
+// Accel scale: 0.9986976385 0.9993721247 1.0561490059
+
+// Accel bias: 0.0145006180 0.0145006180 0.0000000000
+// Accel scale: 0.9989741445 0.9993283749 1.0000000000
+
+// Correct:
+// Accel bias: -0.0048542023 0.0521845818 0.5754694939
+// Accel scale: 1.0008112192 0.9985780716 1.0045746565

flix/led.ino

@@ -0,0 +1,43 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#define LED_PIN 2
+#define BLINK_FAST_PERIOD 300000
+#define BLINK_SLOW_PERIOD 1000000
+
+static bool state;
+
+static enum {
+	OFF,
+	ON,
+	BLINK_FAST,
+	BLINK_SLOW
+} LEDscheme = OFF;
+
+void setupLED()
+{
+	pinMode(LED_BUILTIN, OUTPUT);
+}
+
+void setLED(bool on)
+{
+	digitalWrite(LED_BUILTIN, on ? HIGH : LOW);
+}
+
+void proceedLED()
+{
+	// TODO: this won't work
+	// TODO:: just check is current second even or odd
+	if (LEDscheme == BLINK_FAST && stepTime % BLINK_FAST_PERIOD == 0) {
+		state != state;
+		digitalWrite(LED_BUILTIN, state ? HIGH : LOW);
+	} else if (LEDscheme == BLINK_SLOW && stepTime % BLINK_SLOW_PERIOD == 0) {
+		state != state;
+		digitalWrite(LED_BUILTIN, state ? HIGH : LOW);
+	}
+}
+
+void blinkLED()
+{
+	setLED(micros() / 500000 % 2);
+}

flix/log.ino

@@ -0,0 +1,48 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#define LOG_RATE 200
+#define LOG_DURATION 10
+#define LOG_PERIOD 1000000 / LOG_RATE
+#define LOG_SIZE LOG_DURATION * LOG_RATE
+#define LOG_COLUMNS 11
+
+static float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)
+static int logPointer = 0;
+static uint32_t lastLog = 0;
+
+void logData()
+{
+	if (!armed) return;
+	if (stepTime - lastLog < LOG_PERIOD) return;
+	lastLog = stepTime;
+
+	logBuffer[logPointer][0] = stepTime;
+	logBuffer[logPointer][1] = rates.x;
+	logBuffer[logPointer][2] = rates.y;
+	logBuffer[logPointer][3] = rates.z;
+	logBuffer[logPointer][4] = ratesTarget.x;
+	logBuffer[logPointer][5] = ratesTarget.y;
+	logBuffer[logPointer][6] = ratesTarget.z;
+	logBuffer[logPointer][7] = torqueTarget.x;
+	logBuffer[logPointer][8] = torqueTarget.y;
+	logBuffer[logPointer][9] = torqueTarget.z;
+	logBuffer[logPointer][10] = thrustTarget;
+
+	logPointer++;
+	if (logPointer >= 2000) {
+		logPointer = 0;
+	}
+}
+
+void dumpLog()
+{
+	printf("timestamp,rate.x,rate.y,rate.z,target.rate.x,target.rate.y,target.rate.z,torque.x,torque.y,torque.z,thrust\n");
+	for (int i = 0; i < LOG_SIZE; i++) {
+		for (int j = 0; j < LOG_COLUMNS - 1; j++) {
+			printf("%f,", logBuffer[i][j]);
+		}
+		printf("%f\n", logBuffer[i][LOG_COLUMNS - 1]);
+	}
+	Serial.println();
+}

flix/mavlink.ino

@@ -0,0 +1,70 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#if WIFI_ENABLED == 1
+
+#include "mavlink/common/mavlink.h"
+
+#define SYSTEM_ID 1
+#define PERIOD_SLOW 1000000 // us
+#define PERIOD_FAST 100000 // us
+
+static uint32_t lastSlow;
+static uint32_t lastFast;
+
+void sendMavlink()
+{
+	mavlink_message_t msg;
+
+	if (stepTime - lastSlow >= PERIOD_SLOW) {
+		lastSlow = stepTime;
+
+		mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR,
+			MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED,
+			0, calibrating ? MAV_STATE_CALIBRATING : MAV_STATE_STANDBY);
+		sendMessage(&msg);
+
+		// params test
+		// mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "PITCHRATE_P", PITCHRATE_D, MAV_PARAM_TYPE_REAL32, 3, 0);
+		// sendMessage(&msg);
+		// mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "PITCHRATE_I", PITCHRATE_I, MAV_PARAM_TYPE_REAL32, 3, 1);
+		// sendMessage(&msg);
+		// mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "PITCHRATE_D", PITCHRATE_D, MAV_PARAM_TYPE_REAL32, 3, 2);
+		// sendMessage(&msg);
+	}
+
+	if (stepTime - lastFast >= PERIOD_FAST) {
+		lastFast = stepTime;
+
+		// mavlink_msg_attitude_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, NAN, NAN, NAN, rollRate, pitchRate, yawRate);
+		// sendMessage(&msg);
+
+		const float zeroQuat[] = {0, 0, 0, 0};
+
+		mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+			stepTime / 1000, attitude.w, attitude.x, attitude.y, attitude.z, rates.x, rates.y, rates.z, zeroQuat);
+		// mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+		// 	stepTime / 1000, attitudeTarget.w, attitudeTarget.x, attitudeTarget.y, attitudeTarget.z, rates.x, rates.y, rates.z, zeroQuat);
+		sendMessage(&msg);
+
+		mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, 0,
+			controls[0] * 10000, controls[1] * 10000, controls[2] * 10000,
+			controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,
+			UINT16_MAX, UINT16_MAX, 255);
+		sendMessage(&msg);
+
+		float actuator[32];
+		memcpy(motors, actuator, 4 * sizeof(float));
+		mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, 4, actuator);
+		sendMessage(&msg);
+	}
+}
+
+static inline void sendMessage(const void *msg)
+{
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+	uint16_t len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
+	sendWiFi(buf, len);
+}
+
+#endif

flix/motors.ino

@@ -0,0 +1,127 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#define MOTOR_0_PIN 12
+#define MOTOR_1_PIN 13
+#define MOTOR_2_PIN 14
+#define MOTOR_3_PIN 15
+
+// #define PWM_FREQUENCY 200
+// #define PWM_FREQUENCY 50 // TODO: way low frequency considering the IMU is 1kHz
+#define PWM_FREQUENCY 200 // WARNING: original 50
+
+#define PWM_RESOLUTION 8
+
+// #define PWM_MIN 1575
+// #define PWM_MAX 2300
+#define PWM_NEUTRAL 1500
+// #define PWM_REVERSE_MAX 700
+// #define PWM_REVERSE_MIN 1425
+
+// static const uint16_t pwmMin[] = {1600-50, 1600-50, 1600-50, 1600-50};
+// static const uint16_t pwmMax[] = {2100, 2300, 2000, 2000}; // NOTE: ORIGINAL
+
+static const uint16_t pwmMin[] = {1600, 1600, 1600, 1600}; // NOTE: success
+// static const uint16_t pwmMax[] = {2000, 2000, 2000, 2000}; // NOTE: success
+static const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
+// from esc description
+// static const uint16_t pwmMin[] = {1600, 1600, 1600, 1600};
+// static const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
+
+// static const uint16_t pwmReverseMin[] = {1420+50, 1440+50, 1440+50, 1440+50};
+
+// static const uint16_t pwmReverseMin[] = {1420, 1440, 1440, 1440};
+// static const uint16_t pwmReverseMax[] = {700, 1100, 1100, 1100}; // NOTE: ???
+static const uint16_t pwmReverseMin[] = {1390, 1440, 1440, 1440};
+static const uint16_t pwmReverseMax[] = {1100, 1100, 1100, 1100};
+
+bool useBreak; // TODO: redesign
+
+void setupMotors() {
+	Serial.println("Setup Motors");
+
+	// configure PWM channels
+	ledcSetup(0, PWM_FREQUENCY, PWM_RESOLUTION);
+	ledcSetup(1, PWM_FREQUENCY, PWM_RESOLUTION);
+	ledcSetup(2, PWM_FREQUENCY, PWM_RESOLUTION);
+	ledcSetup(3, PWM_FREQUENCY, PWM_RESOLUTION);
+
+	// attach channels to motor pins
+	ledcAttachPin(MOTOR_0_PIN, 0);
+	ledcAttachPin(MOTOR_1_PIN, 1);
+	ledcAttachPin(MOTOR_2_PIN, 2);
+	ledcAttachPin(MOTOR_3_PIN, 3);
+
+	// send initial break to initialize ESCs
+	// Serial.println("Calibrating ESCs");
+	// useBreak = true;
+	// sendMotors();
+	// delay(2000);
+	// useBreak = false;
+	sendMotors();
+	Serial.println("Motors initialized");
+}
+
+static uint16_t getPWM(float val, int n)
+{
+	if (val == 0) {
+		return PWM_NEUTRAL; // useBreak ? PWM_NEUTRAL : 0;
+	} else if (val > 0) {
+		return mapff(val, 0, 1, pwmMin[n], pwmMax[n]);
+	} else {
+		return mapff(val, 0, -1, pwmReverseMin[n], pwmReverseMax[n]);
+	}
+}
+
+static uint8_t pwmToDutyCycle(uint16_t pwm) {
+	return map(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
+}
+
+void sendMotors()
+{
+	ledcWrite(0, pwmToDutyCycle(getPWM(motors[0], 0)));
+	ledcWrite(1, pwmToDutyCycle(getPWM(motors[1], 1)));
+	ledcWrite(2, pwmToDutyCycle(getPWM(motors[2], 2)));
+	ledcWrite(3, pwmToDutyCycle(getPWM(motors[3], 3)));
+}
+
+// =====================
+
+const uint32_t REVERSE_PAUSE = 0.1 * 1000000;
+
+static uint8_t lastMotorDirection[4];
+static uint32_t lastDirectionChange[4];
+
+static void handleReversePause()
+{
+	for (int i = 0; i < 4; i++) {
+		if (abs(sign(motors[i]) - lastMotorDirection[i]) > 1) { // 0 => 1 is not direction change, -1 => 1 is
+			lastDirectionChange[i] = stepTime;
+		}
+		if (stepTime - lastDirectionChange[i] < REVERSE_PAUSE) {
+			// wait before changing direction
+			motors[i] = 0;
+		}
+		lastMotorDirection[i] = sign(motors[i]);
+	}
+}
+
+// =====================
+
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+
+float motorThrust[1][10] = {
+	{0,0.5513626834,0.5387840671,0.6498951782,0.7023060797,0.7610062893,0.8679245283,1,0.9937106918,0.9916142558}
+};
+uint16_t minPwm = 1500;
+uint16_t pwmStep = 100;
+
+static float thrustToMotorInput(uint8_t n, float thrust)
+{
+	for (int i = 0; i < ARRAY_SIZE(motorThrust[n]); i++) {
+		if (thrust <= motorThrust[n][i]) {
+			// TODO: pwm
+			return mapff(thrust, 0, 1, motorThrust[n][i-1], motorThrust[n][i]);
+		}
+	}
+}

flix/pid.hpp

@@ -0,0 +1,47 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#pragma once
+
+class PID
+{
+public:
+	float p = 0;
+	float i = 0;
+	float d = 0;
+	float windup = 0;
+
+	float derivative = 0;
+	float integral = 0;
+
+	PID(float p, float i, float d, float windup = 0) : p(p), i(i), d(d), windup(windup) {};
+
+	float update(float error, float dt)
+	{
+		if (!isfinite(error) || !isfinite(dt)) {
+			// TODO: brutal way to remove glitches
+			Serial.println("nan in error or dt");
+			return NAN;
+		}
+
+		if (dt > 0 && isfinite(prevError)) {
+			integral += error * dt;
+			float _derivative = (error - prevError) / dt;
+			derivative = derivative * 0.8 + 0.2 * _derivative; // lpf WARNING:
+		}
+
+		prevError = error;
+
+		return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
+	}
+
+	void reset()
+	{
+		prevError = NAN;
+		integral = 0;
+		derivative = 0;
+	}
+
+private:
+	float prevError = NAN;
+};

flix/quaternion.hpp

@@ -0,0 +1,211 @@
+// Lightweight rotation quaternion library
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#pragma once
+
+#include "vector.hpp"
+
+class Quaternion : public Printable {
+public:
+	float w, x, y, z;
+
+	Quaternion(): w(1), x(0), y(0), z(0) {};
+
+	Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {};
+
+	static Quaternion fromAxisAngle(float a, float b, float c, float angle)
+	{
+		float halfAngle = angle * 0.5;
+		float sin2 = sin(halfAngle);
+		float cos2 = cos(halfAngle);
+		float sinNorm = sin2 / sqrt(a * a + b * b + c * c);
+		return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);
+	}
+
+	static Quaternion fromAngularRates(float x, float y, float z)
+	{
+		return Quaternion::fromAxisAngle(x, y, z, sqrt(x * x + y * y + z * z));
+	}
+
+	static Quaternion fromAngularRates(const Vector& rates)
+	{
+		if (rates.zero()) {
+			return Quaternion();
+		}
+		return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
+	}
+
+	static Quaternion fromEulerZYX(float x, float y, float z)
+	{
+		float cx = cos(x / 2);
+		float cy = cos(y / 2);
+		float cz = cos(z / 2);
+		float sx = sin(x / 2);
+		float sy = sin(y / 2);
+		float sz = sin(z / 2);
+
+		return Quaternion(
+			cx * cy * cz + sx * sy * sz,
+			sx * cy * cz - cx * sy * sz,
+			cx * sy * cz + sx * cy * sz,
+			cx * cy * sz - sx * sy * cz);
+	}
+
+	static Quaternion fromBetweenVectors(Vector u, 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;
+		float w3 = u.x * v.y - u.y * v.x;
+
+		Quaternion ret(
+			dot + sqrt(dot * dot + w1 * w1 + w2 * w2 + w3 * w3),
+			w1,
+			w2,
+			w3);
+		ret.normalize();
+		return ret;
+	}
+
+	static Quaternion _fromBetweenVectors(float a, float b, float c, float x, float y, float z)
+	{
+		float dot = a * x + b * y + c * z;
+		float w1 = b * z - c * y;
+		float w2 = c * x - a * z;
+		float w3 = a * y - b * x;
+
+		Quaternion ret(
+			dot + sqrt(dot * dot + w1 * w1 + w2 * w2 + w3 * w3),
+			w1,
+			w2,
+			w3);
+		ret.normalize();
+		return ret;
+	};
+
+	void toAxisAngle(float& a, float& b, float& c, float& angle)
+	{
+		angle = acos(w) * 2;
+		a = x / sin(angle / 2);
+		b = y / sin(angle / 2);
+		c = z / sin(angle / 2);
+	}
+
+	Vector toEulerZYX() const
+	{
+		return Vector(
+			atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
+			asin(2 * (w * y - z * x)),
+			atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)));
+	}
+
+	float getYaw() const
+	{
+		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
+		float yaw;
+		float sqx = x * x;
+		float sqy = y * y;
+		float sqz = z * z;
+		float sqw = w * w;
+
+		double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
+
+		if (sarg <= -0.99999) {
+			yaw = -2 * atan2(y, x);
+		} else if (sarg >= 0.99999) {
+			yaw = 2 * atan2(y, x);
+		} else {
+			yaw = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
+		}
+		return yaw;
+	}
+
+	void setYaw(float yaw)
+	{
+		// TODO: optimize?
+		Vector euler = toEulerZYX();
+		(*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw);
+	}
+
+	void toAngularRates(float& x, float& y, float& z)
+	{
+		// this->toAxisAngle(); // TODO:
+	}
+
+	Quaternion & operator*=(const Quaternion &q)
+	{
+		Quaternion ret(
+			w * q.w - x * q.x - y * q.y - z * q.z,
+			w * q.x + x * q.w + y * q.z - z * q.y,
+			w * q.y + y * q.w + z * q.x - x * q.z,
+			w * q.z + z * q.w + x * q.y - y * q.x);
+		return (*this = ret);
+	}
+
+	Quaternion operator*(const Quaternion& q)
+	{
+		return Quaternion(
+			w * q.w - x * q.x - y * q.y - z * q.z,
+			w * q.x + x * q.w + y * q.z - z * q.y,
+			w * q.y + y * q.w + z * q.x - x * q.z,
+			w * q.z + z * q.w + x * q.y - y * q.x);
+	}
+
+	Quaternion inversed() const
+	{
+		float normSqInv = 1 / (w * w + x * x + y * y + z * z);
+		return Quaternion(
+			w * normSqInv,
+			-x * normSqInv,
+			-y * normSqInv,
+			-z * normSqInv);
+	}
+
+	float norm() const
+	{
+		return sqrt(w * w + x * x + y * y + z * z);
+	}
+
+	void normalize()
+	{
+		float n = norm();
+		w /= n;
+		x /= n;
+		y /= n;
+		z /= n;
+	}
+
+	Vector conjugate(const Vector& v)
+	{
+		Quaternion qv(0, v.x, v.y, v.z);
+		Quaternion res = (*this) * qv * inversed();
+		return Vector(res.x, res.y, res.z);
+	}
+
+	Vector conjugateInversed(const Vector& v)
+	{
+		Quaternion qv(0, v.x, v.y, v.z);
+		Quaternion res = inversed() * qv * (*this);
+		return Vector(res.x, res.y, res.z);
+	}
+
+	inline Vector rotate(const Vector& v)
+	{
+		return conjugateInversed(v);
+	}
+
+	inline bool finite() const
+	{
+		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
+	}
+
+	size_t printTo(Print& p) const {
+		size_t r = 0;
+		r += p.print(w, 15) + p.print(" ");
+		r += p.print(x, 15) + p.print(" ");
+		r += p.print(y, 15) + p.print(" ");
+		r += p.print(z, 15);
+		return r;
+	}
+};

flix/rc.ino

@@ -0,0 +1,34 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include <SBUS.h>
+
+static const uint16_t channelNeutral[] = {995, 883, 200, 972, 512, 512};
+static const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
+
+static SBUS RC(Serial2);
+
+void setupRC()
+{
+	Serial.println("Setup RC");
+	RC.begin();
+}
+
+static uint32_t lastReadRC = 0;
+
+void readRC()
+{
+	bool failSafe, lostFrame;
+	if (RC.read(channels, &failSafe, &lostFrame)) {
+		if (failSafe) { rcFailSafe++; return; } // TODO: NOT TESTED YET
+		if (lostFrame) { rcLostFrame++; return; }
+		normalizeRC();
+		lastReadRC = stepTime;
+	}
+}
+
+static void normalizeRC() {
+	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
+		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
+	}
+}

flix/test_motors.ino

@@ -0,0 +1,67 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+static void _pwm(int n, uint16_t pwm)
+{
+	printf("Motor %d: %d\n", n, pwm);
+	ledcWrite(n, pwmToDutyCycle(pwm));
+	delay(5000);
+}
+
+void fullMotorTest(int n)
+{
+	printf("> Full test for motor %d\n", n);
+	bool reverse = false;
+	if (reverse) {
+		// _pwm(n, 700);
+		// _pwm(n, 800);
+		// _pwm(n, 900);
+		// _pwm(n, 1000);
+		// _pwm(n, 1100);
+		// _pwm(n, 1200);
+		// _pwm(n, 1300);
+		// _pwm(n, 1400);
+		// _pwm(n, 1410);
+		// _pwm(n, 1420);
+		// _pwm(n, 1430);
+		// _pwm(n, 1440);
+		// _pwm(n, 1450);
+		// _pwm(n, 1460);
+		// _pwm(n, 1470);
+		// _pwm(n, 1480);
+		// _pwm(n, 1490);
+	}
+	_pwm(n, 1500);
+	// _pwm(n, 1510);
+	// _pwm(n, 1520);
+	// _pwm(n, 1530);
+	// _pwm(n, 1540);
+	// _pwm(n, 1550);
+	// _pwm(n, 1560);
+	// _pwm(n, 1570);
+	// _pwm(n, 1580);
+	// _pwm(n, 1590);
+	_pwm(n, 1600);
+	_pwm(n, 1700);
+	_pwm(n, 1800);
+	_pwm(n, 1900);
+	_pwm(n, 2000);
+	_pwm(n, 2100);
+	_pwm(n, 2200);
+	_pwm(n, 2300);
+	_pwm(n, 1500);
+}
+
+void fullMotorsTest()
+{
+	printf("Perform full motors test\n");
+	motors[0] = 0;
+	motors[1] = 0;
+	motors[2] = 0;
+	motors[3] = 0;
+
+	fullMotorTest(0);
+	fullMotorTest(1);
+	fullMotorTest(2);
+	fullMotorTest(3);
+}

flix/time.ino

@@ -0,0 +1,51 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+const uint32_t MS = 1000;
+const uint32_t S = 1000000;
+
+static uint32_t stepsPerSecondCurrent;
+static uint32_t stepsPerSecondCurrentLast;
+
+void setupTime()
+{
+	startTime = micros();
+}
+
+void step() {
+	steps++;
+	auto time = micros();
+
+	if (stepTime == 0) { // first step
+		stepTime = time;
+	}
+
+	dt = (time - stepTime) / 1000000.0;
+	stepTime = time;
+
+	// compute steps per seconds
+	stepsPerSecondCurrent++;
+	if (time - stepsPerSecondCurrentLast >= 1000000) {
+		stepsPerSecond = stepsPerSecondCurrent;
+		stepsPerSecondCurrent = 0;
+		stepsPerSecondCurrentLast = time;
+	}
+}
+
+void _step() {
+	steps++;
+	auto currentStepTime = micros();
+	if (stepTime == 0) {
+		stepTime = currentStepTime;
+	}
+	dt = (currentStepTime - stepTime) / 1000000.0;
+	stepTime = currentStepTime;
+
+	// compute steps per second, TODO: move to func
+	stepsPerSecondCurrent++;
+	if (stepTime - stepsPerSecondCurrentLast >= 1000000) {
+		stepsPerSecond = stepsPerSecondCurrent;
+		stepsPerSecondCurrent = 0;
+		stepsPerSecondCurrentLast = stepTime;
+	}
+}

flix/util.ino

@@ -0,0 +1,46 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include "math.h"
+
+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)
+{
+	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+// float hypot3(float x, float y, float z)
+// {
+// 	return sqrt(x * x + y * y + z * z);
+// }
+
+int8_t sign(float x)
+{
+	return (x > 0) - (x < 0);
+}
+
+float randomFloat(float min, float max)
+{
+	return min + (max - min) * (float)rand() / RAND_MAX;
+}
+
+// === printf ===
+// https://github.com/jandelgado/log4arduino/blob/master/src/log4arduino.cpp#L51
+// https://webhamster.ru/mytetrashare/index/mtb0/16381244680p5beet5d6
+
+#ifdef ARDUINO
+#define PRINTF_MAX_STRING_LEN 200
+void printf(const __FlashStringHelper *fmt, ...)
+{
+	char buf[PRINTF_MAX_STRING_LEN];
+	va_list args;
+	va_start(args, fmt);
+	vsnprintf(buf, PRINTF_MAX_STRING_LEN, (PGM_P)fmt, args);
+	va_end(args);
+	Serial.print(buf);
+}
+#endif

flix/vector.hpp

@@ -0,0 +1,90 @@
+// Lightweight vector library
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#pragma once
+
+class Vector : public Printable
+{
+public:
+	float x, y, z;
+
+	Vector(): x(0), y(0), z(0) {};
+
+	Vector(float x, float y, float z): x(x), y(y), z(z) {};
+
+	float norm() const
+	{
+		return sqrt(x * x + y * y + z * z);
+	}
+
+	bool zero() const
+	{
+		return x == 0 && y == 0 && z == 0;
+	}
+
+	void normalize()
+	{
+		float n = norm();
+		x /= n;
+		y /= n;
+		z /= n;
+	}
+
+	Vector operator * (float b)
+	{
+		return Vector(x * b, y * b, z * b);
+	}
+
+	Vector operator + (const Vector& b) const
+	{
+		return Vector(x + b.x, y + b.y, z + b.z);
+	}
+
+	Vector operator - (const Vector& b) const
+	{
+		return Vector(x - b.x, y - b.y, z - b.z);
+	}
+
+	inline bool operator == (const Vector& b) const
+	{
+		return x == b.x && y == b.y && z == b.z;
+	}
+
+	inline bool operator != (const Vector& b) const
+	{
+		return !(*this == b);
+	}
+
+	inline bool finite() const
+	{
+		return isfinite(x) && isfinite(y) && isfinite(z);
+	}
+
+	static float dot(const Vector& a, const Vector& b)
+	{
+		return a.x * b.x + a.y * b.y + a.z * b.z;
+	}
+
+	static Vector cross(const Vector& a, const Vector& b)
+	{
+		return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
+	}
+
+	static float angleBetweenVectors(const Vector& a, const Vector& b)
+	{
+		return acos(dot(a, b) / (a.norm() * b.norm()));
+	}
+
+	static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v)
+	{
+		return cross(u, v);
+	}
+
+	size_t printTo(Print& p) const {
+		return
+			p.print(x, 15) + p.print(" ") +
+			p.print(y, 15) + p.print(" ") +
+			p.print(z, 15);
+	}
+};

flix/wifi.ino

@@ -0,0 +1,35 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#if WIFI_ENABLED == 1
+
+#include <WiFi.h>
+#include <WiFiClient.h>
+#include <WiFiAP.h>
+#include "SBUS.h"
+#include "mavlink/common/mavlink.h"
+
+#define WIFI_SSID "flix"
+#define WIFI_PASSWORD "flixwifi"
+// #define WIFI_UDP_IP "192.168.4.255"
+#define WIFI_UDP_IP "255.255.255.255"
+// #define WIFI_UDP_IP "192.168.4.2"
+#define WIFI_UDP_PORT 14550
+
+WiFiUDP udp;
+
+void setupWiFi()
+{
+	Serial.println("Setup Wi-Fi");
+	WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
+	IPAddress myIP = WiFi.softAPIP();
+}
+
+inline void sendWiFi(const uint8_t *buf, size_t len)
+{
+	udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT);
+	udp.write(buf, len);
+	udp.endPacket();
+}
+
+#endif

gazebo/CMakeLists.txt

@@ -0,0 +1,16 @@
+project(flix_gazebo)
+cmake_minimum_required(VERSION 3.0 FATAL_ERROR)
+
+# === gazebo plugin
+find_package(gazebo REQUIRED)
+find_package(SDL2 REQUIRED)
+include_directories(${GAZEBO_INCLUDE_DIRS})
+link_directories(${GAZEBO_LIBRARY_DIRS})
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
+
+set(FLIX_SOURCE_DIR ../flix)
+include_directories(${FLIX_SOURCE_DIR})
+file(GLOB_RECURSE FLIX_INO_FILES ${FLIX_SOURCE_DIR}/*.ino)
+
+add_library(flix SHARED flix.cpp)
+target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})

gazebo/arduino.hpp

@@ -0,0 +1,86 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include <iostream>
+#include <cmath>
+#include <string>
+
+using std::cout;
+using std::max;
+using std::min;
+using std::isfinite;
+
+// #define PI 3.1415926535897932384626433832795
+#define DEG_TO_RAD 0.017453292519943295769236907684886
+#define RAD_TO_DEG 57.295779513082320876798154814105
+
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
+class Print;
+
+class Printable {
+public:
+	virtual size_t printTo(Print& p) const = 0;
+};
+
+class Print {
+public:
+	size_t print(float n, int digits = 2)
+	{
+		cout << n;
+		return 0; // TODO:
+	}
+
+	size_t println(float n, int digits = 2)
+	{
+		print(n, digits);
+		cout << std::endl;
+		return 0;
+	}
+
+	size_t print(const char* s)
+	{
+		cout << s;
+		return 0;
+	}
+
+	size_t println(const char* s)
+	{
+		print(s);
+		cout << std::endl;
+		return 0;
+	}
+
+	size_t println(const Printable& p)
+	{
+		p.printTo(*this);
+		cout << std::endl;
+		return 0;
+	}
+
+	// int available()
+	// {
+	// 	std::string s;
+	// 	s << std::cin;
+	// 	return s.length();
+	// }
+
+	// char read()
+	// {
+	// 	char c;
+	// 	s >> c;
+	// 	return c;
+	// }
+};
+
+class HardwareSerial: public Print {};
+
+HardwareSerial Serial, Serial2;
+
+// gazebo::common::Time curTime = this->dataPtr->model->GetWorld()->SimTime();
+
+// unsigned long micros()
+// {
+// 	// return (unsigned long) (esp_timer_get_time());
+// 	TODO:
+// }

gazebo/flix.cpp

@@ -0,0 +1,297 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// https://classic.gazebosim.org/tutorials?tut=plugins_model&cat=write_plugin
+// https://classic.gazebosim.org/tutorials?tut=set_velocity&cat=
+// https://github.com/gazebosim/gazebo-classic/blob/gazebo11/plugins/ArduCopterPlugin.cc
+// https://github.com/gazebosim/gazebo-classic/blob/gazebo11/plugins/ArduCopterPlugin.cc#L510 - motor
+// https://classic.gazebosim.org/tutorials?tut=gui_overlay&cat=user_input
+// https://github.com/gazebosim/gazebo-classic/blob/gazebo9/examples/plugins/gui_overlay_plugin_time/GUIExampleTimeWidget.cc
+// https://github.com/yujinrobot/kobuki_desktop/blob/ea5b7283d92f61efbd1a2185b46e1ad344e7e81a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp#L29
+// https://github.com/osrf/swarm/blob/1a2e4040b12b686ed7a13e32301d538b1c7d0b1d/src/RobotPlugin.cc#L936
+
+// motors thrust: https://www.youtube.com/watch?v=VtKI4Pjx8Sk
+
+// https://github.com/gazebosim/gazebo-classic/tree/master/examples/plugins
+
+// publish to topics https://github.com/wuwushrek/sim_cf/blob/df68af275c9f753d9bf1b0494a4e513d9f4c9a7c/crazyflie_gazebo/src/gazebo_lps_plugin.cpp#L104
+// https://github.com/bitcraze/crazyflie-simulation
+
+// GUI overlay:
+// https://github.com/gazebosim/gazebo-classic/blob/gazebo9/examples/plugins/gui_overlay_plugin_time/GUIExampleTimeWidget.cc
+
+#include <functional>
+#include <cmath>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/rendering/rendering.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Quaternion.hh>
+#include <iostream>
+#include <fstream>
+
+#include "arduino.hpp"
+#include "flix.hpp"
+#include "turnigy.hpp"
+#include "util.ino"
+#include "estimate.ino"
+#include "control.ino"
+
+using ignition::math::Vector3d;
+using ignition::math::Pose3d;
+using namespace gazebo;
+using namespace std;
+
+Pose3d flu2frd(const Pose3d& p)
+{
+	return ignition::math::Pose3d(p.Pos().X(), -p.Pos().Y(), -p.Pos().Z(),
+	                              p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().Z());
+}
+
+class ModelFlix : public ModelPlugin
+{
+private:
+	physics::ModelPtr model, estimateModel;
+	physics::LinkPtr body;
+	sensors::ImuSensorPtr imu;
+	common::Time _stepTime;
+	event::ConnectionPtr updateConnection, resetConnection;
+	transport::NodePtr nodeHandle;
+	transport::PublisherPtr motorPub[4];
+	ofstream log;
+
+public:
+	void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
+	{
+		this->model = _parent;
+		this->body = this->model->GetLink("body");
+
+		this->imu = std::dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor(model->GetScopedName(true) + "::body::imu")); // default::flix::body::imu
+		if (imu == nullptr) {
+			gzerr << "IMU sensor not found" << std::endl;
+			return;
+		}
+
+		this->estimateModel = model->GetWorld()->ModelByName("flix_estimate");
+
+		// auto scene = rendering::get_scene();
+		// motorVisual[0] = scene->GetVisual("motor0");
+		// motorVisual[1] = scene->GetVisual("motor1");
+		// motorVisual[2] = scene->GetVisual("motor2");
+		// motorVisual[3] = scene->GetVisual("motor3");
+		// motorVisual[0] = model->GetVisual("motor0");
+
+		this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+			std::bind(&ModelFlix::OnUpdate, this));
+
+		this->resetConnection = event::Events::ConnectWorldReset(
+			std::bind(&ModelFlix::OnReset, this));
+
+		initNode();
+
+		gzmsg << "Flix plugin loaded" << endl;
+	}
+
+public:
+	void OnReset()
+	{
+		attitude = Quaternion();
+		gzmsg << "Flix plugin reset" << endl;
+	}
+
+	void OnUpdate()
+	{
+		// this->model->SetLinearVel(ignition::math::Vector3d(.3, 0, 0));
+
+		// this->model->GetLink("body")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96));
+		// this->model->GetLink("body")->SetTorque(1.0, ignition::math::Vector3d(1.0, 0.0, 0.0);
+
+		// this->model->GetLink("motor0")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
+		// this->model->GetLink("motor1")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
+		// this->model->GetLink("motor2")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
+		// this->model->GetLink("motor3")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4))
+
+		// === GROUNDTRUTH ORIENTATION ===
+		// auto pose = flu2frd(this->model->WorldPose());
+		// attitude = Quaternion(pose.Rot().W(), pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z());
+		// auto vel = this->model->RelativeAngularVel();
+		// rates = Vector(vel.X(), -vel.Y(), -vel.Z()); // flu to frd
+
+		// === GROUNDTRUTH POSITION ===
+		// auto pose = flu2frd(this->model->WorldPose());
+		// position = Vector(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
+		// auto vel = this->model->RelativeLinearVel();
+		// velocity = Vector(vel.X(), -vel.Y(), -vel.Z());
+
+		/// == IMU ===
+		auto imuRates = imu->AngularVelocity();
+		rates = Vector(imuRates.X(), -imuRates.Y(), -imuRates.Z()); // flu to frd
+		auto imuAccel = imu->LinearAcceleration();
+		acc = Vector(imuAccel.X(), -imuAccel.Y(), -imuAccel.Z()); // flu to frd
+
+		// gazebo::common::Time curTime = this->dataPtr->model->GetWorld()->SimTime();
+
+		turnigyGet();
+		controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
+		controls[RC_CHANNEL_AUX] = 1; // armed
+		// std::cout << "yaw: " << yaw << " pitch: " << pitch << " roll: " << roll << " throttle: " << throttle << std::endl;
+
+		if (std::isnan(dt)) {
+			// first step
+			dt = 0;
+		} else {
+			dt = (this->model->GetWorld()->SimTime() - _stepTime).Double();
+		}
+		_stepTime = this->model->GetWorld()->SimTime();
+		stepTime = _stepTime.Double() * 1000000;
+		// std::cout << "dt: " << dt << std::endl;
+
+		estimate();
+
+		// fix yaw
+		attitude.setYaw(-this->model->WorldPose().Yaw());
+		// Serial.print("attitude: "); Serial.println(attitude);
+		// controlMission();
+		// controlPosition();
+
+		// auto pose = flu2frd(this->model->WorldPose());
+
+		control();
+		applyMotorsThrust();
+		updateEstimatePose();
+		// autotune();
+
+		/*
+		working:
+		const double max_thrust = 2.2;
+		double thrust = max_thrust * throttle;
+
+		// rate setpoint
+		double rx = roll * 0.1;
+		double ry = -pitch * 0.1;
+		double rz = yaw * 0.1;
+
+		// this->model->GetLink("body")->AddForce(ignition::math::Vector3d(0.0, 0.0, 2.5 * throttle));
+
+		const double d = 0.035355;
+		double front_left = thrust + ry + rx;
+		double front_right = thrust + ry - rx;
+		double rear_left = thrust - ry + rx;
+		double rear_right = thrust - ry - rx;
+
+		if (throttle < 0.1) return;
+
+		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, front_left), ignition::math::Vector3d(d, d, 0.0));
+		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, front_right), ignition::math::Vector3d(d, -d, 0.0));
+		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, rear_left), ignition::math::Vector3d(-d, d, 0.0));
+		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, rear_right), ignition::math::Vector3d(-d, -d, 0.0));
+		*/
+
+		publishTopics();
+		logData();
+	}
+
+	void applyMotorsThrust()
+	{
+		// thrusts
+		const double d = 0.035355;
+		const double maxThrust = 0.03 * ONE_G; // 30 g, https://www.youtube.com/watch?v=VtKI4Pjx8Sk
+		// 65 mm prop ~40 g
+
+		// std::cout << "fr: " << motors[MOTOR_FRONT_RIGHT]
+		// 	<< " fl: " << motors[MOTOR_FRONT_LEFT]
+		// 	<< " rr: " << motors[MOTOR_REAR_RIGHT]
+		// 	<< " rl: " << motors[MOTOR_REAR_LEFT] << std::endl;
+
+		const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05;
+		const float minThrustRel = 0;
+
+		// apply min thrust
+		float mfl = mapff(motors[MOTOR_FRONT_LEFT], 0, 1, minThrustRel, 1);
+		float mfr = mapff(motors[MOTOR_FRONT_RIGHT], 0, 1, minThrustRel, 1);
+		float mrl = mapff(motors[MOTOR_REAR_LEFT], 0, 1, minThrustRel, 1);
+		float mrr = mapff(motors[MOTOR_REAR_RIGHT], 0, 1, minThrustRel, 1);
+
+		if (motors[MOTOR_FRONT_LEFT] < 0.001) mfl = 0;
+		if (motors[MOTOR_FRONT_RIGHT] < 0.001) mfr = 0;
+		if (motors[MOTOR_REAR_LEFT] < 0.001) mrl = 0;
+		if (motors[MOTOR_REAR_RIGHT] < 0.001) mrr = 0;
+
+		// const float scale0 = 1.0, scale1 = 1.0, scale2 = 1.0, scale3 = 1.0;
+
+		// TODO: min_thrust
+
+		body->AddLinkForce(Vector3d(0.0, 0.0, scale0 * maxThrust * abs(mfl)), Vector3d(d, d, 0.0));
+		body->AddLinkForce(Vector3d(0.0, 0.0, scale1 * maxThrust * abs(mfr)), Vector3d(d, -d, 0.0));
+		body->AddLinkForce(Vector3d(0.0, 0.0, scale2 * maxThrust * abs(mrl)), Vector3d(-d, d, 0.0));
+		body->AddLinkForce(Vector3d(0.0, 0.0, scale3 * maxThrust * abs(mrr)), Vector3d(-d, -d, 0.0));
+
+		// TODO: indicate if > 1
+
+		// torque
+		const double maxTorque = 0.0023614413; // 24.08 g*cm
+		int direction = 1;
+		// z is counter clockwise, normal rotation direction is minus
+		body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale0 * maxTorque * motors[MOTOR_FRONT_LEFT]));
+		body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale1 * -maxTorque * motors[MOTOR_FRONT_RIGHT]));
+		body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale2 * -maxTorque * motors[MOTOR_REAR_LEFT]));
+		body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale3 * maxTorque * motors[MOTOR_REAR_RIGHT]));
+	}
+
+	void updateEstimatePose() {
+		if (estimateModel == nullptr) {
+			return;
+		}
+		if (!attitude.finite()) {
+			// gzerr << "attitude is nan" << std::endl;
+			return;
+		}
+		Pose3d pose(
+			model->WorldPose().Pos().X(), model->WorldPose().Pos().Y(), model->WorldPose().Pos().Z(),
+			attitude.w, attitude.x, -attitude.y, -attitude.z // frd to flu
+		);
+		// std::cout << pose.Pos().X() << " " << pose.Pos().Y() << " " << pose.Pos().Z() <<
+		// 	" " << pose.Rot().W() << " " << pose.Rot().X() << " " << pose.Rot().Y() << " " << pose.Rot().Z() << std::endl;
+
+		// calculate attitude estimation error
+
+		float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
+		if (angle < 0.3) {
+			//gzwarn << "att err: " << angle << endl;
+			// TODO: warning
+			// position under the floor to make it invisible
+			pose.SetZ(-5);
+		}
+
+		estimateModel->SetWorldPose(pose);
+
+	}
+
+	void initNode() {
+		nodeHandle = transport::NodePtr(new transport::Node());
+		nodeHandle->Init(); // TODO: namespace
+		motorPub[0] = nodeHandle->Advertise<msgs::Int>("~/motor0");
+		motorPub[1] = nodeHandle->Advertise<msgs::Int>("~/motor1");
+		motorPub[2] = nodeHandle->Advertise<msgs::Int>("~/motor2");
+		motorPub[3] = nodeHandle->Advertise<msgs::Int>("~/motor3");
+	}
+
+	void publishTopics() {
+		for (int i = 0; i < 4; i++) {
+			msgs::Int msg;
+			msg.set_data(static_cast<int>(std::round(motors[i] * 1000)));
+			motorPub[i]->Publish(msg);
+		}
+	}
+
+	void logData() {
+		if (!log.is_open()) return;
+		log << this->model->GetWorld()->SimTime() << "\t" << rollRatePID.derivative << "\t" << pitchRatePID.derivative << "\n";
+	}
+};
+
+GZ_REGISTER_MODEL_PLUGIN(ModelFlix)

gazebo/flix.hpp

@@ -0,0 +1,51 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include <cmath>
+#include "vector.hpp"
+#include "quaternion.hpp"
+
+#define ONE_G 9.807f
+
+#define RC_CHANNELS 6
+// #define RC_CHANNEL_THROTTLE 2
+// #define RC_CHANNEL_YAW 3
+// #define RC_CHANNEL_PITCH 1
+// #define RC_CHANNEL_ROLL 0
+// #define RC_CHANNEL_AUX 4
+// #define RC_CHANNEL_MODE 5
+
+#define MOTOR_REAR_LEFT 0
+#define MOTOR_FRONT_LEFT 3
+#define MOTOR_FRONT_RIGHT 2
+#define MOTOR_REAR_RIGHT 1
+
+Vector acc;
+Vector rates;
+Quaternion attitude;
+float dt = NAN;
+float motors[4]; // normalized motors thrust in range [-1..1]
+int16_t channels[16]; // raw rc channels WARNING: unsigned in real life
+float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for thrust)
+uint32_t stepTime;
+
+// util
+float mapf(long x, long in_min, long in_max, float out_min, float out_max);
+float mapff(float x, float in_min, float in_max, float out_min, float out_max);
+// float hypot3(float x, float y, float z);
+
+// rc
+void normalizeRC();
+
+// control
+void control();
+void interpretRC();
+static void controlAttitude();
+static void controlAttitudeAlter();
+static void controlManual();
+static void controlRate();
+void desaturate(float& a, float& b, float& c, float& d);
+static void indicateSaturation();
+
+// mocks
+void setLED(bool on) {}

gazebo/flix.world

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+	<world name="default">
+		<scene>
+			<ambient>1 1 1 1</ambient>
+		</scene>
+		<gui>
+			<!-- <camera name="user_camera">
+				<pose>-1 0 0.3 0 0 0</pose>
+				<track_visual>
+					<name>flix</name>
+					<min_dist>2</min_dist>
+					<max_dist>2</max_dist>
+					<use_model_frame>1</use_model_frame>
+					<static>1</static>
+					<inherit_yaw>1</inherit_yaw>
+				</track_visual>
+			</camera> -->
+			<camera name="user_camera">
+				<pose>-2 -0.3 1.5 0 0.5 0.1</pose>
+			</camera>
+		</gui>
+		<physics type="ode">
+			<!-- <real_time_update_rate>100</real_time_update_rate> -->
+			<max_step_size>0.001</max_step_size>
+		</physics>
+		<include>
+			<uri>model://floor</uri>
+		</include>
+		<include>
+			<uri>model://sun</uri>
+		</include>
+		<include>
+			<uri>model://flix</uri>
+			<pose>0 0 0.2 0 0 0</pose>
+		</include>
+		<include>
+			<uri>model://Table</uri>
+			<pose>0.2 1.5 0 0 0 0</pose>
+		</include>
+
+		<!-- TODO: redesign, move to flix model -->
+		<!-- <box><size>0.1 0.1 0.02</size></box> -->
+		<model name="flix_estimate">
+			<static>true</static>
+			<link name="estimate">
+				<visual name="estimate">
+					<pose>0 0 0 0 0 1.57</pose>
+					<geometry>
+						<!-- <mesh>
+							<uri>model://flix/plane.dae</uri>
+							<scale>0.003 0.003 0.003</scale>
+						</mesh> -->
+						<box>
+							<size>0.125711 0.125711 0.022</size>
+						</box>
+					</geometry>
+				</visual>
+			</link>
+		</model>
+	</world>
+</sdf>

gazebo/models/flix/flix.sdf

@@ -0,0 +1,172 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+	<model name="flix">
+		<static>false</static>
+		<link name="body">
+			<inertial>
+				<mass>0.065</mass>
+				<inertia>
+					<ixx>3.55E-5</ixx>
+					<iyy>4.23E-5</iyy>
+					<izz>7.47E-5</izz>
+				</inertia>
+			</inertial>
+			<collision name="collision">
+				<geometry>
+					<box>
+						<size>0.125711 0.125711 0.022</size>
+					</box>
+				</geometry>
+			</collision>
+			<visual name="body">
+				<geometry>
+					<mesh><uri>model://flix/flix.dae</uri></mesh>
+					<!-- <box>
+						<size>0.125711 0.125711 0.022</size>
+					</box> -->
+					<!-- led -->
+				</geometry>
+			</visual>
+			<!-- motors visual -->
+			<!-- <visual name="prop0">
+				<geometry>
+					<mesh><uri>model://flix/prop.dae</uri></mesh>
+				</geometry>
+				<pose relative_to="body">-0.035355 0.035355 0.011 0 0 0</pose>
+			</visual>
+			<visual name="prop1">
+				<geometry>
+					<mesh><uri>model://flix/prop.dae</uri></mesh>
+				</geometry>
+				<pose relative_to="body">-0.035355 -0.035355 0.011 0 0 1</pose>
+			</visual>
+			<visual name="prop2">
+				<geometry>
+					<mesh><uri>model://flix/prop.dae</uri></mesh>
+				</geometry>
+				<pose relative_to="body">0.035355 -0.035355 0.011 0 0 0</pose>
+			</visual>
+			<visual name="prop3">
+				<geometry>
+					<mesh><uri>model://flix/prop.dae</uri></mesh>
+				</geometry>
+				<pose relative_to="body">0.035355 0.035355 0.011 0 0 0</pose>
+			</visual> -->
+			<sensor name="imu" type="imu">
+				<always_on>1</always_on>
+				<visualize>1</visualize>
+				<update_rate>1000</update_rate>
+				<imu>
+					<angular_velocity>
+						<x>
+							<noise type="gaussian">
+								<mean>0.0</mean>
+								<stddev>1.5e-2</stddev>
+								<!-- <bias_mean>0.0000075</bias_mean>
+								<bias_stddev>0.0000008</bias_stddev> -->
+							</noise>
+						</x>
+						<y>
+							<noise type="gaussian">
+								<mean>0.0</mean>
+								<stddev>1.5e-2</stddev>
+								<!-- <bias_mean>0.0000075</bias_mean>
+								<bias_stddev>0.0000008</bias_stddev> -->
+							</noise>
+						</y>
+						<z>
+							<noise type="gaussian">
+								<mean>0.0</mean>
+								<stddev>1.5e-2</stddev>
+								<!-- <bias_mean>0.0000075</bias_mean>
+								<bias_stddev>0.0000008</bias_stddev> -->
+							</noise>
+						</z>
+					</angular_velocity>
+					<linear_acceleration>
+						<x>
+							<noise type="gaussian">
+								<mean>0.0</mean>
+								<stddev>3.5e-1</stddev>
+								<!-- <bias_mean>0.1</bias_mean>
+								<bias_stddev>0.001</bias_stddev> -->
+							</noise>
+						</x>
+						<y>
+							<noise type="gaussian">
+								<mean>0.0</mean>
+								<stddev>3.5e-1</stddev>
+								<!-- <bias_mean>0.1</bias_mean>
+								<bias_stddev>0.001</bias_stddev> -->
+							</noise>
+						</y>
+						<z>
+							<noise type="gaussian">
+								<mean>0.0</mean>
+								<stddev>3.5e-1</stddev>
+								<!-- <bias_mean>0.1</bias_mean>
+								<bias_stddev>0.001</bias_stddev> -->
+							</noise>
+						</z>
+					</linear_acceleration>
+				</imu>
+
+			</sensor>
+		</link>
+
+		<!-- <link name="imu_link">
+			<inertial>
+				<mass>0.001</mass>
+				<inertia>
+					<ixx>1e-05</ixx>
+					<ixy>0</ixy>
+					<ixz>0</ixz>
+					<iyy>1e-05</iyy>
+					<iyz>0</iyz>
+					<izz>1e-05</izz>
+				</inertia>
+			</inertial>
+			<sensor name="imu" type="imu">
+				<always_on>1</always_on>
+				<visualize>1</visualize>
+				<update_rate>50</update_rate>
+			</sensor>
+		</link>
+		<joint name="hokuyo_joint" type="fixed">
+			<child>imu_link</child>
+			<parent>body</parent>
+		</joint> -->
+
+		<!-- <link name="motor0"></link>
+		<link name="motor1"></link>
+		<link name="motor2"></link>
+		<link name="motor3"></link>
+
+		<joint name="motor0_joint" type="fixed">
+			<parent>body</parent>
+			<child>motor0</child>
+			<pose relative_to="body">-0.035355 0.035355 0 0 0 0</pose>
+		</joint>
+		<joint name="motor1_joint" type="fixed">
+			<parent>body</parent>
+			<child>motor1</child>
+			<pose relative_to="body">-0.035355 -0.035355 0 0 0 0</pose>
+		</joint>
+		<joint name="motor2_joint" type="fixed">
+			<parent>body</parent>
+			<child>motor2</child>
+			<pose relative_to="body">0.035355 -0.035355 0 0 0 0</pose>
+		</joint>
+		<joint name="motor3_joint" type="fixed">
+			<parent>body</parent>
+			<child>motor3</child>
+			<pose relative_to="body">0.035355 0.035355 0 0 0 0</pose>
+		</joint> -->
+
+		<plugin name="flix" filename="libflix.so">
+			<always_on>1</always_on>
+			<robotNamespace></robotNamespace>
+			<visualize>1</visualize>
+		</plugin>
+	</model>
+</sdf>

gazebo/models/flix/model.config

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<model>
+    <name>flix</name>
+    <version>1.0</version>
+    <sdf version="1.5">flix.sdf</sdf>
+    <author>
+        <name>Oleg Kalachev</name>
+        <email>okalachev@gmail.com</email>
+    </author>
+    <license>Unknown</license>
+    <description>
+        Flix quadrotor
+    </description>
+</model>

gazebo/models/floor/floor.sdf

@@ -0,0 +1,31 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="floor">
+    <static>true</static>
+    <link name="link">
+      <pose>0 0 -0.02 0 0 0</pose>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>200 200 .02</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>200 200 .02</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://floor/materials/scripts</uri>
+            <uri>model://floor/materials/textures</uri>
+            <name>parquet</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

gazebo/models/floor/materials/scripts/floor.material

@@ -0,0 +1,20 @@
+material parquet
+{
+	technique
+	{
+		pass
+		{
+			ambient 0.5 0.5 0.5 1.0
+			diffuse 0.5 0.5 0.5 1.0
+			specular 0.2 0.2 0.2 1.0 12.5
+
+			texture_unit
+			{
+				texture floor.jpg
+				filtering anistropic
+				max_anisotropy 16
+				scale 0.01 0.01
+			}
+		}
+	}
+}

gazebo/models/floor/model.config

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Floor</name>
+  <version>1.0</version>
+  <sdf version="1.5">floor.sdf</sdf>
+
+  <author>
+    <name>Oleg Kalachev</name>
+    <email>okalachev@gmail.com</email>
+  </author>
+
+  <license>Unknown</license>
+
+  <description>
+    Floor.
+  </description>
+
+</model>

gazebo/turnigy.hpp

@@ -0,0 +1,70 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+#include <SDL2/SDL.h>
+#include <gazebo/gazebo.hh>
+#include <iostream>
+
+using namespace std;
+
+static const int16_t channelNeutralMin[] = {-1290,	-258,	-26833,	0,	0, 0};
+static const int16_t channelNeutralMax[] = {-1032,	-258,	-27348,	3353,	0, 0};
+
+static const int16_t channelMax[] =        {27090,	27090,	27090,	27090, 0, 0};
+
+#define RC_CHANNEL_THROTTLE 2
+#define RC_CHANNEL_YAW 3
+#define RC_CHANNEL_PITCH 1
+#define RC_CHANNEL_ROLL 0
+#define RC_CHANNEL_AUX 4
+#define RC_CHANNEL_MODE 5
+
+static SDL_Joystick *joystick;
+
+bool turnigyInitialized = false, warnShown = false;
+
+void turnigyInit()
+{
+	SDL_Init(SDL_INIT_JOYSTICK);
+	joystick = SDL_JoystickOpen(0);
+	if (joystick != NULL) {
+		turnigyInitialized = true;
+		gzmsg << "Joystick initialized: " << SDL_JoystickNameForIndex(0) << endl;
+	} else if (!warnShown) {
+		gzwarn << "Joystick not found, begin waiting for joystick..." << endl;
+		warnShown = true;
+	}
+}
+
+void turnigyGet()
+{
+	if (!turnigyInitialized) {
+		turnigyInit();
+		return;
+	}
+
+	SDL_JoystickUpdate();
+
+	for (uint8_t i = 0; i < 4; i++) {
+		channels[i] = SDL_JoystickGetAxis(joystick, i);
+	}
+
+	normalizeRC();
+}
+
+void normalizeRC() {
+	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
+		if (channels[i] >= channelNeutralMin[i] && channels[i] <= channelNeutralMax[i]) {
+			controls[i] = 0; // make neutral position strictly equal to 0
+		// } else if (channels[i] > channelNeutralMax[i]) {
+		// 	controls[i] = mapf(channels[i], channelNeutralMax[i], channelMax[i], 0, 1);
+		// } else {
+		// 	float channelMin = channelNeutralMin[i] - (channelMax[i] - channelNeutralMax[i]);
+		// 	controls[i] = mapf(channels[i], channelNeutralMin[i], channelMin, -1, 0);
+		// }
+		} else {
+			controls[i] = mapf(channels[i], (channelNeutralMin[i] + channelNeutralMax[i]) / 2, channelMax[i], 0, 1);
+		}
+	}
+	controls[RC_CHANNEL_THROTTLE] = constrain(controls[RC_CHANNEL_THROTTLE], 0, 1);
+}