From 6b852e39c91a94d88374f843d9f915ebba91b320 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Tue, 22 Nov 2022 15:00:11 -0600 Subject: [PATCH] AP_BattMonitor: add synthetic current sensor,fix SITL current --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 6 ++ libraries/AP_BattMonitor/AP_BattMonitor.h | 51 +++++++------ .../AP_BattMonitor/AP_BattMonitor_Analog.cpp | 6 +- .../AP_BattMonitor_Synthetic_Current.cpp | 73 +++++++++++++++++++ .../AP_BattMonitor_Synthetic_Current.h | 31 ++++++++ 5 files changed, 142 insertions(+), 25 deletions(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 89381a35c2..cfe02d3b29 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -17,6 +17,7 @@ #include "AP_BattMonitor_LTC2946.h" #include "AP_BattMonitor_Torqeedo.h" #include "AP_BattMonitor_FuelLevel_Analog.h" +#include "AP_BattMonitor_Synthetic_Current.h" #include @@ -323,6 +324,11 @@ AP_BattMonitor::init() case Type::Torqeedo: drivers[instance] = new AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]); break; +#endif +#if AP_BATTMON_SYNTHETIC_CURRENT_ENABLED + case Type::Analog_Volt_Synthetic_Current: + drivers[instance] = new AP_BattMonitor_Synthetic_Current(*this, state[instance], _params[instance]); + break; #endif case Type::NONE: default: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 70d8e0b523..009e2efe11 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -44,6 +44,10 @@ #define AP_BATTMON_FUELLEVEL_ANALOG_ENABLE (BOARD_FLASH_SIZE > 1024) #endif +#ifndef AP_BATTMON_SYNTHETIC_CURRENT_ENABLED +#define AP_BATTMON_SYNTHETIC_CURRENT_ENABLED 1 +#endif + // declare backend class class AP_BattMonitor_Backend; class AP_BattMonitor_Analog; @@ -59,6 +63,7 @@ class AP_BattMonitor_LTC2946; class AP_BattMonitor_Torqeedo; class AP_BattMonitor_FuelLevel_Analog; + class AP_BattMonitor { friend class AP_BattMonitor_Backend; @@ -78,6 +83,7 @@ class AP_BattMonitor friend class AP_BattMonitor_Torqeedo; friend class AP_BattMonitor_FuelLevel_Analog; + friend class AP_BattMonitor_Synthetic_Current; public: @@ -90,29 +96,30 @@ public: // Battery monitor driver types enum class Type { - NONE = 0, - ANALOG_VOLTAGE_ONLY = 3, - ANALOG_VOLTAGE_AND_CURRENT = 4, - SOLO = 5, - BEBOP = 6, - SMBus_Generic = 7, - UAVCAN_BatteryInfo = 8, - BLHeliESC = 9, - Sum = 10, - FuelFlow = 11, - FuelLevel_PWM = 12, - SUI3 = 13, - SUI6 = 14, - NeoDesign = 15, - MAXELL = 16, - GENERATOR_ELEC = 17, - GENERATOR_FUEL = 18, - Rotoye = 19, + NONE = 0, + ANALOG_VOLTAGE_ONLY = 3, + ANALOG_VOLTAGE_AND_CURRENT = 4, + SOLO = 5, + BEBOP = 6, + SMBus_Generic = 7, + UAVCAN_BatteryInfo = 8, + BLHeliESC = 9, + Sum = 10, + FuelFlow = 11, + FuelLevel_PWM = 12, + SUI3 = 13, + SUI6 = 14, + NeoDesign = 15, + MAXELL = 16, + GENERATOR_ELEC = 17, + GENERATOR_FUEL = 18, + Rotoye = 19, // 20 was MPPT_PacketDigital - INA2XX = 21, - LTC2946 = 22, - Torqeedo = 23, - FuelLevel_Analog = 24, + INA2XX = 21, + LTC2946 = 22, + Torqeedo = 23, + FuelLevel_Analog = 24, + Analog_Volt_Synthetic_Current = 25, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp index b6a74557ae..9847804175 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp @@ -31,21 +31,21 @@ const AP_Param::GroupInfo AP_BattMonitor_Analog::var_info[] = { // @Param: AMP_PERVLT // @DisplayName: Amps per volt - // @Description: Number of amps that a 1V reading on the current sensor corresponds to. With a Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. + // @Description: Number of amps that a 1V reading on the current sensor corresponds to. With a Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. For Synthetic Current sensor monitors, this is the maximum, full throttle current draw. // @Units: A/V // @User: Standard AP_GROUPINFO("AMP_PERVLT", 4, AP_BattMonitor_Analog, _curr_amp_per_volt, AP_BATT_CURR_AMP_PERVOLT_DEFAULT), // @Param: AMP_OFFSET // @DisplayName: AMP offset - // @Description: Voltage offset at zero current on current sensor + // @Description: Voltage offset at zero current on current sensor for Analog Sensors. For Synthetic Current sensor, this offset is the zero throttle system current and is added to the calculated throttle base current. // @Units: V // @User: Standard AP_GROUPINFO("AMP_OFFSET", 5, AP_BattMonitor_Analog, _curr_amp_offset, AP_BATT_CURR_AMP_OFFSET_DEFAULT), // @Param: VLT_OFFSET // @DisplayName: Voltage offset - // @Description: Voltage offset on voltage pin. This allows for an offset due to a diode. This voltage is subtracted before the scaling is applied + // @Description: Voltage offset on voltage pin. This allows for an offset due to a diode. This voltage is subtracted before the scaling is applied. // @Units: V // @User: Advanced AP_GROUPINFO("VLT_OFFSET", 6, AP_BattMonitor_Analog, _volt_offset, 0), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp new file mode 100644 index 0000000000..bb772e08c7 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp @@ -0,0 +1,73 @@ +#include +#include "AP_BattMonitor_Synthetic_Current.h" +#include +#include + +#if AP_BATTMON_SYNTHETIC_CURRENT_ENABLED + +/* + Analog Voltage and Current Monitor for systems with only a voltage sense pin + Current is calculated from throttle output and modified for voltage droop using + a square law calculation + */ +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_BattMonitor_Synthetic_Current::var_info[] = { + + // @Param: MAX_VOLT + // @DisplayName: Maximum Battery Voltage + // @Description: Maximum voltage of battery. Provides scaling of current versus voltage + // @Range: 7 100 + // @User: Advanced + + AP_GROUPINFO("MAX_VOLT", 50, AP_BattMonitor_Synthetic_Current, _max_voltage, 12.6), + + // also inherit analog backend parameters + AP_SUBGROUPEXTENSION("", 51, AP_BattMonitor_Synthetic_Current, AP_BattMonitor_Analog::var_info), + + // Param indexes must be between 50 and 55 to avoid conflict with other battery monitor param tables loaded by pointer + + AP_GROUPEND +}; + +/// Constructor +AP_BattMonitor_Synthetic_Current::AP_BattMonitor_Synthetic_Current(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms) : + AP_BattMonitor_Analog(mon, mon_state, params) +{ + AP_Param::setup_object_defaults(this, var_info); + _state.var_info = var_info; + + _volt_pin_analog_source = hal.analogin->channel(_volt_pin); +} + +// read - read the voltage and current +void +AP_BattMonitor_Synthetic_Current::read() +{ + // this copes with changing the pin at runtime + _state.healthy = _volt_pin_analog_source->set_pin(_volt_pin); + + // get voltage + _state.voltage = (_volt_pin_analog_source->voltage_average() - _volt_offset) * _volt_multiplier; + + // read current + // calculate time since last current read + const uint32_t tnow = AP_HAL::micros(); + const uint32_t dt_us = tnow - _state.last_time_micros; + + // this copes with changing the pin at runtime + _state.healthy &= _curr_pin_analog_source->set_pin(_curr_pin); + + // read current + _state.current_amps = ((_state.voltage/_max_voltage)*sq(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) * 0.0001 * _curr_amp_per_volt) + _curr_amp_offset ; + + update_consumed(_state, dt_us); + + // record time + _state.last_time_micros = tnow; + +} + +#endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h new file mode 100644 index 0000000000..7b12ed58bf --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h @@ -0,0 +1,31 @@ +#pragma once + +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Analog.h" + +#if AP_BATTMON_SYNTHETIC_CURRENT_ENABLED +class AP_BattMonitor_Synthetic_Current : public AP_BattMonitor_Analog +{ +public: + + /// Constructor + AP_BattMonitor_Synthetic_Current(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms); + + /// Read the battery voltage and current. Should be called at 10hz + void read() override; + + /// returns true if battery monitor provides consumed energy info + bool has_consumed_energy() const override { return true; } + + /// returns true if battery monitor provides current info + bool has_current() const override { return true; } + + void init(void) override {} + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + AP_Float _max_voltage; /// maximum battery voltage used in current caluculation +}; +#endif