AP_BattMonitor: add scripting backend

AP_BattMonitor_Scripting: whitespace consistency

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 <AP_HAL/AP_HAL.h>
 
@@ -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()

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

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

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),

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
+

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
+

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