Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_SYNTHETIC_CURRENT_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include "AP_BattMonitor_Synthetic_Current.h"
       7             : #include <GCS_MAVLink/GCS.h>
       8             : #include <SRV_Channel/SRV_Channel.h>
       9             : 
      10             : /*
      11             :   Analog Voltage and Current Monitor for systems with only a voltage sense pin
      12             :   Current is calculated from throttle output and modified for voltage droop using
      13             :   a square law calculation
      14             :  */
      15             : extern const AP_HAL::HAL& hal;
      16             : 
      17             : const AP_Param::GroupInfo AP_BattMonitor_Synthetic_Current::var_info[] = {
      18             : 
      19             :     // @Param: MAX_VOLT
      20             :     // @DisplayName: Maximum Battery Voltage
      21             :     // @Description: Maximum voltage of battery. Provides scaling of current versus voltage
      22             :     // @Range: 7 100
      23             :     // @User: Advanced
      24             : 
      25             :     AP_GROUPINFO("MAX_VOLT", 50, AP_BattMonitor_Synthetic_Current, _max_voltage, 12.6),
      26             :     
      27             :     // also inherit analog backend parameters
      28             :     AP_SUBGROUPEXTENSION("", 51, AP_BattMonitor_Synthetic_Current, AP_BattMonitor_Analog::var_info),
      29             : 
      30             :     // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
      31             : 
      32             :     AP_GROUPEND
      33             : };
      34             : 
      35             : /// Constructor
      36           0 : AP_BattMonitor_Synthetic_Current::AP_BattMonitor_Synthetic_Current(AP_BattMonitor &mon,
      37             :                                                  AP_BattMonitor::BattMonitor_State &mon_state,
      38           0 :                                                  AP_BattMonitor_Params &params) :
      39           0 :     AP_BattMonitor_Analog(mon, mon_state, params)
      40             : {
      41           0 :     AP_Param::setup_object_defaults(this, var_info);
      42           0 :     _state.var_info = var_info;
      43             :     
      44           0 :     _volt_pin_analog_source = hal.analogin->channel(_volt_pin);
      45           0 : }
      46             : 
      47             : // read - read the voltage and current
      48             : void
      49           0 : AP_BattMonitor_Synthetic_Current::read()
      50             : {
      51             :     // this copes with changing the pin at runtime
      52           0 :     _state.healthy = _volt_pin_analog_source->set_pin(_volt_pin);
      53             : 
      54             :     // get voltage
      55           0 :     _state.voltage = (_volt_pin_analog_source->voltage_average() - _volt_offset) * _volt_multiplier;
      56             : 
      57             :     // read current
      58             :     // calculate time since last current read
      59           0 :     const uint32_t tnow = AP_HAL::micros();
      60           0 :     const uint32_t dt_us = tnow - _state.last_time_micros;
      61             : 
      62             :     // read current
      63           0 :     _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 ;
      64             : 
      65           0 :     update_consumed(_state, dt_us);
      66             : 
      67             :     // record time
      68           0 :     _state.last_time_micros = tnow;
      69             :  
      70           0 : }
      71             : 
      72             : #endif  // AP_BATTERY_SYNTHETIC_CURRENT_ENABLED