Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : 
      17             : #include "AP_BattMonitor_config.h"
      18             : 
      19             : #if AP_BATTERY_TORQEEDO_ENABLED
      20             : 
      21             : #include "AP_BattMonitor_Torqeedo.h"
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : 
      25             : #define AP_BATTMON_TORQEEDO_TIMEOUT_US 5000000
      26             : 
      27             : extern const AP_HAL::HAL &hal;
      28             : 
      29           0 : void AP_BattMonitor_Torqeedo::read(void)
      30             : {
      31           0 :     AP_Torqeedo *torqeedo = AP_Torqeedo::get_singleton();
      32           0 :     if (torqeedo == nullptr) {
      33           0 :         _state.healthy = false;
      34           0 :         return;
      35             :     }
      36             : 
      37             :     // get voltage, current, temp and remaining capacity percentage
      38             :     // assumes battery monitor instance matches torqeedo instance
      39             :     float volts;
      40             :     float current_amps;
      41             :     float temp_C;
      42           0 :     if (torqeedo->get_batt_info(_state.instance, volts, current_amps, temp_C, remaining_pct)) {
      43           0 :         have_info = true;
      44           0 :         _state.voltage = volts;
      45           0 :         _state.current_amps = current_amps;
      46           0 :         _state.temperature = temp_C;
      47           0 :         _state.temperature_time = AP_HAL::millis();
      48             : 
      49             :         // update total current draw
      50           0 :         const uint32_t tnow_us = AP_HAL::micros();
      51           0 :         const uint32_t diff_us = tnow_us - _state.last_time_micros;
      52           0 :         if (diff_us < AP_BATTMON_TORQEEDO_TIMEOUT_US) {
      53           0 :             _state.consumed_mah += _state.current_amps * diff_us * 1e-6f / 3600.0 * 1000.0;
      54             :         }
      55           0 :         _state.last_time_micros = tnow_us;
      56           0 :         _state.healthy = true;
      57             :     }
      58             : 
      59             :     // read battery pack capacity
      60             :     // assumes battery monitor instance matches torqeedo instance
      61           0 :     if (!have_capacity) {
      62             :         uint16_t batt_capacity_ah;
      63           0 :         if (torqeedo->get_batt_capacity_Ah(_state.instance, batt_capacity_ah)) {
      64           0 :             have_capacity = true;
      65           0 :             if (batt_capacity_ah * 1000 != _params._pack_capacity) {
      66           0 :                 _params._pack_capacity.set_and_notify(batt_capacity_ah * 1000);
      67             :             }
      68             :         }
      69             :     }
      70             : }
      71             : 
      72             : // capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument
      73           0 : bool AP_BattMonitor_Torqeedo::capacity_remaining_pct(uint8_t &percentage) const
      74             : {
      75           0 :     if (have_info) {
      76           0 :         percentage = remaining_pct;
      77             :     }
      78           0 :     return have_info;
      79             : }
      80             : 
      81             : #endif // AP_BATTERY_TORQEEDO_ENABLED