From b235825f3db5d20a90dd70838183d906cc33ac58 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 21 Apr 2026 05:18:52 +0300 Subject: [PATCH] Implement battery voltage monitoring Add power subsystem. Add PWR_VOLT_PIN, PWR_VOLT_SCALE, PWR_VOLT_LPF_A parameters. Support BATTERY_STATUS mavlink messages streaming. Add pw cli command. Add voltage field to pyflix library. --- README.md | 2 ++ docs/usage.md | 9 +++++++++ flix/cli.ino | 4 ++++ flix/flix.ino | 1 + flix/mavlink.ino | 8 ++++++++ flix/parameters.ino | 7 +++++++ flix/power.ino | 21 +++++++++++++++++++++ gazebo/Arduino.h | 1 + gazebo/simulator.cpp | 1 + tools/example.py | 1 + tools/pyflix/README.md | 2 ++ tools/pyflix/flix.py | 7 ++++++- 12 files changed, 63 insertions(+), 1 deletion(-) create mode 100644 flix/power.ino diff --git a/README.md b/README.md index bf62098..d9907a7 100644 --- a/README.md +++ b/README.md @@ -156,6 +156,8 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir *¹ — 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). + ## Resources * Telegram channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter. diff --git a/docs/usage.md b/docs/usage.md index 5439b0f..01342a3 100644 --- a/docs/usage.md +++ b/docs/usage.md @@ -146,6 +146,15 @@ If using brushless motors and ESCs: > [!CAUTION] > **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them. +### Battery voltage monitoring + +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: diff --git a/flix/cli.ino b/flix/cli.ino index 3250d0e..a6dec83 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -16,6 +16,7 @@ extern float controlTime; extern int mode; extern bool armed; extern LowPassFilter 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 - setup Wi-Fi access point\n" "sta - 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") { diff --git a/flix/flix.ino b/flix/flix.ino index 422b818..e03b3e3 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -39,6 +39,7 @@ void loop() { sendMotors(); handleInput(); processMavlink(); + readVoltage(); logData(); syncParameters(); } diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 2df74d6..1459872 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -7,6 +7,7 @@ #include "util.h" extern float controlTime; +extern float voltage; int mavlinkSysId = 1; Rate telemetryFast(10); @@ -39,6 +40,13 @@ 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) { diff --git a/flix/parameters.ino b/flix/parameters.ino index cd4d779..9f81a1d 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -12,6 +12,9 @@ extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, extern int rcRxPin; extern int wifiMode, udpLocalPort, udpRemotePort; extern float rcLossTimeout, descendTime; +extern int voltagePin; +extern float voltageScale; +extern LowPassFilter voltageFilter; Preferences storage; @@ -109,6 +112,10 @@ 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}, diff --git a/flix/power.ino b/flix/power.ino new file mode 100644 index 0000000..d5d7304 --- /dev/null +++ b/flix/power.ino @@ -0,0 +1,21 @@ +// Copyright (c) 2026 Oleg Kalachev +// Repository: https://github.com/okalachev/flix + +// Power management + +#include "lpf.h" +#include "util.h" + +float voltage; +LowPassFilter voltageFilter(0.2); +int voltagePin = -1; +float voltageScale = 2; + +void readVoltage() { + if (voltagePin < 0) return; + static Rate rate(10); + if (!rate) return; + + float v = analogReadMilliVolts(voltagePin) * voltageScale / 1000.0f; + voltage = voltageFilter.update(v); +} diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index ad4bef4..2cbaaf0 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -166,6 +166,7 @@ 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; diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index b25e717..cf3e9e5 100644 --- a/gazebo/simulator.cpp +++ b/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" diff --git a/tools/example.py b/tools/example.py index e8bbdd1..0738bda 100755 --- a/tools/example.py +++ 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) diff --git a/tools/pyflix/README.md b/tools/pyflix/README.md index 6f90eb1..ade6d4a 100644 --- a/tools/pyflix/README.md +++ b/tools/pyflix/README.md @@ -37,6 +37,7 @@ 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)*| diff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.py index 757ebfa..fb88e55 100644 --- a/tools/pyflix/flix.py +++ b/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] @@ -190,6 +191,10 @@ class Flix: 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')