5b961891ee
If you over allocate the number of analog channels this results in a crash. It's easy to trigger this if you have voltage only monitors as we still eat up a current input channel, regarless of if we use it. There are only 16 channels at this time on ChibiOS, so if you have 9 voltage only battery monitors you are out. This PR improves that situation by only allocating channels when needed, and in the case where we run out we now set a ConfigError, which on a flight controller is much more friendly then a instant segfault the moment we read a battery monitor. NOTE: on AP_Periph this takes the node off the bus, rather then just sitting in the bootloader. This was consideted acceptable as the current behaviour was to segfault and then sit in the bootloader, unless you made new firmware that limited the number of channels allocated it wasn't possible to recover in this situation anyways.
73 lines
2.4 KiB
C++
73 lines
2.4 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),
|
|
|
|
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
|
|
|
|
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;
|
|
|
|
// 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
|