BattMonitor: initial draft lib

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -0,0 +1,115 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include "AP_BattMonitor.h"
+
+extern const AP_HAL::HAL& hal;
+
+// static methods
+AP_HAL::AnalogSource *AP_BattMonitor::_volt_pin_analog_source;
+AP_HAL::AnalogSource *AP_BattMonitor::_curr_pin_analog_source;
+
+const AP_Param::GroupInfo AP_BattMonitor::var_info[] PROGMEM = {
+    // @Param: BATT_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("MONITOR", 0, AP_BattMonitor, _monitoring, AP_BATT_MONITOR_DISABLED),
+
+    // @Param: BATT_VOLT_PIN
+    // @DisplayName: Battery Voltage 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 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),
+
+    // @Param: BATT_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),
+
+    // @Param: BATT_VOLTDIVIDER
+    // @DisplayName: Voltage Divider
+    // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_DIVIDER). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the PX4 using the PX4IO power supply this should be set to 1.
+    // @User: Advanced
+    AP_GROUPINFO("VOLTDIVIDER", 3, AP_BattMonitor, _volt_div_ratio, AP_BATT_VOLTDIVIDER_DEFAULT),
+
+    // @Param: BATT_APM_PERVOLT
+    // @DisplayName: Apms 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.
+    // @Units: A/V
+    // @User: Standard
+    AP_GROUPINFO("AMP_PERVOLT", 4, AP_BattMonitor, _curr_amp_per_volt, AP_BATT_CURR_AMP_PERVOLT_DEFAULT),
+
+    // @Param: BATT_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),
+
+    // @Param: BATT_CAPACITY
+    // @DisplayName: Battery capacity
+    // @Description: Capacity of the battery in mAh when full
+    // @Units: mAh
+    // @User: Standard
+    AP_GROUPINFO("CAPACITY", 6, AP_BattMonitor, _pack_capacity, AP_BATT_CAPACITY_DEFAULT),
+
+    AP_GROUPEND
+};
+
+// Default constructor.
+// Note that the Vector/Matrix constructors already implicitly zero
+// their values.
+//
+AP_BattMonitor::AP_BattMonitor(void) :
+    _voltage(AP_BATT_INITIAL_VOLTAGE)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// init - setup the battery and voltage pins
+void
+AP_BattMonitor::init()
+{
+    _volt_pin_analog_source = hal.analogin->channel(_volt_pin);
+    _curr_pin_analog_source = hal.analogin->channel(_curr_pin);
+}
+
+// read - read the voltage and current
+void
+AP_BattMonitor::read()
+{
+    if (_monitoring == AP_BATT_MONITOR_DISABLED) {
+        _voltage = 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_div_ratio;
+    }
+
+    // read current
+    if (_monitoring == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
+        uint32_t tnow = hal.scheduler->millis();
+        float dt = tnow - _last_time_ms;
+        // 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_ms != 0 && dt < 2000) {
+            // .0002778 is 1/3600 (conversion to hours)
+            _current_total_mah += _current_amps * dt * 0.0002778f;
+        }
+        _last_time_ms = tnow;
+    }
+}
+
+/// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100)
+uint8_t AP_BattMonitor::capacity_remaining_pct() const
+{
+    return (100.0 * (_pack_capacity - _current_total_mah) / _pack_capacity);
+}

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -0,0 +1,122 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#ifndef AP_BATTMONITOR_H
+#define AP_BATTMONITOR_H
+
+#include <inttypes.h>
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_Math.h>                // ArduPilot Mega Vector/Matrix math Library
+#include <AP_ADC.h>                 // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource.h>
+
+// battery monitor types
+#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.0
+ # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT   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  0
+#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
+ # define AP_BATT_VOLT_PIN                  2
+ # define AP_BATT_CURR_PIN                  3
+ # define AP_BATT_VOLTDIVIDER_DEFAULT       10.0
+ # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT  17
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+ # define AP_BATT_VOLT_PIN                  1
+ # define AP_BATT_CURR_PIN                  2
+ # define AP_BATT_VOLTDIVIDER_DEFAULT       10.0
+ # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT  0
+#else
+ # define AP_BATT_VOLT_PIN                  -1
+ # define AP_BATT_CURR_PIN                  -1
+ # define AP_BATT_VOLTDIVIDER_DEFAULT       10.0
+ # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT  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_CAPACITY_DEFAULT            1760
+
+#define AP_BATT_INITIAL_VOLTAGE 99              // initial voltage set on start-up to avoid low voltage alarms
+
+class AP_BattMonitor
+{
+public:
+
+    /// Constructor
+    AP_BattMonitor();
+
+    /// Initialize the battery monitor
+    void init();
+
+    /// Read the battery voltage and current.  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; }
+
+    /// pack_capacity - battery pack capacity in mAh less reserve
+    float pack_capacity() const { return _pack_capacity; }
+
+    /// Battery voltage.  Initialized to 99 to prevent low voltage events at startup
+    float voltage() const { return _voltage; }
+
+    /// Battery pack instantaneous currrent draw in amperes
+    float current_amps() const { return _current_amps; }
+
+    /// Total current drawn since start-up (Amp-hours)
+    float current_total_mah() const { return _current_total_mah; }
+
+    /// time when current was last read
+    uint32_t last_time_ms() const { return _last_time_ms; }
+
+    /// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100)
+    uint8_t capacity_remaining_pct() const;
+
+    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;
+    AP_Int8     _curr_pin;
+    AP_Float    _volt_div_ratio;
+    AP_Float    _curr_amp_per_volt;
+    AP_Float    _curr_amp_offset;
+    AP_Int32    _pack_capacity;              /// battery pack capacity less reserve
+
+    /// internal variables
+    float       _voltage;                   /// last read voltage
+    float       _current_amps;              /// last read current drawn
+    float       _current_total_mah;         /// total current drawn since startup (Amp-hours)
+    uint32_t    _last_time_ms;              /// time when current was last read
+
+    static AP_HAL::AnalogSource *_volt_pin_analog_source;
+    static AP_HAL::AnalogSource *_curr_pin_analog_source;
+
+};
+#endif  // AP_BATTMONITOR_H

libraries/AP_BattMonitor/examples/AP_BattMonitor_test/AP_BattMonitor_test.pde

@@ -0,0 +1,63 @@
+/*
+ *       Example of AP_BattMonitor library
+ *       Code by DIYDrones.com
+ */
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Param.h>
+#include <AP_Math.h>                // ArduPilot Mega Vector/Matrix math Library
+#include <AP_ADC.h>                 // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_PX4.h>
+#include <AP_HAL_Empty.h>
+#include <AP_BattMonitor.h>
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+AP_BattMonitor battery_mon;
+uint32_t timer;
+
+void setup() {
+    hal.console->println("Battery monitor library test");
+
+    // initialise the battery monitor
+    battery_mon.init();
+
+    hal.scheduler->delay(1000);
+    timer = hal.scheduler->millis();
+}
+
+void loop()
+{
+    static uint8_t counter; // counter to slow output to the user
+    uint32_t now = hal.scheduler->millis();
+
+    // call battery monitor at 10hz
+    if((now - timer) > 100) {
+        // update voltage and current readings
+        battery_mon.read();
+
+        // reset timer
+        timer = now;
+
+        // increment counter
+        counter++;
+    }
+
+    // display output at 1hz
+    if (counter >= 10) {
+        counter = 0;
+        hal.console->printf("\nVoltage: %.2f \tCurrent: %.2f \tTotCurr:%.2f",
+			    battery_mon.voltage(),
+			    battery_mon.current_amps(),
+                battery_mon.current_total_mah());
+    }
+
+    // delay 1ms
+    hal.scheduler->delay(1);
+}
+
+AP_HAL_MAIN();

libraries/AP_BattMonitor/examples/AP_BattMonitor_test/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk