Reduce angle drift adding level correction to the estimator

Utilize a-priori knowledge that the overall average attitude of the flying drone is level.

README.md

@@ -77,7 +77,7 @@ Additional articles:
 |Motor|8520 3.7V brushed motor.<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).<br>Make sure the motor shaft diameter and propeller hole diameter match!|<img src="docs/img/motor.jpeg" width=100>|4|
 |Propeller|55 mm (alternatively 65 mm)|<img src="docs/img/prop.jpg" width=100>|4|
 |MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|
-|Pull-down resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|4|
+|Pull-down resistor<br>Voltage measurement resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|6|
 |3.7V Li-Po battery|LW 952540 (or any compatible by the size)|<img src="docs/img/battery.jpg" width=100>|1|
 |Battery connector cable|MX2.0 2P female|<img src="docs/img/mx.png" width=100>|1|
 |Li-Po Battery charger|Any|<img src="docs/img/charger.jpg" width=100>|1|
@@ -154,7 +154,11 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir
   |VIN|VCC (or 3.3V depending on the receiver)|
   |Signal (TX)|GPIO4¹|
 
-*¹ — UART2 RX pin was [changed](https://docs.espressif.com/projects/arduino-esp32/en/latest/migration_guides/2.x_to_3.0.html#id14) to GPIO4 in Arduino ESP32 core 3.0.*
+  *¹ — UART2 RX pin was [changed](https://docs.espressif.com/projects/arduino-esp32/en/latest/migration_guides/2.x_to_3.0.html#id14) to GPIO4 in Arduino ESP32 core 3.0.*
+
+* Optionally connect the battery voltage divider for voltage monitoring to any ADC1 pin (e. g. *GPIO32* on ESP32, *GPIO3* on ESP32S3).
+
+  ESP32 and ESP32S3 [can measure](https://docs.espressif.com/projects/arduino-esp32/en/latest/api/adc.html#analogsetattenuation) up to 3.1 V and ESP32S3/ESP32C3 can measure up to 2.5 V, so choose the voltage divider resistors accordingly.
 
 ## Resources
 

docs/troubleshooting.md

@@ -13,7 +13,7 @@ Do the following:
 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 startup output.
+* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button or `reboot` command to see the whole startup 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/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
 * **Check if the console is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the console using QGroundControl *(Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console)*.
@@ -35,5 +35,7 @@ Do the following:
 * **Check the propeller directions are correct**. Make sure your propeller types (A or B) are installed as on the picture:
   <img src="img/user/peter_ukhov-2/1.jpg" width="200">
 * **Check the remote control**. Using `rc` command, check the control values reflect your sticks movement. All the controls should change between -1 and 1, and throttle between 0 and 1.
-* If using SBUS receiver, **calibrate the RC**. Type `cr` command in Serial Monitor and follow the instructions.
+* **If using SBUS receiver**:
+  * **Define the used GPIO pin** in `RC_RX_PIN` parameter.
+  * **Calibrate the RC** using `cr` command in the console.
 * **Check the IMU output using QGroundControl**. Connect to the drone using QGroundControl on your computer. Go to the *Analyze* tab, *MAVLINK Inspector*. Plot the data from the `SCALED_IMU` message. The gyroscope and accelerometer data should change according to the drone movement.

docs/usage.md

@@ -112,7 +112,7 @@ You can also work with parameters using `p` command in the console. Parameter na
 
 ### Define IMU orientation
 
-Use parameters, to define the IMU board axes orientation relative to the drone's axes: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
+The IMU orientation (relative to the drone's axes) is defined using the parameters: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
 
 The drone has *X* axis pointing forward, *Y* axis pointing left, and *Z* axis pointing up, and the supported IMU boards have *X* axis pointing to the pins side and *Z* axis pointing up from the component side:
 
@@ -138,37 +138,48 @@ Before flight you need to calibrate the accelerometer:
 
 If using non-default motor pins, set the pin numbers using the parameters: `MOTOR_PIN_FL`, `MOTOR_PIN_FR`, `MOTOR_PIN_RL`, `MOTOR_PIN_RR` (front-left, front-right, rear-left, rear-right respectively).
 
+Certain ESP32 models (such as ESP32-S3) support a lower maximum PWM frequency; on these boards the parameter `MOT_PWM_FREQ` should be set to 40000 Hz.
+
 If using brushless motors and ESCs:
 
 1. Set the appropriate PWM using the parameters: `MOT_PWM_STOP`, `MOT_PWM_MIN`, and `MOT_PWM_MAX` (1000, 1000, and 2000 is typical).
 2. Decrease the PWM frequency using the `MOT_PWM_FREQ` parameter (400 is typical).
 
-Reboot the drone to apply the changes.
-
 > [!CAUTION]
 > **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
 
-### Check everything works
+### Battery voltage monitoring
 
-1. Check the IMU is working: perform `imu` command and check its output:
+ESP32 ADC can measure only up to 3.3 V, so you need to use a voltage divider to monitor the battery voltage. To enable voltage measurement, set the following parameters:
+
+1. `PWR_VOLT_PIN` — GPIO pin number where the voltage divider is connected (*-1* to disable).
+2. `PWR_VOLT_SCALE` — voltage divider coefficient (*2* for two equal resistors).
+
+After this setup, you should see the battery voltage in QGroundControl top panel or using `pw` command in the console.
+
+### Important: check everything works
+
+1. Check the IMU is working: perform `imu` command in the console and check the output:
 
    * The `status` field should be `OK`.
    * The `rate` field should be about 1000 (Hz).
    * The `accel` and `gyro` fields should change as you move the drone.
+   * The `accel bias` and `accel scale` fields should contain calibration parameters (not zeros and ones).
+   * The `gyro bias` field should contain estimated gyro bias (not zeros).
    * The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
 
 2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video:
 
     <a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
 
-3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!**
+3. Perform motor tests. Use the following commands **— remove the propellers before running the tests!**
 
-   * `mfr` — should rotate front right motor (counter-clockwise).
+   * `mfr` — rotate front right motor (counter-clockwise).
-   * `mfl` — should rotate front left motor (clockwise).
+   * `mfl` — rotate front left motor (clockwise).
-   * `mrl` — should rotate rear left motor (counter-clockwise).
+   * `mrl` — rotate rear left motor (counter-clockwise).
-   * `mrr` — should rotate rear right motor (clockwise).
+   * `mrr` — rotate rear right motor (clockwise).
 
-   Rotation diagram:
+   Make sure rotation directions and propeller types match the following diagram:
 
    <img src="img/motors.svg" width=200>
 
@@ -193,11 +204,13 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
 
 ### Control with a remote control
 
-Before using remote SBUS-connected remote control, you need to calibrate it:
+Before using SBUS-connected remote control you need to enable SBUS and calibrate it:
 
-1. Access the console using QGroundControl (recommended) or Serial Monitor.
+1. Connect to the drone using QGroundControl.
-2. Type `cr` command and follow the instructions.
+2. In parameters, set the `RC_RX_PIN` parameter to the GPIO pin number where the SBUS signal is connected, for example: 4. Negative value disables SBUS.
-3. Use the remote control to fly the drone!
+3. Reboot the drone to apply changes.
+4. Open the console, type `cr` command and follow the instructions to calibrate the remote control.
+5. Use the remote control to fly the drone!
 
 ### Control with a USB remote control
 
@@ -234,11 +247,11 @@ When finished flying, **disarm** the drone, moving the left stick to the bottom
 
 ### Flight modes
 
-Flight mode is changed using mode switch on the remote control or using the command line.
+Flight mode is changed using mode switch on the remote control (if configured) or using the console commands. The main flight mode is *STAB*. In order to change modes using SBUS remote control, set the parameters: `CTL_FLT_MODE_0`, `CTL_FLT_MODE_1`, and `CTL_FLT_MODE_2` to required mode numbers (0 for *RAW*, 1 for *ACRO*, 2 for *STAB*, 3 for *AUTO*).
 
 #### STAB
 
-The default mode is *STAB*. In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
+In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
 
 > [!IMPORTANT]
 > The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually.
@@ -253,9 +266,9 @@ In this mode, the pilot controls the angular rates. This control method is diffi
 
 #### AUTO
 
-In this mode, the pilot inputs are ignored (except the mode switch, if configured). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed autonomous behavior.
+In this mode, the pilot inputs are ignored (except the mode switch). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed behavior.
 
-If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
+If the pilot moves the control sticks and mode switch is not configured, the drone will switch back to *STAB* mode.
 
 ## Wi-Fi configuration
 

docs/user.md

@@ -4,6 +4,22 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
+Author: [FanBy0ru](https://https://github.com/FanBy0ru).<br>
+Description: custom 3D-printed frame.<br>
+Frame STLs and flight validation: https://cults3d.com/en/3d-model/gadget/armature-pour-flix-drone.
+
+<img src="img/user/fanby0ru/1.jpg" height=200> <img src="img/user/fanby0ru/2.jpg" height=200>
+
+---
+
+Author: Ivan44 Phalko.<br>
+Description: custom PCB, cusom test bench.<br>
+[Flight validation](https://drive.google.com/file/d/17DNDJ1gPmCmDRAwjedCbJ9RXAyqMqqcX/view?usp=sharing).
+
+<img src="img/user/phalko/1.jpg" height=200> <img src="img/user/phalko/2.jpg" height=200> <img src="img/user/phalko/3.jpg" height=200>
+
+---
+
 Author: **Arkadiy "Arky" Matsekh**, Foucault Dynamics, Gold Coast, Australia.<br>
 The drone was built for the University of Queensland industry-led Master's capstone project.
 

flix/cli.ino

@@ -16,6 +16,7 @@ extern float controlTime;
 extern int mode;
 extern bool armed;
 extern LowPassFilter<Vector> gyroBiasFilter;
+extern float voltage;
 
 const char* motd =
 "\nWelcome to\n"
@@ -39,6 +40,7 @@ const char* motd =
 "disarm - disarm the drone\n"
 "raw/stab/acro/auto - set mode\n"
 "rc - show RC data\n"
+"pw - show power info\n"
 "wifi - show Wi-Fi info\n"
 "ap <ssid> <password> - setup Wi-Fi access point\n"
 "sta <ssid> <password> - setup Wi-Fi client mode\n"
@@ -135,6 +137,8 @@ void doCommand(String str, bool echo = false) {
 		print("time: %.1f\n", controlTime);
 		print("mode: %s\n", getModeName());
 		print("armed: %d\n", armed);
+	} else if (command == "pw") {
+		print("Voltage: %.1f V\n", voltage);
 	} else if (command == "wifi") {
 		printWiFiInfo();
 	} else if (command == "ap") {
@@ -174,7 +178,7 @@ void doCommand(String str, bool echo = false) {
 			String core = systemState[i].xCoreID == tskNO_AFFINITY ? "*" : String(systemState[i].xCoreID);
 			int cpuPercentage = systemState[i].ulRunTimeCounter / (totalRunTime / 100);
 			print("%-5d%-20s%-7d%-6d%-6s%d\n",systemState[i].xTaskNumber, systemState[i].pcTaskName,
-				systemState[i].usStackHighWaterMark, systemState[i].uxCurrentPriority, core, cpuPercentage);
+				systemState[i].usStackHighWaterMark, systemState[i].uxCurrentPriority, core.c_str(), cpuPercentage);
 		}
 		delete[] systemState;
 #endif

flix/control.ino

@@ -67,7 +67,7 @@ void control() {
 
 void interpretControls() {
 	if (controlMode < 0.25) mode = flightModes[0];
-	else if (controlMode < 0.75) mode = flightModes[1];
+	else if (controlMode <= 0.75) mode = flightModes[1];
 	else if (controlMode > 0.75) mode = flightModes[2];
 
 	if (mode == AUTO) return; // pilot is not effective in AUTO mode
@@ -149,12 +149,25 @@ void controlTorque() {
 	motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
 	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
 
+	desaturate(motors[MOTOR_FRONT_LEFT], motors[MOTOR_FRONT_RIGHT], motors[MOTOR_REAR_LEFT], motors[MOTOR_REAR_RIGHT]);
+
 	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 maxThrust = max(max(a, b), max(c, d));
+	if (maxThrust > 1) {
+		float diff = maxThrust - 1;
+		a -= diff;
+		b -= diff;
+		c -= diff;
+		d -= diff;
+	}
+}
+
 const char* getModeName() {
 	switch (mode) {
 		case RAW: return "RAW";

flix/estimate.ino

@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Attitude estimation from gyro and accelerometer
+// Attitude estimation using gyro and accelerometer
 
 #include "quaternion.h"
 #include "vector.h"
@@ -13,11 +13,13 @@ Quaternion attitude; // estimated attitude
 bool landed;
 
 float accWeight = 0.003;
+float levelWeight = 0.0002;
 LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
 
 void estimate() {
 	applyGyro();
 	applyAcc();
+	applyLevel();
 }
 
 void applyGyro() {
@@ -42,3 +44,12 @@ void applyAcc() {
 	// apply correction
 	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
 }
+
+void applyLevel() {
+	if (landed) return;
+
+	// assume the pilot keeps the drone more or less level in flight
+	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+	Vector correction = Vector::rotationVectorBetween(Vector(0, 0, 1), up) * levelWeight;
+	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
+}

flix/flix.ino

@@ -18,11 +18,11 @@ extern float motors[4];
 void setup() {
 	Serial.begin(115200);
 	print("Initializing flix\n");
-	disableBrownOut();
 	setupParameters();
+	setupPower();
 	setupLED();
-	setupMotors();
 	setLED(true);
+	setupMotors();
 	setupWiFi();
 	setupIMU();
 	setupRC();
@@ -39,6 +39,7 @@ void loop() {
 	sendMotors();
 	handleInput();
 	processMavlink();
+	readVoltage();
 	logData();
 	syncParameters();
 }

flix/imu.ino

@@ -121,7 +121,7 @@ void printIMUInfo() {
 	print("model: %s\n", imu.getModel());
 	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("gyro: %f %f %f\n", gyro.x, gyro.y, gyro.z);
 	print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
 	imu.waitForData();
 	Vector rawGyro, rawAcc;

flix/mavlink.ino

@@ -7,13 +7,15 @@
 #include "util.h"
 
 extern float controlTime;
+extern float voltage;
 
-bool mavlinkConnected = false;
-String mavlinkPrintBuffer;
 int mavlinkSysId = 1;
 Rate telemetryFast(10);
 Rate telemetrySlow(2);
 
+bool mavlinkConnected = false;
+String mavlinkPrintBuffer;
+
 void processMavlink() {
 	sendMavlink();
 	receiveMavlink();
@@ -38,12 +40,19 @@ void sendMavlink() {
 		mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 			MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
 		sendMessage(&msg);
+
+		uint16_t voltages[] = {voltage * 1000, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX};
+		uint16_t voltagesExt[] = {0, 0, 0, 0};
+		float remaining = constrain(mapf(voltage, 3.4, 4.2, 0, 1), 0, 1);
+		mavlink_msg_battery_status_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, MAV_BATTERY_FUNCTION_ALL,
+			MAV_BATTERY_TYPE_LIPO, INT16_MAX, voltages, -1, -1, -1, remaining * 100, 0, MAV_BATTERY_CHARGE_STATE_OK, voltagesExt, 0, 0);
+		sendMessage(&msg);
 	}
 
 	if (telemetryFast && mavlinkConnected) {
-		const float zeroQuat[] = {0, 0, 0, 0};
+		const float offset[] = {0, 0, 0, 0};
 		mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
-			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
+			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, offset); // convert to frd
 		sendMessage(&msg);
 
 		mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
@@ -56,7 +65,7 @@ void sendMavlink() {
 		sendMessage(&msg);
 
 		mavlink_msg_scaled_imu_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time,
-			acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd
+			acc.x / ONE_G * 1000, -acc.y / ONE_G * 1000, -acc.z / ONE_G * 1000, // convert to frd
 			gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
 			0, 0, 0, 0);
 		sendMessage(&msg);

flix/motors.ino

@@ -24,6 +24,7 @@ void setupMotors() {
 	// configure pins
 	for (int i = 0; i < 4; i++) {
 		ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
+		pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // when reconfiguring
 	}
 	sendMotors();
 	print("Motors initialized\n");

flix/parameters.ino

@@ -6,21 +6,26 @@
 #include <Preferences.h>
 #include "util.h"
 
-extern float channelZero[16];
+extern int channelZero[16];
-extern float channelMax[16];
+extern int channelMax[16];
-extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern int rcRxPin;
 extern int wifiMode, udpLocalPort, udpRemotePort;
 extern float rcLossTimeout, descendTime;
+extern int voltagePin;
+extern float voltageScale;
+extern LowPassFilter<float> voltageFilter;
 
 Preferences storage;
 
 struct Parameter {
-	const char *name; // max length is 15 (Preferences key limit)
+	const char *name; // max length is 15
 	bool integer;
-	union { float *f; int *i; }; // pointer to variable
+	union { float *f; int *i; }; // pointer to the variable
 	float cache; // what's stored in flash
-	Parameter(const char *name, float *variable) : name(name), integer(false), f(variable) {};
+	void (*callback)(); // called after parameter change
-	Parameter(const char *name, int *variable) : name(name), integer(true), i(variable) {};
+	Parameter(const char *name, float *variable, void (*callback)() = nullptr) : name(name), integer(false), f(variable), callback(callback) {};
+	Parameter(const char *name, int *variable, void (*callback)() = nullptr) : name(name), integer(true), i(variable), callback(callback) {};
 	float getValue() const { return integer ? *i : *f; };
 	void setValue(const float value) { if (integer) *i = value; else *f = value; };
 };
@@ -31,13 +36,16 @@ Parameter parameters[] = {
 	{"CTL_R_RATE_I", &rollRatePID.i},
 	{"CTL_R_RATE_D", &rollRatePID.d},
 	{"CTL_R_RATE_WU", &rollRatePID.windup},
+	{"CTL_R_RATE_D_A", &rollRatePID.lpf.alpha},
 	{"CTL_P_RATE_P", &pitchRatePID.p},
 	{"CTL_P_RATE_I", &pitchRatePID.i},
 	{"CTL_P_RATE_D", &pitchRatePID.d},
 	{"CTL_P_RATE_WU", &pitchRatePID.windup},
+	{"CTL_P_RATE_D_A", &pitchRatePID.lpf.alpha},
 	{"CTL_Y_RATE_P", &yawRatePID.p},
 	{"CTL_Y_RATE_I", &yawRatePID.i},
 	{"CTL_Y_RATE_D", &yawRatePID.d},
+	{"CTL_Y_RATE_D_A", &yawRatePID.lpf.alpha},
 	{"CTL_R_P", &rollPID.p},
 	{"CTL_R_I", &rollPID.i},
 	{"CTL_R_D", &rollPID.d},
@@ -65,18 +73,20 @@ Parameter parameters[] = {
 	{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
 	// estimate
 	{"EST_ACC_WEIGHT", &accWeight},
+	{"EST_LVL_WEIGHT", &levelWeight},
 	{"EST_RATES_LPF_A", &ratesFilter.alpha},
 	// motors
-	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT]},
+	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},
-	{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT]},
+	{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},
-	{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT]},
+	{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT], setupMotors},
-	{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT]},
+	{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT], setupMotors},
-	{"MOT_PWM_FREQ", &pwmFrequency},
+	{"MOT_PWM_FREQ", &pwmFrequency, setupMotors},
-	{"MOT_PWM_RES", &pwmResolution},
+	{"MOT_PWM_RES", &pwmResolution, setupMotors},
 	{"MOT_PWM_STOP", &pwmStop},
 	{"MOT_PWM_MIN", &pwmMin},
 	{"MOT_PWM_MAX", &pwmMax},
 	// rc
+	{"RC_RX_PIN", &rcRxPin},
 	{"RC_ZERO_0", &channelZero[0]},
 	{"RC_ZERO_1", &channelZero[1]},
 	{"RC_ZERO_2", &channelZero[2]},
@@ -106,13 +116,18 @@ Parameter parameters[] = {
 	{"MAV_SYS_ID", &mavlinkSysId},
 	{"MAV_RATE_SLOW", &telemetrySlow.rate},
 	{"MAV_RATE_FAST", &telemetryFast.rate},
+	// power
+	{"PWR_VOLT_PIN", &voltagePin},
+	{"PWR_VOLT_SCALE", &voltageScale},
+	{"PWR_VOLT_LPF_A", &voltageFilter.alpha},
 	// safety
 	{"SF_RC_LOSS_TIME", &rcLossTimeout},
 	{"SF_DESCEND_TIME", &descendTime},
 };
 
 void setupParameters() {
-	storage.begin("flix", false);
+	print("Setup parameters\n");
+	storage.begin("flix");
 	// Read parameters from storage
 	for (auto &parameter : parameters) {
 		if (!storage.isKey(parameter.name)) {
@@ -151,6 +166,7 @@ bool setParameter(const char *name, const float value) {
 		if (strcasecmp(parameter.name, name) == 0) {
 			if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
 			parameter.setValue(value);
+			if (parameter.callback) parameter.callback();
 			return true;
 		}
 	}
@@ -164,8 +180,7 @@ void syncParameters() {
 
 	for (auto &parameter : parameters) {
 		if (parameter.getValue() == parameter.cache) continue; // no change
-		if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NaN
+		if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NAN
-		if (isinf(parameter.getValue()) && isinf(parameter.cache)) continue; // both are Inf
 
 		storage.putFloat(parameter.name, parameter.getValue());
 		parameter.cache = parameter.getValue(); // update cache

flix/power.ino

@@ -0,0 +1,28 @@
+// Copyright (c) 2026 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Power management
+
+#include <soc/soc.h>
+#include <soc/rtc_cntl_reg.h>
+#include "lpf.h"
+#include "util.h"
+
+float voltage;
+LowPassFilter<float> voltageFilter(0.2);
+int voltagePin = -1;
+float voltageScale = 2;
+
+void setupPower() {
+	// Disable reset on low voltage
+	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
+}
+
+void readVoltage() {
+	if (voltagePin < 0) return;
+	static Rate rate(10);
+	if (!rate) return;
+
+	float v = analogReadMilliVolts(voltagePin) * voltageScale / 1000.0f;
+	voltage = voltageFilter.update(v);
+}

flix/rc.ino

@@ -7,24 +7,26 @@
 #include "util.h"
 
 SBUS rc(Serial2);
+int rcRxPin = -1; // -1 means disabled
 
 uint16_t channels[16]; // raw rc channels
-float channelZero[16]; // calibration zero values
+int channelZero[16]; // calibration zero values
-float channelMax[16]; // calibration max values
+int channelMax[16]; // calibration max values
 
 float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
 float controlMode = NAN;
 float controlTime = NAN; // time of the last controls update
 
-// Channels mapping (nan means not assigned):
+int rollChannel = -1, pitchChannel = -1, throttleChannel = -1, yawChannel = -1, modeChannel = -1; // channel mapping
-float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
 
 void setupRC() {
+	if (rcRxPin < 0) return;
 	print("Setup RC\n");
-	rc.begin();
+	rc.begin(rcRxPin);
 }
 
 bool readRC() {
+	if (rcRxPin < 0) return false;
 	if (rc.read()) {
 		SBUSData data = rc.data();
 		for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
@@ -41,14 +43,18 @@ void normalizeRC() {
 		controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
 	}
 	// Update control values
-	controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
+	controlRoll = rollChannel < 0 ? 0 : controls[rollChannel];
-	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
+	controlPitch = pitchChannel < 0 ? 0 : controls[pitchChannel];
-	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
+	controlYaw = yawChannel < 0 ? 0 : controls[yawChannel];
-	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
+	controlThrottle = throttleChannel < 0 ? 0 : controls[throttleChannel];
-	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
+	controlMode = modeChannel < 0 ? NAN : controls[modeChannel]; // mode control is ineffective if not mapped
 }
 
 void calibrateRC() {
+	if (rcRxPin < 0) {
+		print("RC_RX_PIN = %d, set the RC pin!\n", rcRxPin);
+		return;
+	}
 	uint16_t zero[16];
 	uint16_t center[16];
 	uint16_t max[16];
@@ -64,7 +70,7 @@ void calibrateRC() {
 	printRCCalibration();
 }
 
-void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {
+void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const char *str) {
 	print("%s", str);
 	pause(3);
 	for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
@@ -85,15 +91,15 @@ void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const
 		channelZero[ch] = in[ch];
 		channelMax[ch] = out[ch];
 	} else {
-		*channel = NAN;
+		*channel = -1;
 	}
 }
 
 void printRCCalibration() {
 	print("Control   Ch     Zero   Max\n");
-	print("Roll      %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);
+	print("Roll      %-7d%-7d%-7d\n", rollChannel, rollChannel < 0 ? 0 : channelZero[rollChannel], rollChannel < 0 ? 0 : channelMax[rollChannel]);
-	print("Pitch     %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
+	print("Pitch     %-7d%-7d%-7d\n", pitchChannel, pitchChannel < 0 ? 0 : channelZero[pitchChannel], pitchChannel < 0 ? 0 : channelMax[pitchChannel]);
-	print("Yaw       %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
+	print("Yaw       %-7d%-7d%-7d\n", yawChannel, yawChannel < 0 ? 0 : channelZero[yawChannel], yawChannel < 0 ? 0 : channelMax[yawChannel]);
-	print("Throttle  %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
+	print("Throttle  %-7d%-7d%-7d\n", throttleChannel, throttleChannel < 0 ? 0 : channelZero[throttleChannel], throttleChannel < 0 ? 0 : channelMax[throttleChannel]);
-	print("Mode      %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
+	print("Mode      %-7d%-7d%-7d\n", modeChannel, modeChannel < 0 ? 0 : channelZero[modeChannel], modeChannel < 0 ? 0 : channelMax[modeChannel]);
 }

flix/safety.ino

@@ -37,8 +37,8 @@ void descend() {
 void autoFailsafe() {
 	static float roll, pitch, yaw, throttle;
 	if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
-		// controls changed
+		// controls changed and mode switch is not configured
-		if (mode == AUTO) mode = STAB; // regain control by the pilot
+		if (mode == AUTO && invalid(controlMode)) mode = STAB; // regain control by the pilot
 	}
 	roll = controlRoll;
 	pitch = controlPitch;

flix/util.h

@@ -6,8 +6,6 @@
 #pragma once
 
 #include <math.h>
-#include <soc/soc.h>
-#include <soc/rtc_cntl_reg.h>
 
 const float ONE_G = 9.80665;
 extern float t;
@@ -35,11 +33,6 @@ float wrapAngle(float angle) {
 	return angle;
 }
 
-// Disable reset on low voltage
-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) {
 	str.trim();

flix/vector.h

@@ -105,10 +105,23 @@ public:
 	}
 
 	static Vector rotationVectorBetween(const Vector& a, const Vector& b) {
+		float an = a.norm();
+		float bn = b.norm();
+		if (an < 1e-6 || bn < 1e-6) {
+			return Vector(0, 0, 0);
+		}
 		Vector direction = cross(a, b);
-		if (direction.zero()) {
+		if (direction.norm() < 1e-6) { // vectors are parallel
-			// vectors are opposite, return any perpendicular vector
+			if (dot(a, b) > 0) { // same direction
-			return cross(a, Vector(1, 0, 0));
+				return Vector(0, 0, 0);
+			}
+			// opposite direction
+			Vector perp = cross(a, Vector(1, 0, 0));
+			if (perp.norm() < 1e-6) {
+				perp = cross(a, Vector(0, 1, 0));
+			}
+			perp.normalize();
+			return perp * PI;
 		}
 		direction.normalize();
 		float angle = angleBetween(a, b);

flix/wifi.ino

@@ -25,6 +25,7 @@ void setupWiFi() {
 	} else if (wifiMode == W_STA) {
 		WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str());
 	}
+	WiFi.setSleep(false); // disable power save
 	udp.begin(udpLocalPort);
 }
 

gazebo/Arduino.h

@@ -21,6 +21,8 @@
 #define degrees(rad) ((rad)*RAD_TO_DEG)
 
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+template<typename T> T max(T a, T b) { return a > b ? a : b; }
+template<typename T> T min(T a, T b) { return a < b ? a : b; }
 
 long map(long x, long in_min, long in_max, long out_min, long out_max) {
 	const long run = in_max - in_min;
@@ -165,6 +167,8 @@ void delay(uint32_t ms) {
 
 bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
 bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
+uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution) { return freq; }
+uint32_t analogReadMilliVolts(uint8_t pin) { return 0; }
 
 unsigned long __micros;
 unsigned long __resetTime = 0;

gazebo/SBUS.h

@@ -13,7 +13,7 @@ class SBUS {
 public:
 	SBUS(HardwareSerial& bus, const bool inv = true) {};
 	SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
-	void begin() {};
+	void begin(int rxpin = -1, int txpin = -1, bool inv = true, bool fast = false) {};
 	bool read() { return joystickInit(); };
 	SBUSData data() {
 		SBUSData data;

gazebo/flix.h

@@ -27,11 +27,13 @@ void step();
 void computeLoopRate();
 void applyGyro();
 void applyAcc();
+void applyLevel();
 void control();
 void interpretControls();
 void controlAttitude();
 void controlRates();
 void controlTorque();
+void desaturate(float& a, float& b, float& c, float& d);
 const char* getModeName();
 void sendMotors();
 int getDutyCycle(float value);
@@ -43,7 +45,7 @@ void doCommand(String str, bool echo);
 void handleInput();
 void normalizeRC();
 void calibrateRC();
-void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
+void calibrateRCChannel(int *channel, uint16_t zero[16], uint16_t max[16], const char *str);
 void printRCCalibration();
 void printLogHeader();
 void printLogData();

gazebo/simulator.cpp

@@ -27,6 +27,7 @@
 #include "mavlink.ino"
 #include "motors.ino"
 #include "parameters.ino"
+#include "power.ino"
 #include "rc.ino"
 #include "time.ino"
 

gazebo/soc/soc.h

@@ -1,4 +1,3 @@
 // Dummy file to make it possible to compile simulator with Flix' util.h
 
-#define WRITE_PERI_REG(addr, val) {}
 #define REG_CLR_BIT(_r, _b) {}

tools/example.py

@@ -10,6 +10,7 @@ print('Connected:', flix.connected)
 print('Mode:', flix.mode)
 print('Armed:', flix.armed)
 print('Landed:', flix.landed)
+print('Voltage:', flix.voltage, 'V')
 print('Rates:', *[f'{math.degrees(r):.0f}°/s' for r in flix.rates])
 print('Attitude:', *[f'{math.degrees(a):.0f}°' for a in flix.attitude_euler])
 print('Motors:', flix.motors)
@@ -23,11 +24,11 @@ print('> imu')
 print(flix.cli('imu'))
 
 print('=== Get parameter...')
-pitch_p = flix.get_param('PITCH_P')
+pitch_p = flix.get_param('CTL_P_P')
-print('PITCH_P = ', pitch_p)
+print('CTL_P_P = ', pitch_p)
 
 print('=== Set parameter...')
-flix.set_param('PITCH_P', pitch_p)
+flix.set_param('CTL_P_P', pitch_p)
 
 print('=== Wait for gyro update...')
 print('Gyro: ', flix.wait('gyro'))

tools/pyflix/README.md

@@ -24,19 +24,20 @@ pip install pyflix
 The API is accessed through the `Flix` class:
 
 ```python
-from flix import Flix
+from pyflix import Flix
 flix = Flix()  # create a Flix object and wait for connection
 ```
 
 ### Telemetry
 
-Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware itself:
+Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware code:
 
 ```python
 print(flix.connected)       # True if connected to the drone
 print(flix.mode)            # current flight mode (str)
 print(flix.armed)           # True if the drone is armed
 print(flix.landed)          # True if the drone is landed
+print(flix.voltage)         # battery voltage
 print(flix.attitude)        # attitude quaternion [w, x, y, z]
 print(flix.attitude_euler)  # attitude as Euler angles [roll, pitch, yaw]
 print(flix.rates)           # angular rates [roll_rate, pitch_rate, yaw_rate]
@@ -95,6 +96,7 @@ Full list of events:
 |`armed`|Armed state update|Armed state *(bool)*|
 |`mode`|Flight mode update|Flight mode *(str)*|
 |`landed`|Landed state update|Landed state *(bool)*|
+|`voltage`|Battery voltage update|Voltage *(float)*|
 |`print`|The drone prints text to the console|Text|
 |`attitude`|Attitude update|Attitude quaternion *(list)*|
 |`attitude_euler`|Attitude update|Euler angles *(list)*|
@@ -117,8 +119,8 @@ Full list of events:
 Get and set firmware parameters using `get_param` and `set_param` methods:
 
 ```python
-pitch_p = flix.get_param('PITCH_P')  # get parameter value
+pitch_p = flix.get_param('CTL_P_P')  # get parameter value
-flix.set_param('PITCH_P', 5)         # set parameter value
+flix.set_param('CTL_P_P', 5)         # set parameter value
 ```
 
 Execute console commands using `cli` method. This method returns the command response:

tools/pyflix/flix.py

@@ -26,6 +26,7 @@ class Flix:
     mode: str = ''
     armed: bool = False
     landed: bool = False
+    voltage: float = 0
     attitude: List[float]
     attitude_euler: List[float]  # roll, pitch, yaw
     rates: List[float]
@@ -68,7 +69,7 @@ class Flix:
         self._heartbeat_thread.start()
         if wait_connection:
             self.wait('mavlink.HEARTBEAT')
-            time.sleep(0.2) # give some time to receive initial state
+            time.sleep(0.6) # give some time to receive initial state
 
     def _init_state(self):
         self.attitude = [1, 0, 0, 0]
@@ -138,7 +139,7 @@ class Flix:
         while True:
             try:
                 msg: Optional[mavlink.MAVLink_message] = self.connection.recv_match(blocking=True)
-                if msg is None:
+                if msg is None or msg.get_srcSystem() != self.system_id:
                     continue
                 self._connected()
                 msg_dict = msg.to_dict()
@@ -185,11 +186,16 @@ class Flix:
             self._trigger('motors', self.motors)
 
         if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
-            self.acc = self._mavlink_to_flu([msg.xacc / 1000, msg.yacc / 1000, msg.zacc / 1000])
+            ONE_G = 9.80665
+            self.acc = self._mavlink_to_flu([msg.xacc * ONE_G / 1000, msg.yacc * ONE_G / 1000, msg.zacc * ONE_G / 1000])
             self.gyro = self._mavlink_to_flu([msg.xgyro / 1000, msg.ygyro / 1000, msg.zgyro / 1000])
             self._trigger('acc', self.acc)
             self._trigger('gyro', self.gyro)
 
+        if isinstance(msg, mavlink.MAVLink_battery_status_message):
+            self.voltage = msg.voltages[0] / 1000
+            self._trigger('voltage', self.voltage)
+
         if isinstance(msg, mavlink.MAVLink_serial_control_message):
             # new chunk of data
             text = bytes(msg.data)[:msg.count].decode('utf-8', errors='ignore')

tools/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "pyflix"
-version = "0.11"
+version = "0.14"
 description = "Python API for Flix drone"
 authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
 license = "MIT"