Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_ANALOG_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_BoardConfig/AP_BoardConfig.h>
       9             : 
      10             : #include "AP_BattMonitor_Analog.h"
      11             : 
      12             : extern const AP_HAL::HAL& hal;
      13             : 
      14             : // default pins and dividers
      15             : #if defined(HAL_BATT_CURR_PIN)
      16             :  // e.g. pins defined in hwdef.dat
      17             :  #ifndef HAL_BATT_CURR_SCALE
      18             :  #error "You must specify HAL_BATT_CURR_SCALE when you specify HAL_BATT_CURR_PIN"
      19             :  #endif
      20             :  # define AP_BATT_CURR_PIN                  HAL_BATT_CURR_PIN
      21             :  # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT  HAL_BATT_CURR_SCALE
      22             : #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
      23             :  # define AP_BATT_CURR_PIN                  3
      24             :  # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT  17.0f
      25             : #else
      26             :  # define AP_BATT_CURR_PIN                  -1
      27             :  # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT  17.0f
      28             : #endif
      29             : 
      30             : #if defined(HAL_BATT_VOLT_PIN)
      31             :  // e.g. pins defined in hwdef.dat
      32             :  #ifndef HAL_BATT_VOLT_SCALE
      33             :  #error "You must specify HAL_BATT_VOLT_SCALE when you specify HAL_BATT_VOLT_PIN"
      34             :  #endif
      35             :  # define AP_BATT_VOLT_PIN                  HAL_BATT_VOLT_PIN
      36             :  # define AP_BATT_VOLTDIVIDER_DEFAULT       HAL_BATT_VOLT_SCALE
      37             : #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
      38             :  # define AP_BATT_VOLT_PIN                  4
      39             :  # define AP_BATT_VOLTDIVIDER_DEFAULT       10.1f
      40             : #else
      41             :  # define AP_BATT_VOLT_PIN                  -1
      42             :  # define AP_BATT_VOLTDIVIDER_DEFAULT       10.1f
      43             : #endif
      44             : 
      45             : // This is 0 for the majority of the power modules.
      46             : #ifndef AP_BATT_CURR_AMP_OFFSET_DEFAULT
      47             :  #define AP_BATT_CURR_AMP_OFFSET_DEFAULT 0.0f
      48             : #endif
      49             : 
      50             : // Other values normally set directly by mission planner
      51             : // # define AP_BATT_VOLTDIVIDER_DEFAULT 15.70   // Volt divider for AttoPilot 50V/90A sensor
      52             : // # define AP_BATT_VOLTDIVIDER_DEFAULT 4.127   // Volt divider for AttoPilot 13.6V/45A sensor
      53             : // # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 27.32  // Amp/Volt for AttoPilot 50V/90A sensor
      54             : // # define AP_BATT_CURR_AMP_PERVOLT_DEFAULT 13.66  // Amp/Volt for AttoPilot 13.6V/45A sensor
      55             : 
      56             : const AP_Param::GroupInfo AP_BattMonitor_Analog::var_info[] = {
      57             : 
      58             :     // @Param: VOLT_PIN
      59             :     // @DisplayName: Battery Voltage sensing pin
      60             :     // @Description: Sets the analog input pin that should be used for voltage monitoring.
      61             :     // @Values: -1:Disabled, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 5:Navigator, 13:Pixhawk2_PM2/CubeOrange_PM2, 14:CubeOrange, 16:Durandal, 100:PX4-v1
      62             :     // @Range: -1 127
      63             :     // @User: Standard
      64             :     // @RebootRequired: True
      65             :     AP_GROUPINFO("VOLT_PIN", 1, AP_BattMonitor_Analog, _volt_pin, AP_BATT_VOLT_PIN),
      66             : 
      67             :     // @Param: CURR_PIN
      68             :     // @DisplayName: Battery Current sensing pin
      69             :     // @Description: Sets the analog input pin that should be used for current monitoring.
      70             :     // @Values: -1:Disabled, 3:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 4:CubeOrange_PM2/Navigator, 14:Pixhawk2_PM2, 15:CubeOrange, 17:Durandal, 101:PX4-v1
      71             :     // @Range: -1 127
      72             :     // @User: Standard
      73             :     // @RebootRequired: True
      74             :     AP_GROUPINFO("CURR_PIN", 2, AP_BattMonitor_Analog, _curr_pin, AP_BATT_CURR_PIN),
      75             : 
      76             :     // @Param: VOLT_MULT
      77             :     // @DisplayName: Voltage Multiplier
      78             :     // @Description: Used to convert the voltage of the voltage sensing pin (@PREFIX@VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick with a Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX using the PX4IO power supply this should be set to 1.
      79             :     // @User: Advanced
      80             :     AP_GROUPINFO("VOLT_MULT", 3, AP_BattMonitor_Analog, _volt_multiplier, AP_BATT_VOLTDIVIDER_DEFAULT),
      81             : 
      82             :     // @Param: AMP_PERVLT
      83             :     // @DisplayName: Amps per volt
      84             :     // @Description: Number of amps that a 1V reading on the current sensor corresponds to. With a Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. For Synthetic Current sensor monitors, this is the maximum, full throttle current draw.
      85             :     // @Units: A/V
      86             :     // @User: Standard
      87             :     AP_GROUPINFO("AMP_PERVLT", 4, AP_BattMonitor_Analog, _curr_amp_per_volt, AP_BATT_CURR_AMP_PERVOLT_DEFAULT),
      88             : 
      89             :     // @Param: AMP_OFFSET
      90             :     // @DisplayName: AMP offset
      91             :     // @Description: Voltage offset at zero current on current sensor for Analog Sensors. For Synthetic Current sensor, this offset is the zero throttle system current and is added to the calculated throttle base current.
      92             :     // @Units: V
      93             :     // @User: Standard
      94             :     AP_GROUPINFO("AMP_OFFSET", 5, AP_BattMonitor_Analog, _curr_amp_offset, AP_BATT_CURR_AMP_OFFSET_DEFAULT),
      95             : 
      96             :     // @Param: VLT_OFFSET
      97             :     // @DisplayName: Voltage offset
      98             :     // @Description: Voltage offset on voltage pin. This allows for an offset due to a diode. This voltage is subtracted before the scaling is applied.
      99             :     // @Units: V
     100             :     // @User: Advanced
     101             :     AP_GROUPINFO("VLT_OFFSET", 6, AP_BattMonitor_Analog, _volt_offset, 0),
     102             :     
     103             :     // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
     104             : 
     105             :     AP_GROUPEND
     106             : };
     107             : 
     108             : /// Constructor
     109        1859 : AP_BattMonitor_Analog::AP_BattMonitor_Analog(AP_BattMonitor &mon,
     110             :                                              AP_BattMonitor::BattMonitor_State &mon_state,
     111        1859 :                                              AP_BattMonitor_Params &params) :
     112        1859 :     AP_BattMonitor_Backend(mon, mon_state, params)
     113             : {
     114        1859 :     AP_Param::setup_object_defaults(this, var_info);
     115             : 
     116             :     // no other good way of setting these defaults
     117             : #if AP_BATT_MONITOR_MAX_INSTANCES > 1
     118        1859 :     if (mon_state.instance == 1) {
     119             : #ifdef HAL_BATT2_VOLT_PIN
     120             :         _volt_pin.set_default(HAL_BATT2_VOLT_PIN);
     121             : #endif
     122             : #ifdef HAL_BATT2_CURR_PIN
     123             :         _curr_pin.set_default(HAL_BATT2_CURR_PIN);
     124             : #endif
     125             : #ifdef HAL_BATT2_VOLT_SCALE
     126             :         _volt_multiplier.set_default(HAL_BATT2_VOLT_SCALE);
     127             : #endif
     128             : #ifdef HAL_BATT2_CURR_SCALE
     129             :         _curr_amp_per_volt.set_default(HAL_BATT2_CURR_SCALE);
     130             : #endif
     131             :     }
     132             : #endif
     133        1859 :     _state.var_info = var_info;
     134             :     
     135        1859 :     if (_params._type != AP_BattMonitor::Type::ANALOG_CURRENT_ONLY) {
     136        1859 :         _volt_pin_analog_source = hal.analogin->channel(_volt_pin);
     137        1859 :         if (_volt_pin_analog_source == nullptr) {
     138           0 :             AP_BoardConfig::config_error("No analog voltage channel for battery %d", mon_state.instance);
     139             :         }
     140             :     }
     141        1859 :     if (_params._type == AP_BattMonitor::Type::ANALOG_VOLTAGE_AND_CURRENT ||
     142           0 :         _params._type == AP_BattMonitor::Type::ANALOG_CURRENT_ONLY) {
     143        1859 :         _curr_pin_analog_source = hal.analogin->channel(_curr_pin);
     144        1859 :         if (_curr_pin_analog_source == nullptr) {
     145           0 :             AP_BoardConfig::config_error("No analog current channel for battery %d", mon_state.instance);
     146             :         }
     147             :     }
     148             : 
     149        1859 : }
     150             : 
     151             : // read - read the voltage and current
     152             : void
     153     1237084 : AP_BattMonitor_Analog::read()
     154             : {
     155     1237084 :     if (_state.type != AP_BattMonitor::Type::ANALOG_CURRENT_ONLY) {
     156             :         // this copes with changing the pin at runtime
     157     1237084 :         _state.healthy = _volt_pin_analog_source->set_pin(_volt_pin);
     158             : 
     159             :         // get voltage
     160     1237084 :         _state.voltage = (_volt_pin_analog_source->voltage_average() - _volt_offset) * _volt_multiplier;
     161             :     } else {
     162           0 :         _state.healthy = 1;
     163           0 :         _state.voltage = 0.0f;
     164             :     }
     165             : 
     166             :     // read current
     167     1237084 :     if (has_current()) {
     168             :         // calculate time since last current read
     169     1237084 :         const uint32_t tnow = AP_HAL::micros();
     170     1237084 :         const uint32_t dt_us = tnow - _state.last_time_micros;
     171             : 
     172             :         // this copes with changing the pin at runtime
     173     1237084 :         _state.healthy &= _curr_pin_analog_source->set_pin(_curr_pin);
     174             : 
     175             :         // read current
     176     1237084 :         _state.current_amps = (_curr_pin_analog_source->voltage_average() - _curr_amp_offset) * _curr_amp_per_volt;
     177             : 
     178     1237084 :         update_consumed(_state, dt_us);
     179             : 
     180             :         // record time
     181     1237084 :         _state.last_time_micros = tnow;
     182             :     }
     183     1237084 : }
     184             : 
     185             : /// return true if battery provides current info
     186    12034025 : bool AP_BattMonitor_Analog::has_current() const
     187             : {
     188    24068050 :     return (_curr_pin_analog_source != nullptr) &&
     189    12034025 :         (_state.type == AP_BattMonitor::Type::ANALOG_VOLTAGE_AND_CURRENT ||
     190    12034025 :          _state.type == AP_BattMonitor::Type::ANALOG_CURRENT_ONLY);
     191             : }
     192             : 
     193             : #endif  // AP_BATTERY_ANALOG_ENABLED