diff --git a/flix/mavlink.ino b/flix/mavlink.ino index feaef8d..0fae287 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -46,7 +46,7 @@ void sendMavlink() { 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 (valid(voltage)) sendMessage(&msg); } if (telemetryFast && mavlinkConnected) { diff --git a/flix/parameters.ino b/flix/parameters.ino index 75d0288..d0d88bd 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -117,7 +117,7 @@ Parameter parameters[] = { {"MAV_RATE_SLOW", &telemetrySlow.rate}, {"MAV_RATE_FAST", &telemetryFast.rate}, // power - {"PWR_VOLT_PIN", &voltagePin}, + {"PWR_VOLT_PIN", &voltagePin, setupPower}, {"PWR_VOLT_SCALE", &voltageScale}, {"PWR_VOLT_LPF_A", &voltageFilter.alpha}, // safety diff --git a/flix/power.ino b/flix/power.ino index b9e07f0..a6df104 100644 --- a/flix/power.ino +++ b/flix/power.ino @@ -8,14 +8,14 @@ #include "lpf.h" #include "util.h" -float voltage; +float voltage = NAN; LowPassFilter 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); + REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA); // disable reset on low voltage + if (digitalPinToAnalogChannel(voltagePin) == -1) voltagePin = -1; // test ADC pin } void readVoltage() { diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index 52c849b..6122225 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -168,6 +168,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; } +int8_t digitalPinToAnalogChannel(uint8_t pin) { return -1; } uint32_t analogReadMilliVolts(uint8_t pin) { return 0; } unsigned long __micros; diff --git a/gazebo/flix.h b/gazebo/flix.h index 83a956f..122bdb5 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -58,6 +58,7 @@ void handleMavlink(const void *_msg); void mavlinkPrint(const char* str); void sendMavlinkPrint(); inline Quaternion fluToFrd(const Quaternion &q); +void setupPower(); void failsafe(); void rcLossFailsafe(); void descend(); diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index cf3e9e5..ca2776c 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -73,6 +73,8 @@ public: gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z()); acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z())); + voltage = 4.2f; // dummy voltage value + readRC(); estimate(); diff --git a/tools/pyflix/README.md b/tools/pyflix/README.md index 702e39d..eff2ab1 100644 --- a/tools/pyflix/README.md +++ b/tools/pyflix/README.md @@ -37,7 +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.voltage) # battery voltage (NaN - unknown, ~0 - USB powered) 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] diff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.py index 211b5df..a6b44a3 100644 --- a/tools/pyflix/flix.py +++ b/tools/pyflix/flix.py @@ -5,6 +5,7 @@ import os import time +import math from queue import Queue, Empty from typing import Optional, Callable, List, Dict, Any, Union, Sequence import logging @@ -26,7 +27,7 @@ class Flix: mode: str = '' armed: bool = False landed: bool = False - voltage: float = 0 + voltage: float = math.nan attitude: List[float] attitude_euler: List[float] # roll, pitch, yaw rates: List[float] diff --git a/tools/pyproject.toml b/tools/pyproject.toml index 0ac51a0..7b6031c 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "pyflix" -version = "0.14" +version = "0.15" description = "Python API for Flix drone" authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }] license = "MIT"