Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: add synthetic current sensor,fix SITL current

Browse Source
This commit is contained in:
Henry Wurzburg 2022-11-22 15:00:11 -06:00 committed by Andrew Tridgell
parent c1cd095508
commit 6b852e39c9
5 changed files with 142 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

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

51
libraries/AP_BattMonitor/AP_BattMonitor.h
View File

@ -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);

6
libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp
View File

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

73
libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp Normal file
View File

@ -0,0 +1,73 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_BattMonitor_Synthetic_Current.h"
#include <GCS_MAVLink/GCS.h>
#include <SRV_Channel/SRV_Channel.h>
#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 &params) :
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

31
libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h Normal file
View File

@ -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 &params);
/// 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