Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_SUM_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_Math/AP_Math.h>
       8             : #include "AP_BattMonitor.h"
       9             : 
      10             : #include "AP_BattMonitor_Sum.h"
      11             : 
      12             : #if AP_BATT_MONITOR_MAX_INSTANCES < 2
      13             : #error "AP_BATT_MONITOR_MAX_INSTANCES must be at least three for AP_BATTERY_SUM_ENABLED"
      14             : #endif  // AP_BATT_MONITOR_MAX_INSTANCES < 2
      15             : 
      16             : /*
      17             :   battery monitor that is the sum of other battery monitors after this one
      18             : 
      19             :   This can be used to combined other current/voltage sensors into a
      20             :   single backend
      21             :  */
      22             : extern const AP_HAL::HAL& hal;
      23             : 
      24             : const AP_Param::GroupInfo AP_BattMonitor_Sum::var_info[] = {
      25             : 
      26             :     // @Param: SUM_MASK
      27             :     // @DisplayName: Battery Sum mask
      28             :     // @Description: 0: sum of remaining battery monitors, If none 0 sum of specified monitors. Current will be summed and voltages averaged.
      29             :     // @Bitmask: 0:monitor 1, 1:monitor 2, 2:monitor 3, 3:monitor 4, 4:monitor 5, 5:monitor 6, 6:monitor 7, 7:monitor 8, 8:monitor 9
      30             :     // @User: Standard
      31             :     AP_GROUPINFO("SUM_MASK", 20, AP_BattMonitor_Sum, _sum_mask, 0),
      32             : 
      33             :     // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
      34             : 
      35             :     AP_GROUPEND
      36             : };
      37             : 
      38             : /// Constructor
      39           0 : AP_BattMonitor_Sum::AP_BattMonitor_Sum(AP_BattMonitor &mon,
      40             :                                        AP_BattMonitor::BattMonitor_State &mon_state,
      41             :                                        AP_BattMonitor_Params &params,
      42           0 :                                        uint8_t instance) :
      43             :     AP_BattMonitor_Backend(mon, mon_state, params),
      44           0 :     _instance(instance)
      45             : {
      46           0 :     AP_Param::setup_object_defaults(this, var_info);
      47           0 :     _state.var_info = var_info;
      48           0 : }
      49             : 
      50             : // read - read the voltage and current
      51             : void
      52           0 : AP_BattMonitor_Sum::read()
      53             : {
      54           0 :     float voltage_sum = 0;
      55           0 :     float voltage_min = 0;
      56           0 :     uint8_t voltage_count = 0;
      57           0 :     float current_sum = 0;
      58           0 :     uint8_t current_count = 0;
      59             : 
      60           0 :     float temperature_sum = 0.0;
      61           0 :     uint8_t temperature_count = 0;
      62             : 
      63           0 :     float consumed_mah_sum = 0;
      64           0 :     float consumed_wh_sum = 0;
      65             : 
      66           0 :     for (uint8_t i=0; i<_mon.num_instances(); i++) {
      67           0 :         if (i == _instance) {
      68             :             // never include self
      69           0 :             continue;
      70             :         }
      71           0 :         if ((_sum_mask == 0) && (i <= _instance)) {
      72             :             // sum of remaining, skip lower instances
      73           0 :             continue;
      74             :         }
      75           0 :         if ((_sum_mask != 0) && ((_sum_mask & 1U<<i) == 0)) {
      76             :             // mask param, skip if mask bit not set
      77           0 :             continue;
      78             :         }
      79           0 :         if (!_mon.healthy(i)) {
      80           0 :             continue;
      81             :         }
      82           0 :         const float voltage = _mon.voltage(i);
      83           0 :         if (voltage_count == 0 || voltage < voltage_min) {
      84           0 :             voltage_min = voltage;
      85             :         }
      86           0 :         voltage_sum += voltage;
      87           0 :         voltage_count++;
      88             :         float current;
      89           0 :         if (_mon.current_amps(current, i)) {
      90           0 :             current_sum += current;
      91           0 :             current_count++;
      92             :         }
      93             : 
      94             :         float temperature;
      95           0 :         if (_mon.get_temperature(temperature, i)) {
      96           0 :             temperature_sum += temperature;
      97           0 :             temperature_count++;
      98             :         }
      99             : 
     100             :         float consumed_mah;
     101           0 :         if (_mon.consumed_mah(consumed_mah, i)) {
     102           0 :             consumed_mah_sum += consumed_mah;
     103             :         }
     104             : 
     105             :         float consumed_wh;
     106           0 :         if (_mon.consumed_wh(consumed_wh, i)) {
     107           0 :             consumed_wh_sum += consumed_wh;
     108             :         }
     109             :     }
     110           0 :     const uint32_t tnow_us = AP_HAL::micros();
     111             : 
     112           0 :     if (voltage_count > 0) {
     113           0 :         if (option_is_set(AP_BattMonitor_Params::Options::Minimum_Voltage)) {
     114           0 :             _state.voltage = voltage_min;
     115             :         } else {
     116           0 :             _state.voltage = voltage_sum / voltage_count;
     117             :         }
     118             :     }
     119           0 :     if (current_count > 0) {
     120           0 :         _state.current_amps = current_sum;
     121           0 :         _state.consumed_mah = consumed_mah_sum;
     122           0 :         _state.consumed_wh = consumed_wh_sum;
     123             :     }
     124           0 :     if (temperature_count > 0) {
     125           0 :         _state.temperature =  temperature_sum / temperature_count;
     126           0 :         _state.temperature_time = AP_HAL::millis();
     127             :     }
     128             : 
     129           0 :     _has_current = (current_count > 0);
     130           0 :     _has_temperature = (temperature_count > 0);
     131           0 :     _state.healthy = (voltage_count > 0);
     132             : 
     133           0 :     if (_state.healthy) {
     134           0 :         _state.last_time_micros = tnow_us;
     135             :     }
     136           0 : }
     137             : 
     138             : #endif  // AP_BATTERY_SUM_ENABLED