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             : #include "AP_BattMonitor_config.h"
      17             : 
      18             : #if AP_BATTERY_GENERATOR_ENABLED
      19             : 
      20             : #include <AP_Common/AP_Common.h>
      21             : #include <AP_Math/AP_Math.h>
      22             : 
      23             : #include "AP_BattMonitor_Generator.h"
      24             : 
      25             : /*
      26             :     Fuel class
      27             : */
      28             : // This is where we tell the battery monitor 'we have current' if we want to report a fuel level remaining
      29       29276 : bool AP_BattMonitor_Generator_FuelLevel::has_current(void) const
      30             : {
      31             :     // If the generator has fuel remaining we must also state that we have current
      32       29276 :     return has_consumed_energy();
      33             : }
      34             : 
      35             : // This is where we tell the battery monitor 'we have consumed energy' if we want to report a fuel level remaining
      36       31057 : bool AP_BattMonitor_Generator_FuelLevel::has_consumed_energy(void) const
      37             : {
      38             :     // Get pointer to generator singleton
      39       31057 :     AP_Generator *generator = AP::generator();
      40             : 
      41       31057 :     if (generator == nullptr) {
      42           0 :         return false;
      43             :     }
      44             : 
      45             :     // Use consumed_mAh in BattMonitor to display fuel remaining
      46       31057 :     return generator->has_consumed_energy();
      47             : }
      48             : 
      49          10 : bool AP_BattMonitor_Generator_FuelLevel::reset_remaining(float percentage)
      50             : {
      51             :     // Get pointer to generator singleton
      52          10 :     AP_Generator *generator = AP::generator();
      53             : 
      54          10 :     if (generator == nullptr) {
      55           0 :         return false;
      56             :     }
      57             : 
      58          10 :     if (generator->reset_consumed_energy()) {
      59          10 :         percentage = constrain_float(percentage, 0.0, 100.0);
      60          10 :         _initial_fuel_pct = percentage;
      61          10 :         return true;
      62             :     }
      63             : 
      64           0 :     return false;
      65             : }
      66             : 
      67          14 : void AP_BattMonitor_Generator_FuelLevel::init()
      68             : {
      69          14 : }
      70             : 
      71             : // Read the fuel level.  Should be called at 10hz
      72       10744 : void AP_BattMonitor_Generator_FuelLevel::read()
      73             : {
      74       10744 :     _state.healthy = false;
      75             : 
      76             :     // Get pointer to generator singleton
      77       10744 :     AP_Generator *generator = AP::generator();
      78             : 
      79             :     // Not healthy if we can't find a generator
      80       10744 :     if (generator == nullptr) {
      81           0 :         return;
      82             :     }
      83             : 
      84       10744 :     if (!generator->healthy()) {
      85         144 :         return;
      86             :     }
      87             : 
      88       10600 :     if (generator->has_fuel_remaining()) {
      89             :         // Set params for users:
      90             :         // Fuel level is only reported as a percentage
      91        3218 :         _params._pack_capacity.set(100);
      92             :         // Fuel only reports a fixed 1v, don't want batt monitor failsafes on this instance
      93        3218 :         _params._low_voltage.set(0);
      94        3218 :         _params._critical_voltage.set(0);
      95             :     }
      96             : 
      97             :     // As this is a battery monitor instance report voltage
      98             :     // Report fixed voltage of 1V
      99       10600 :     _state.voltage = 1.0f;
     100             : 
     101             :     // This is a bodge to display tank level as a percentage on GCS.  Users should set _params.pack_capacity == 100 to get a clear percentage in GCS
     102       10600 :     if (generator->has_fuel_remaining()) {
     103        3218 :         _state.consumed_mah = (1 - generator->get_fuel_remaining()) * _params._pack_capacity.get();
     104             :     } else {
     105        7382 :         _state.consumed_mah = generator->get_batt_consumed();
     106             :     }
     107             : 
     108             :     // If we got this far then must be healthy
     109       10600 :     _state.healthy = true;
     110       10600 :     _state.last_time_micros = AP_HAL::micros();
     111             : }
     112             : 
     113             : // capacity_remaining_ml - returns true if the capacity remaining in (mL) is valid and writes to capacity_ml
     114       12188 : bool AP_BattMonitor_Generator_FuelLevel::capacity_remaining_ml(float &capacity_ml) const
     115             : {
     116             :         // we consider anything under 10 ml as being an invalid capacity and so will be our measurement of remaining capacity
     117       12188 :         if ( _params._pack_capacity <= 10) {
     118           0 :             return false;
     119             :         }
     120             : 
     121             :         // we get the initial fuel multiplying pack capacity parameter by initial fuel percentage.
     122             :         // this initial fuel percentage will be 100% by default, but we can change it using the
     123             :         // mavlink command MAV_CMD_BATTERY_RESET
     124       12188 :         const float initial_fuel = _params._pack_capacity * _initial_fuel_pct * 0.01;
     125             : 
     126             :         // to get the actual remaining level we need to substract the consumed ml
     127       12188 :         capacity_ml = initial_fuel - _state.consumed_mah;
     128             : 
     129       12188 :         return true;
     130             : }
     131             : 
     132       12491 : bool AP_BattMonitor_Generator_FuelLevel::capacity_remaining_pct(uint8_t &percentage) const
     133             : {
     134             :     // Get pointer to generator singleton
     135       12491 :     AP_Generator *generator = AP::generator();
     136             : 
     137             :     // If generator is not setup or not healthy, use the base class method
     138       12491 :     if (generator == nullptr) {
     139           0 :         return AP_BattMonitor_Backend::capacity_remaining_pct(percentage);
     140             :     }
     141             : 
     142       12491 :     if (!generator->healthy()) {
     143         170 :          return AP_BattMonitor_Backend::capacity_remaining_pct(percentage);
     144             :     }
     145             : 
     146             :     // If generator doesn't have its own pct count, use ours
     147       12321 :     if (!generator->has_fuel_remaining()) {
     148             :         float remaining_ml;
     149             :         // return false if _params._pack_capacity is too small
     150        8568 :         if (!capacity_remaining_ml(remaining_ml)) {
     151           0 :             return false;
     152             :         }
     153             : 
     154             :         // now we get the percentage according to tank capacity (pack_capacity parameter)
     155        8568 :         percentage = constrain_float(100 * remaining_ml / _params._pack_capacity, 0, UINT8_MAX);;
     156        8568 :         return true;
     157             :         
     158             :     // Otherwise use generator's own percentage
     159             :     } else {
     160        3753 :         percentage = generator->get_fuel_remaining() * 100;  // convert from scale to actual percentage
     161        3753 :         return true;
     162             :     }
     163             : }
     164             : 
     165        5092 : AP_BattMonitor::Failsafe AP_BattMonitor_Generator_FuelLevel::update_failsafes()
     166             : {
     167        5092 :     AP_Generator *generator = AP::generator();
     168        5092 :     if (generator == nullptr) {
     169           0 :         return AP_BattMonitor::Failsafe::None;
     170             :     }
     171             : 
     172        5092 :     if (generator->has_fuel_remaining()) {
     173             :         // work is being done for us by the generator
     174        1472 :         return AP_BattMonitor_Backend::update_failsafes();
     175             :     }
     176             : 
     177             :     // If we are using our own calculated percentage, not the one from
     178             :     // the generator, manage capacity failsafes here
     179             :     float remaining_ml;
     180             :     // only returns false if _params._pack_capacity is too small less than 10 mL
     181        3620 :     if (!capacity_remaining_ml(remaining_ml)) {
     182           0 :         return AP_BattMonitor::Failsafe::Critical;
     183             :     }
     184             : 
     185        3620 :     if (remaining_ml < _params._critical_capacity) {
     186         692 :         return AP_BattMonitor::Failsafe::Critical;
     187             :     }
     188             : 
     189        2928 :     if (remaining_ml < _params._low_capacity) {
     190         406 :         return AP_BattMonitor::Failsafe::Low;
     191             :     }
     192             : 
     193        2522 :     return AP_BattMonitor::Failsafe::None;
     194             : }
     195             : 
     196             : /*
     197             :     Electrical class
     198             : */
     199       52456 : bool AP_BattMonitor_Generator_Elec::has_current(void) const
     200             : {
     201             :     // Get pointer to generator singleton
     202       52456 :     AP_Generator *generator = AP::generator();
     203             : 
     204       52456 :     if (generator == nullptr) {
     205           0 :         return false;
     206             :     }
     207             : 
     208       52456 :     return generator->has_current();
     209             : }
     210             : 
     211        2018 : bool AP_BattMonitor_Generator_Elec::has_consumed_energy(void) const
     212             : {
     213             :     // Get pointer to generator singleton
     214        2018 :     AP_Generator *generator = AP::generator();
     215             : 
     216        2018 :     if (generator == nullptr) {
     217           0 :         return false;
     218             :     }
     219             : 
     220        2018 :     return generator->has_consumed_energy();
     221             : }
     222             : 
     223             : // Read the electrical measurements from the generator
     224       11672 : void AP_BattMonitor_Generator_Elec::read()
     225             : {
     226       11672 :     _state.healthy = false;
     227             : 
     228             :     // Get pointer to generator singleton
     229       11672 :     AP_Generator *generator = AP::generator();
     230             : 
     231             :     // Not healthy if we can't find a generator
     232       11672 :     if (generator == nullptr) {
     233           0 :         return;
     234             :     }
     235             : 
     236       11672 :     if (!generator->healthy()) {
     237         165 :         return;
     238             :     }
     239             : 
     240             :     // Update readings
     241       11507 :     _state.voltage = generator->get_voltage();
     242             : 
     243       11507 :     _state.current_amps = generator->get_current();
     244             : 
     245             :     // Always reset consumed value, integration is done in AP_Generator library
     246       11507 :     _state.consumed_mah = generator->get_batt_consumed();
     247       11507 :     _state.consumed_wh = 0.001f * _state.consumed_mah * _state.voltage;
     248             : 
     249             :     // If we got this far then must be healthy
     250       11507 :     _state.healthy = true;
     251       11507 :     _state.last_time_micros = AP_HAL::micros();
     252             : }
     253             : 
     254        5092 : AP_BattMonitor::Failsafe AP_BattMonitor_Generator_Elec::update_failsafes()
     255             : {
     256        5092 :     AP_BattMonitor::Failsafe failsafe = AP_BattMonitor::Failsafe::None;
     257             : 
     258        5092 :     AP_Generator *generator = AP::generator();
     259             : 
     260             :     // Only check for failsafes on the electrical monitor
     261             :     // no point in having the same failsafe on two battery monitors
     262        5092 :     if (generator != nullptr) {
     263        5092 :         failsafe = generator->update_failsafes();
     264             :     }
     265        5092 :     return MAX(AP_BattMonitor_Backend::update_failsafes(), failsafe);
     266             : }
     267             : #endif  // AP_BATTERY_GENERATOR_ENABLED