diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 45f630ce3b..35819b8d2e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -21,6 +21,7 @@ #include "AP_BattMonitor_FuelLevel_Analog.h" #include "AP_BattMonitor_Synthetic_Current.h" #include "AP_BattMonitor_AD7091R5.h" +#include "AP_BattMonitor_Scripting.h" #include @@ -560,6 +561,11 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]); break; #endif// AP_BATTERY_AD7091R5_ENABLED +#if AP_BATTERY_SCRIPTING_ENABLED + case Type::Scripting: + drivers[instance] = new AP_BattMonitor_Scripting(*this, state[instance], _params[instance]); + break; +#endif // AP_BATTERY_SCRIPTING_ENABLED case Type::NONE: default: break; @@ -1081,6 +1087,19 @@ bool AP_BattMonitor::healthy() const return true; } +#if AP_BATTERY_SCRIPTING_ENABLED +/* + handle state update from a lua script + */ +bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State &_state) +{ + if (idx >= _num_instances) { + return false; + } + return drivers[idx]->handle_scripting(_state); +} +#endif + namespace AP { AP_BattMonitor &battery() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index dbeed2e470..2df3fd5210 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -45,6 +45,7 @@ class AP_BattMonitor_LTC2946; class AP_BattMonitor_Torqeedo; class AP_BattMonitor_FuelLevel_Analog; class AP_BattMonitor_EFI; +class AP_BattMonitor_Scripting; class AP_BattMonitor @@ -70,6 +71,7 @@ class AP_BattMonitor friend class AP_BattMonitor_Torqeedo; friend class AP_BattMonitor_FuelLevel_Analog; friend class AP_BattMonitor_Synthetic_Current; + friend class AP_BattMonitor_Scripting; public: @@ -109,6 +111,7 @@ public: INA239_SPI = 26, EFI = 27, AD7091R5 = 28, + Scripting = 29, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); @@ -273,6 +276,10 @@ public: static const struct AP_Param::GroupInfo var_info[]; +#if AP_BATTERY_SCRIPTING_ENABLED + bool handle_scripting(uint8_t idx, const struct BattMonitorScript_State &state); +#endif + protected: /// parameters diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 85d6212770..8cc972e7e8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -63,7 +63,7 @@ public: float voltage_resting_estimate() const; // update battery resistance estimate and voltage_resting_estimate - void update_resistance_estimate(); + virtual void update_resistance_estimate(); // updates failsafe timers, and returns what failsafes are active virtual AP_BattMonitor::Failsafe update_failsafes(void); @@ -95,6 +95,10 @@ public: bool option_is_set(const AP_BattMonitor_Params::Options option) const { return (uint16_t(_params._options.get()) & uint16_t(option)) != 0; } + +#if AP_BATTERY_SCRIPTING_ENABLED + virtual bool handle_scripting(const BattMonitorScript_State &battmon_state) { return false; } +#endif protected: AP_BattMonitor &_mon; // reference to front-end @@ -114,3 +118,23 @@ private: float _resistance_voltage_ref; // voltage used for maximum resistance calculation float _resistance_current_ref; // current used for maximum resistance calculation }; + +#if AP_BATTERY_SCRIPTING_ENABLED +struct BattMonitorScript_State { + float voltage; // Battery voltage in volts + bool healthy; // True if communicating properly + uint8_t cell_count; // Number of valid cells in state + uint8_t capacity_remaining_pct=UINT8_MAX; // Remaining battery capacity in percent, 255 for invalid + uint16_t cell_voltages[32]; // allow script to have up to 32 cells, will be limited internally + uint16_t cycle_count=UINT16_MAX; // Battery cycle count, 65535 for unavailable + /* + all of the following float variables should be set to NaN by the + script if they are not known. + consumed_mah will auto-integrate if set to NaN + */ + float current_amps=nanf(""); // Battery current in amperes + float consumed_mah=nanf(""); // Total current drawn since start-up in milliampere hours + float consumed_wh=nanf(""); // Total energy drawn since start-up in watt hours + float temperature=nanf(""); // Battery temperature in degrees Celsius +}; +#endif // AP_BATTERY_SCRIPTING_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 5302bf7730..0eb859d2a4 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -17,7 +17,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: MONITOR // @DisplayName: Battery monitoring // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5 + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp new file mode 100644 index 0000000000..337c9d9b7a --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp @@ -0,0 +1,84 @@ +#include "AP_BattMonitor_Scripting.h" + +#if AP_BATTERY_SCRIPTING_ENABLED + +#define AP_BATT_MONITOR_SCRIPTING_TIMEOUT_US 5000000 + +bool AP_BattMonitor_Scripting::capacity_remaining_pct(uint8_t &percentage) const +{ + if (internal_state.capacity_remaining_pct != UINT8_MAX) { + percentage = internal_state.capacity_remaining_pct; + return true; + } + // Fall back to default implementation + return AP_BattMonitor_Backend::capacity_remaining_pct(percentage); +} + +bool AP_BattMonitor_Scripting::get_cycle_count(uint16_t &cycles) const +{ + if (internal_state.cycle_count == UINT16_MAX) { + return false; + } + cycles = internal_state.cycle_count; + return true; +} + +// Called by frontend to update the state. Called at 10Hz +void AP_BattMonitor_Scripting::read() +{ + WITH_SEMAPHORE(sem); + + // Check for timeout, to prevent a faulty script from appearing healthy + if (last_update_us == 0 || AP_HAL::micros() - last_update_us > AP_BATT_MONITOR_SCRIPTING_TIMEOUT_US) { + _state.healthy = false; + return; + } + + if (_state.last_time_micros == last_update_us) { + // No new data + return; + } + + /* + the script can fill in voltages up to 32 cells, for mavlink reporting + the extra cell voltages get distributed over the max of 14 for mavlink + */ + for (uint8_t i = 0; i < MIN(AP_BATT_MONITOR_CELLS_MAX,internal_state.cell_count); i++) { + _state.cell_voltages.cells[i] = internal_state.cell_voltages[i]; + } + _state.voltage = internal_state.voltage; + if (!isnan(internal_state.current_amps)) { + _state.current_amps = internal_state.current_amps; + } + if (!isnan(internal_state.consumed_mah)) { + _state.consumed_mah = internal_state.consumed_mah; + } + // Overide integrated consumed energy with script value if it has been set + if (!isnan(internal_state.consumed_wh)) { + _state.consumed_wh = internal_state.consumed_wh; + } + if (!isnan(internal_state.temperature)) { + _state.temperature = internal_state.temperature; + } + + _state.healthy = internal_state.healthy; + + // Update the timestamp (has to be done after the consumed_mah calculation) + _state.last_time_micros = last_update_us; +} + +bool AP_BattMonitor_Scripting::handle_scripting(const BattMonitorScript_State &battmon_state) +{ + WITH_SEMAPHORE(sem); + internal_state = battmon_state; + const uint32_t now_us = AP_HAL::micros(); + uint32_t dt_us = now_us - last_update_us; + if (last_update_us != 0 && !isnan(internal_state.current_amps) && isnan(internal_state.consumed_mah)) { + AP_BattMonitor_Backend::update_consumed(_state, dt_us); + } + last_update_us = now_us; + return true; +} + +#endif // AP_BATTERY_SCRIPTING_ENABLED + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h new file mode 100644 index 0000000000..f68f11b9dc --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h @@ -0,0 +1,32 @@ +#pragma once + +#include "AP_BattMonitor_Backend.h" + +#if AP_BATTERY_SCRIPTING_ENABLED + +class AP_BattMonitor_Scripting : public AP_BattMonitor_Backend +{ +public: + // Inherit constructor + using AP_BattMonitor_Backend::AP_BattMonitor_Backend; + + bool has_current() const override { return last_update_us != 0 && !isnan(internal_state.current_amps); } + bool has_consumed_energy() const override { return has_current(); } + bool has_cell_voltages() const override { return internal_state.cell_count > 0; } + bool has_temperature() const override { return last_update_us != 0 && !isnan(internal_state.temperature); } + bool capacity_remaining_pct(uint8_t &percentage) const override; + bool get_cycle_count(uint16_t &cycles) const override; + + void read() override; + + bool handle_scripting(const BattMonitorScript_State &battmon_state) override; + +protected: + BattMonitorScript_State internal_state; + uint32_t last_update_us; + + HAL_Semaphore sem; +}; + +#endif // AP_BATTMONITOR_SCRIPTING_ENABLED + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index 92d5ad074e..e15909c143 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_config.h @@ -116,3 +116,6 @@ #define AP_BATTERY_SMBUS_ROTOYE_ENABLED AP_BATTERY_SMBUS_GENERIC_ENABLED #endif +#ifndef AP_BATTERY_SCRIPTING_ENABLED +#define AP_BATTERY_SCRIPTING_ENABLED (AP_SCRIPTING_ENABLED && AP_BATTERY_BACKEND_DEFAULT_ENABLED) +#endif