From eec66d845162a065c0a0d5adbe7d1d1b12e318f3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 4 Dec 2014 14:27:56 +0900 Subject: [PATCH] BattMon: parent class becomes frontend class --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 251 ++++++++++++++------ libraries/AP_BattMonitor/AP_BattMonitor.h | 208 +++++++--------- 2 files changed, 261 insertions(+), 198 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index b33270c9c8..fc4de5a817 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "AP_BattMonitor.h" +#include "AP_BattMonitor_Analog.h" +#include "AP_BattMonitor_SMBus.h" extern const AP_HAL::HAL& hal; @@ -9,41 +11,41 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] PROGMEM = { // @Description: Controls enabling monitoring of the battery's voltage and current // @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current // @User: Standard - AP_GROUPINFO("MONITOR", 0, AP_BattMonitor, _monitoring, AP_BATT_MONITOR_DISABLED), + AP_GROUPINFO("_MONITOR", 0, AP_BattMonitor, _monitoring[0], AP_BATT_MONITOR_DISABLED), // @Param: VOLT_PIN // @DisplayName: Battery Voltage sensing pin // @Description: Setting this to 0 ~ 13 will enable battery voltage sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 13. On the PX4 it should be set to 100. On the Pixhawk powered from the PM connector it should be set to 2. // @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk, 13:A13, 100:PX4 // @User: Standard - AP_GROUPINFO("VOLT_PIN", 1, AP_BattMonitor, _volt_pin, AP_BATT_VOLT_PIN), + AP_GROUPINFO("_VOLT_PIN", 1, AP_BattMonitor, _volt_pin[0], AP_BATT_VOLT_PIN), // @Param: CURR_PIN // @DisplayName: Battery Current sensing pin // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 12. On the PX4 it should be set to 101. On the Pixhawk powered from the PM connector it should be set to 3. // @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk, 12:A12, 101:PX4 // @User: Standard - AP_GROUPINFO("CURR_PIN", 2, AP_BattMonitor, _curr_pin, AP_BATT_CURR_PIN), + AP_GROUPINFO("_CURR_PIN", 2, AP_BattMonitor, _curr_pin[0], AP_BATT_CURR_PIN), // @Param: VOLT_MULT // @DisplayName: Voltage Multiplier // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX4 using the PX4IO power supply this should be set to 1. // @User: Advanced - AP_GROUPINFO("VOLT_MULT", 3, AP_BattMonitor, _volt_multiplier, AP_BATT_VOLTDIVIDER_DEFAULT), + AP_GROUPINFO("_VOLT_MULT", 3, AP_BattMonitor, _volt_multiplier[0], AP_BATT_VOLTDIVIDER_DEFAULT), // @Param: AMP_PERVOLT // @DisplayName: Amps per volt // @Description: Number of amps that a 1V reading on the current sensor corresponds to. On the APM2 or Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. // @Units: Amps/Volt // @User: Standard - AP_GROUPINFO("AMP_PERVOLT", 4, AP_BattMonitor, _curr_amp_per_volt, AP_BATT_CURR_AMP_PERVOLT_DEFAULT), + AP_GROUPINFO("_AMP_PERVOLT", 4, AP_BattMonitor, _curr_amp_per_volt[0], AP_BATT_CURR_AMP_PERVOLT_DEFAULT), // @Param: AMP_OFFSET // @DisplayName: AMP offset // @Description: Voltage offset at zero current on current sensor // @Units: Volts // @User: Standard - AP_GROUPINFO("AMP_OFFSET", 5, AP_BattMonitor, _curr_amp_offset, 0), + AP_GROUPINFO("_AMP_OFFSET", 5, AP_BattMonitor, _curr_amp_offset[0], 0), // @Param: CAPACITY // @DisplayName: Battery capacity @@ -51,19 +53,62 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] PROGMEM = { // @Units: mAh // @Increment: 50 // @User: Standard - AP_GROUPINFO("CAPACITY", 6, AP_BattMonitor, _pack_capacity, AP_BATT_CAPACITY_DEFAULT), + AP_GROUPINFO("_CAPACITY", 6, AP_BattMonitor, _pack_capacity[0], AP_BATT_CAPACITY_DEFAULT), - // @Param: VOLT2_PIN - // @DisplayName: 2nd Battery Voltage sensing pin - // @Description: This sets the pin for sensing the voltage on a 2nd battery. Set to -1 to disable sensing of a second battery + // 7 & 8 were used for VOLT2_PIN and VOLT2_MULT + // 9..10 left for future expansion + +#if AP_BATT_MONITOR_MAX_INSTANCES > 1 + // @Param: 2_MONITOR + // @DisplayName: Battery monitoring + // @Description: Controls enabling monitoring of the battery's voltage and current + // @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current // @User: Standard - AP_GROUPINFO("VOLT2_PIN", 7, AP_BattMonitor, _volt2_pin, -1), + AP_GROUPINFO("2_MONITOR", 11, AP_BattMonitor, _monitoring[1], AP_BATT_MONITOR_DISABLED), - // @Param: VOLT2_MULT - // @DisplayName: 2nd battery voltage multiplier - // @Description: Used to convert the voltage of the VOLT2_PIN to the actual battery's voltage (pin_voltage * VOLT_MULT). + // @Param: 2_VOLT_PIN + // @DisplayName: Battery Voltage sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery voltage sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 13. On the PX4 it should be set to 100. On the Pixhawk powered from the PM connector it should be set to 2. + // @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk, 13:A13, 100:PX4 + // @User: Standard + AP_GROUPINFO("2_VOLT_PIN", 12, AP_BattMonitor, _volt_pin[1], AP_BATT_VOLT_PIN), + + // @Param: 2_CURR_PIN + // @DisplayName: Battery Current sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 12. On the PX4 it should be set to 101. On the Pixhawk powered from the PM connector it should be set to 3. + // @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk, 12:A12, 101:PX4 + // @User: Standard + AP_GROUPINFO("2_CURR_PIN", 13, AP_BattMonitor, _curr_pin[1], AP_BATT_CURR_PIN), + + // @Param: 2_VOLT_MULT + // @DisplayName: Voltage Multiplier + // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX4 using the PX4IO power supply this should be set to 1. // @User: Advanced - AP_GROUPINFO("VOLT2_MULT", 8, AP_BattMonitor, _volt2_multiplier, 1), + AP_GROUPINFO("2_VOLT_MULT", 14, AP_BattMonitor, _volt_multiplier[1], AP_BATT_VOLTDIVIDER_DEFAULT), + + // @Param: 2_AMP_PERVOL + // @DisplayName: Amps per volt + // @Description: Number of amps that a 1V reading on the current sensor corresponds to. On the APM2 or Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. + // @Units: Amps/Volt + // @User: Standard + AP_GROUPINFO("2_AMP_PERVOL", 15, AP_BattMonitor, _curr_amp_per_volt[1], AP_BATT_CURR_AMP_PERVOLT_DEFAULT), + + // @Param: 2_AMP_OFFSET + // @DisplayName: AMP offset + // @Description: Voltage offset at zero current on current sensor + // @Units: Volts + // @User: Standard + AP_GROUPINFO("2_AMP_OFFSET", 16, AP_BattMonitor, _curr_amp_offset[1], 0), + + // @Param: 2_CAPACITY + // @DisplayName: Battery capacity + // @Description: Capacity of the battery in mAh when full + // @Units: mAh + // @Increment: 50 + // @User: Standard + AP_GROUPINFO("2_CAPACITY", 17, AP_BattMonitor, _pack_capacity[1], AP_BATT_CAPACITY_DEFAULT), + +#endif // AP_BATT_MONITOR_MAX_INSTANCES > 1 AP_GROUPEND }; @@ -73,72 +118,140 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] PROGMEM = { // their values. // AP_BattMonitor::AP_BattMonitor(void) : - _voltage(0), - _voltage2(0), - _current_amps(0), - _current_total_mah(0), - _last_time_micros(0) + _num_instances(0) { AP_Param::setup_object_defaults(this, var_info); } -// init - setup the battery and voltage pins +// init - instantiate the battery monitors void AP_BattMonitor::init() { - _volt_pin_analog_source = hal.analogin->channel(_volt_pin); - _curr_pin_analog_source = hal.analogin->channel(_curr_pin); - if (_volt2_pin != -1) { - _volt2_pin_analog_source = hal.analogin->channel(_volt2_pin); - } else { - _volt2_pin_analog_source = NULL; - } -} - -// read - read the voltage and current -void -AP_BattMonitor::read() -{ - if (_monitoring == AP_BATT_MONITOR_DISABLED) { + // check init has not been called before + if (_num_instances != 0) { return; } - // read voltage - if (_monitoring == AP_BATT_MONITOR_VOLTAGE_ONLY || _monitoring == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) { - // this copes with changing the pin at runtime - _volt_pin_analog_source->set_pin(_volt_pin); - _voltage = _volt_pin_analog_source->voltage_average() * _volt_multiplier; - if (_volt2_pin_analog_source != NULL) { - _voltage2 = _volt2_pin_analog_source->voltage_average() * _volt2_multiplier; + // create each instance + for (uint8_t instance=0; instanceread(); + } + } +} + +// healthy - returns true if monitor is functioning +bool AP_BattMonitor::healthy(uint8_t instance) const { + return instance < AP_BATT_MONITOR_MAX_INSTANCES && _BattMonitor_STATE(instance).healthy; +} + +/// has_current - returns true if battery monitor instance provides current info +bool AP_BattMonitor::has_current(uint8_t instance) const +{ + // check for analog voltage and current monitor or smbus monitor + if (instance < AP_BATT_MONITOR_MAX_INSTANCES) { + if (drivers[instance] != NULL) { + return (_monitoring[instance] == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT || _monitoring[instance] == AP_BATT_MONITOR_SMBUS); } } - // read current - if (_monitoring == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) { - uint32_t tnow = hal.scheduler->micros(); - float dt = tnow - _last_time_micros; - // this copes with changing the pin at runtime - _curr_pin_analog_source->set_pin(_curr_pin); - _current_amps = (_curr_pin_analog_source->voltage_average()-_curr_amp_offset)*_curr_amp_per_volt; - if (_last_time_micros != 0 && dt < 2000000.0f) { - // .0002778 is 1/3600 (conversion to hours) - _current_total_mah += _current_amps * dt * 0.0000002778f; + // not monitoring current + return false; +} + +/// voltage - returns battery voltage in volts +float AP_BattMonitor::voltage(uint8_t instance) const +{ + if (instance < AP_BATT_MONITOR_MAX_INSTANCES) { + return _BattMonitor_STATE(instance).voltage; + } else { + return 0.0f; + } +} + +// voltage2 - returns the voltage of the second battery (helper function to send 2nd voltage to GCS) +float AP_BattMonitor::voltage2() const +{ + // exit immediately if one or fewer monitors + if (_num_instances < 2) { + return 0.0f; + } + + // get next battery's voltage + for (uint8_t i=1; i= AP_BATT_MONITOR_MAX_INSTANCES || drivers[instance] == NULL) { + return 0; + } else { + return drivers[instance]->capacity_remaining_pct(); + } } /// exhausted - returns true if the voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity_mah -bool AP_BattMonitor::exhausted(float low_voltage, float min_capacity_mah) +bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_capacity_mah) { - // return immediately if disabled - if (_monitoring == AP_BATT_MONITOR_DISABLED) { + // exit immediately if no monitors setup + if (_num_instances == 0 || instance >= AP_BATT_MONITOR_MAX_INSTANCES || drivers[instance] == NULL) { return false; } @@ -146,33 +259,23 @@ bool AP_BattMonitor::exhausted(float low_voltage, float min_capacity_mah) uint32_t tnow = hal.scheduler->millis(); // check voltage - if ((_voltage != 0) && (low_voltage > 0) && (_voltage < low_voltage)) { + if ((state[instance].voltage > 0.0f) && (low_voltage > 0) && (state[instance].voltage < low_voltage)) { // this is the first time our voltage has dropped below minimum so start timer - if (_low_voltage_start_ms == 0) { - _low_voltage_start_ms = tnow; - }else if (tnow - _low_voltage_start_ms > AP_BATT_LOW_VOLT_TIMEOUT_MS) { + if (state[instance].low_voltage_start_ms == 0) { + state[instance].low_voltage_start_ms = tnow; + }else if (tnow - state[instance].low_voltage_start_ms > AP_BATT_LOW_VOLT_TIMEOUT_MS) { return true; } }else{ // acceptable voltage so reset timer - _low_voltage_start_ms = 0; + state[instance].low_voltage_start_ms = 0; } // check capacity if current monitoring is enabled - if ((_monitoring == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) && (min_capacity_mah>0) && (_pack_capacity - _current_total_mah < min_capacity_mah)) { + if (has_current(instance) && (min_capacity_mah>0) && (_pack_capacity[instance] - state[instance].current_total_mah < min_capacity_mah)) { return true; } // if we've gotten this far battery is ok return false; } - -/// 2nd Battery voltage, if available. return false otherwise -bool AP_BattMonitor::voltage2(float &v) const -{ - if (_volt2_pin_analog_source != NULL) { - v = _voltage2; - return true; - } - return false; -} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index df685941a7..b7571640c2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -2,165 +2,125 @@ #ifndef AP_BATTMONITOR_H #define AP_BATTMONITOR_H -#include #include #include -#include // ArduPilot Mega Vector/Matrix math Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include +#include -// battery monitor types +// maximum number of battery monitors +#define AP_BATT_MONITOR_MAX_INSTANCES 2 + +// first monitor is always the primary monitor +#define AP_BATT_PRIMARY_INSTANCE 0 + +// battery monitor #define AP_BATT_MONITOR_DISABLED 0 #define AP_BATT_MONITOR_VOLTAGE_ONLY 3 #define AP_BATT_MONITOR_VOLTAGE_AND_CURRENT 4 - -// setup default mag orientation for each board type -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 - # define AP_BATT_VOLT_PIN 0 // Battery voltage on A0 - # define AP_BATT_CURR_PIN 1 // Battery current on A1 - # define AP_BATT_VOLTDIVIDER_DEFAULT 3.56 // on-board APM1 voltage divider with a 3.9kOhm resistor - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 - # define AP_BATT_VOLT_PIN 13 // APM2.5/2.6 with 3dr power module - # define AP_BATT_CURR_PIN 12 - # define AP_BATT_VOLTDIVIDER_DEFAULT 10.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE -// Flymaple board pin 20 is connected to the external battery supply -// via a 24k/5.1k voltage divider. The schematic claims the divider is 25k/5k, -// but the actual installed resistors are not so. -// So the divider ratio is 5.70588 = (24000+5100)/5100 - # define AP_BATT_VOLT_PIN 20 - # define AP_BATT_CURR_PIN 19 - # define AP_BATT_VOLTDIVIDER_DEFAULT 5.70588 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 && defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - // px4 - # define AP_BATT_VOLT_PIN 100 - # define AP_BATT_CURR_PIN 101 - # define AP_BATT_VOLTDIVIDER_DEFAULT 1.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 && defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - // pixhawk - # define AP_BATT_VOLT_PIN 2 - # define AP_BATT_CURR_PIN 3 - # define AP_BATT_VOLTDIVIDER_DEFAULT 10.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL - # define AP_BATT_VOLT_PIN 13 - # define AP_BATT_CURR_PIN 12 - # define AP_BATT_VOLTDIVIDER_DEFAULT 10.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && defined(CONFIG_ARCH_BOARD_VRBRAIN_V40) - # define AP_BATT_VOLT_PIN 10 - # define AP_BATT_CURR_PIN -1 - # define AP_BATT_VOLTDIVIDER_DEFAULT 1.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) - # define AP_BATT_VOLT_PIN 10 - # define AP_BATT_CURR_PIN 11 - # define AP_BATT_VOLTDIVIDER_DEFAULT 1.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && defined(CONFIG_ARCH_BOARD_VRBRAIN_V50) - # define AP_BATT_VOLT_PIN 10 - # define AP_BATT_CURR_PIN 11 - # define AP_BATT_VOLTDIVIDER_DEFAULT 1.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) - # define AP_BATT_VOLT_PIN 10 - # define AP_BATT_CURR_PIN 11 - # define AP_BATT_VOLTDIVIDER_DEFAULT 1.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) - # define AP_BATT_VOLT_PIN 10 - # define AP_BATT_CURR_PIN -1 - # define AP_BATT_VOLTDIVIDER_DEFAULT 1.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && defined(CONFIG_ARCH_BOARD_VRHERO_V10) - # define AP_BATT_VOLT_PIN 100 - # define AP_BATT_CURR_PIN 101 - # define AP_BATT_VOLTDIVIDER_DEFAULT 1.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#else - # define AP_BATT_VOLT_PIN -1 - # define AP_BATT_CURR_PIN -1 - # define AP_BATT_VOLTDIVIDER_DEFAULT 10.1 - # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 17.0 -#endif - -// Other values normally set directly by mission planner -// # define AP_BATT_VOLTDIVIDER_DEFAULT 15.70 // Volt divider for AttoPilot 50V/90A sensor -// # define AP_BATT_VOLTDIVIDER_DEFAULT 4.127 // Volt divider for AttoPilot 13.6V/45A sensor -// # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 27.32 // Amp/Volt for AttoPilot 50V/90A sensor -// # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 13.66 // Amp/Volt for AttoPilot 13.6V/45A sensor +#define AP_BATT_MONITOR_SMBUS 5 #define AP_BATT_CAPACITY_DEFAULT 3300 #define AP_BATT_LOW_VOLT_TIMEOUT_MS 10000 // low voltage of 10 seconds will cause battery_exhausted to return true +// declare backend class +class AP_BattMonitor_Backend; +class AP_BattMonitor_Analog; +class AP_BattMonitor_SMBus; +class AP_BattMonitor_SMBus_I2C; +class AP_BattMonitor_SMBus_PX4; + class AP_BattMonitor { + friend class AP_BattMonitor_Backend; + friend class AP_BattMonitor_Analog; + friend class AP_BattMonitor_SMBus; + friend class AP_BattMonitor_SMBus_I2C; + friend class AP_BattMonitor_SMBus_PX4; + public: /// Constructor AP_BattMonitor(); - /// Initialize the battery monitor + // Battery monitor driver types + enum BattMonitor_Type { + BattMonitor_TYPE_NONE = 0, + BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY = 3, + BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4, + RangeFinder_TYPE_SMBUS = 5 + }; + + // The BattMonitor_State structure is filled in by the backend driver + struct BattMonitor_State { + uint8_t instance; // the instance number of this monitor + bool healthy; // battery monitor is communicating correctly + float voltage; // voltage in volts + float current_amps; // current in amperes + float current_total_mah; // total current draw since start-up + uint32_t last_time_micros; // time when voltage and current was last read + uint32_t low_voltage_start_ms; // time when voltage dropped below the minimum + }; + + // Return the number of battery monitor instances + uint8_t num_instances(void) const { return _num_instances; } + + // detect and initialise any available battery monitors void init(); - /// Read the battery voltage and current. Should be called at 10hz + /// Read the battery voltage and current for all batteries. Should be called at 10hz void read(); - /// monitoring - returns whether we are monitoring voltage only or voltage and current - int8_t monitoring() const { return _monitoring; } +#define _BattMonitor_STATE(instance) state[instance] - /// monitoring - returns whether we are monitoring voltage only or voltage and current - void set_monitoring(uint8_t mon) { _monitoring.set(mon); } + // healthy - returns true if monitor is functioning + bool healthy(uint8_t instance) const; + bool healthy() const { return healthy(AP_BATT_PRIMARY_INSTANCE); } - /// Battery voltage. Initialized to 0 - float voltage() const { return _voltage; } + /// has_current - returns true if battery monitor instance provides current info + bool has_current(uint8_t instance) const; + bool has_current() const { return has_current(AP_BATT_PRIMARY_INSTANCE); } - /// 2nd Battery voltage, if available. return false otherwise - bool voltage2(float &voltage) const; + /// voltage - returns battery voltage in millivolts + float voltage(uint8_t instance) const; + float voltage() const { return voltage(AP_BATT_PRIMARY_INSTANCE); } - /// Battery pack instantaneous currrent draw in amperes - float current_amps() const { return _current_amps; } + // voltage2 - returns the voltage of the second battery (helper function to send 2nd voltage to GCS) + float voltage2() const; - /// Total current drawn since start-up (Amp-hours) - float current_total_mah() const { return _current_total_mah; } + /// current_amps - returns the instantaneous current draw in amperes + float current_amps(uint8_t instance) const; + float current_amps() const { return current_amps(AP_BATT_PRIMARY_INSTANCE); } + + /// current_total_mah - returns total current drawn since start-up in amp-hours + float current_total_mah(uint8_t instance) const; + float current_total_mah() const { return current_total_mah(AP_BATT_PRIMARY_INSTANCE); } /// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100) - uint8_t capacity_remaining_pct() const; + virtual uint8_t capacity_remaining_pct(uint8_t instance) const; + uint8_t capacity_remaining_pct() const { return capacity_remaining_pct(AP_BATT_PRIMARY_INSTANCE); } - /// exhausted - returns true if the voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity - bool exhausted(float low_voltage, float min_capacity_mah); + /// exhausted - returns true if the battery's voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity + bool exhausted(uint8_t instance, float low_voltage, float min_capacity_mah); + bool exhausted(float low_voltage, float min_capacity_mah) { return exhausted(AP_BATT_PRIMARY_INSTANCE, low_voltage, min_capacity_mah); } + + /// monitoring - sets the monitor type (used for example sketch only) + void set_monitoring(uint8_t instance, uint8_t mon) { _monitoring[instance].set(mon); } static const struct AP_Param::GroupInfo var_info[]; protected: /// parameters - AP_Int8 _monitoring; /// 0=disabled, 3=voltage only, 4=voltage and current - AP_Int8 _volt_pin; /// board pin used to measure battery voltage - AP_Int8 _curr_pin; /// board pin used to measure battery current - AP_Float _volt_multiplier; /// voltage on volt pin multiplied by this to calculate battery voltage - AP_Float _curr_amp_per_volt; /// voltage on current pin multiplied by this to calculate current in amps - AP_Float _curr_amp_offset; /// offset voltage that is subtracted from current pin before conversion to amps - AP_Int32 _pack_capacity; /// battery pack capacity less reserve in mAh + AP_Int8 _monitoring[AP_BATT_MONITOR_MAX_INSTANCES]; /// 0=disabled, 3=voltage only, 4=voltage and current + AP_Int8 _volt_pin[AP_BATT_MONITOR_MAX_INSTANCES]; /// board pin used to measure battery voltage + AP_Int8 _curr_pin[AP_BATT_MONITOR_MAX_INSTANCES]; /// board pin used to measure battery current + AP_Float _volt_multiplier[AP_BATT_MONITOR_MAX_INSTANCES]; /// voltage on volt pin multiplied by this to calculate battery voltage + AP_Float _curr_amp_per_volt[AP_BATT_MONITOR_MAX_INSTANCES]; /// voltage on current pin multiplied by this to calculate current in amps + AP_Float _curr_amp_offset[AP_BATT_MONITOR_MAX_INSTANCES]; /// offset voltage that is subtracted from current pin before conversion to amps + AP_Int32 _pack_capacity[AP_BATT_MONITOR_MAX_INSTANCES]; /// battery pack capacity less reserve in mAh - // 2nd battery monitoring - AP_Int8 _volt2_pin; /// board pin used to measure 2nd battery voltage - AP_Float _volt2_multiplier; /// voltage on volt2 pin multiplier - - /// internal variables - float _voltage; /// last read voltage - float _voltage2; /// last read voltage 2nd battery - float _current_amps; /// last read current drawn - float _current_total_mah; /// total current drawn since startup (Amp-hours) - uint32_t _last_time_micros; /// time when current was last read - uint32_t _low_voltage_start_ms; /// time when voltage dropped below the minimum - - AP_HAL::AnalogSource *_volt_pin_analog_source; - AP_HAL::AnalogSource *_curr_pin_analog_source; - AP_HAL::AnalogSource *_volt2_pin_analog_source; +private: + BattMonitor_State state[AP_BATT_MONITOR_MAX_INSTANCES]; + AP_BattMonitor_Backend *drivers[AP_BATT_MONITOR_MAX_INSTANCES]; + uint8_t _num_instances; /// number of monitors }; #endif // AP_BATTMONITOR_H