diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp index 6e341dd035692..c6e1a26c5766b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "AP_BattMonitor_Analog.h" @@ -88,7 +89,15 @@ AP_BattMonitor_Analog::AP_BattMonitor_Analog(AP_BattMonitor &mon, _state.var_info = var_info; _volt_pin_analog_source = hal.analogin->channel(_volt_pin); - _curr_pin_analog_source = hal.analogin->channel(_curr_pin); + if (_volt_pin_analog_source == nullptr) { + AP_BoardConfig::config_error("No analog channels for battery %d", mon_state.instance); + } + if ((AP_BattMonitor::Type)_params._type.get() == AP_BattMonitor::Type::ANALOG_VOLTAGE_AND_CURRENT) { + _curr_pin_analog_source = hal.analogin->channel(_curr_pin); + if (_curr_pin_analog_source == nullptr) { + AP_BoardConfig::config_error("No analog channels for battery %d", mon_state.instance); + } + } } @@ -124,7 +133,8 @@ AP_BattMonitor_Analog::read() /// return true if battery provides current info bool AP_BattMonitor_Analog::has_current() const { - return ((AP_BattMonitor::Type)_params._type.get() == AP_BattMonitor::Type::ANALOG_VOLTAGE_AND_CURRENT); + return (_curr_pin_analog_source != nullptr) && + ((AP_BattMonitor::Type)_params._type.get() == AP_BattMonitor::Type::ANALOG_VOLTAGE_AND_CURRENT); } #endif // AP_BATTERY_ANALOG_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp index 43d2566e4396e..c24fee29e0bb3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp @@ -59,9 +59,6 @@ AP_BattMonitor_Synthetic_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 ;