76 lines
2.5 KiB
C++
76 lines
2.5 KiB
C++
#include "AP_BattMonitor_config.h"
|
|
|
|
#if AP_BATTERY_SYNTHETIC_CURRENT_ENABLED
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_BattMonitor_Synthetic_Current.h"
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include <SRV_Channel/SRV_Channel.h>
|
|
|
|
/*
|
|
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 // AP_BATTERY_SYNTHETIC_CURRENT_ENABLED
|