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             : #pragma GCC optimize("Os")
      17             : 
      18             : #include "AP_Generator_IE_650_800.h"
      19             : 
      20             : #if AP_GENERATOR_IE_650_800_ENABLED
      21             : 
      22             : extern const AP_HAL::HAL& hal;
      23             : 
      24           7 : void AP_Generator_IE_650_800::init()
      25             : {
      26             :     // Call init from base class to do common setup
      27           7 :     AP_Generator_IE_FuelCell::init();
      28             : 
      29             :     // Tell frontend what measurements are available for this generator
      30             :     // This unit does not have current but this needs to be true to make use of consumed_mah in BattMonitor
      31           7 :     _frontend._has_current = true;
      32           7 :     _frontend._has_consumed_energy = true;
      33           7 :     _frontend._has_fuel_remaining = true;
      34           7 : }
      35             : 
      36             : // Update fuel cell, expected to be called at 20hz
      37           0 : void AP_Generator_IE_650_800::assign_measurements(const uint32_t now)
      38             : {
      39             :     // Successfully decoded a new valid sentence
      40             :     // Update internal fuel cell state
      41           0 :     _state = (State)_parsed.state;
      42           0 :     _err_code = _parsed.err_code;
      43             : 
      44             :     // Update variables to be returned to front end
      45           0 :     _fuel_remaining = _parsed.tank_pct * 0.01;
      46             : 
      47             :     // Invert bat remaining percent to match AP_BattMonitor convention
      48           0 :     _consumed_mah = 100.0f - _parsed.battery_pct;
      49             : 
      50             :     // This unit does not report voltage, always report 1 volt
      51           0 :     _voltage = 1;
      52             : 
      53           0 :     _last_time_ms = now;
      54           0 : }
      55             : 
      56             : // Process characters received and extract terms for IE 650/800 W
      57           0 : void AP_Generator_IE_650_800::decode_latest_term()
      58             : {
      59             :     // Null terminate and move onto the next term
      60           0 :     _term[_term_offset] = 0;
      61           0 :     _term_offset = 0;
      62           0 :     _term_number++;
      63             : 
      64           0 :     if (_start_char != '<') {
      65           0 :         _sentence_valid = false;
      66           0 :         return;
      67             :     }
      68             : 
      69           0 :     switch (_term_number) {
      70           0 :         case 1:
      71           0 :             _parsed.tank_pct = strtof(_term, NULL);
      72             :             // Out of range values
      73           0 :             if (_parsed.tank_pct > 100.0f || _parsed.tank_pct < 0.0f) {
      74           0 :                 _data_valid = false;
      75             :             }
      76             :             break;
      77             : 
      78           0 :         case 2:
      79           0 :             _parsed.battery_pct = strtof(_term, NULL);
      80             :             // Out of range values
      81           0 :             if (_parsed.battery_pct > 100.0f || _parsed.battery_pct < 0.0f) {
      82           0 :                 _data_valid = false;
      83             :             }
      84             :             break;
      85             : 
      86           0 :         case 3:
      87           0 :             _parsed.state = strtoul(_term, nullptr, 10);
      88           0 :             break;
      89             :         
      90           0 :         case 4:
      91           0 :             _parsed.err_code = strtoul(_term, nullptr, 16);
      92             :             // Sentence only declared valid when we have the expected number of terms
      93           0 :             _sentence_valid = _data_valid;
      94           0 :             break;
      95             : 
      96           0 :         default:
      97             :             // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence
      98           0 :             _sentence_valid = false;
      99           0 :             break;
     100             :     }
     101             : }
     102             : 
     103             : // Check error codes and populate message with error code
     104           0 : bool AP_Generator_IE_650_800::check_for_err_code(char* msg_txt, uint8_t msg_len) const
     105             : {
     106             :     // Check for any valid error codes
     107           0 :     if ((_err_code & (fs_crit_mask | fs_low_mask)) == 0) {
     108             :         return false;
     109             :     }
     110             : 
     111             :     // Error codes are converted to hex to make it easier to compare to user manual for these units
     112           0 :     hal.util->snprintf(msg_txt, msg_len, "Fuel cell err code <0x%x>", (unsigned)_err_code);
     113           0 :     return true;
     114             : }
     115             : 
     116             : // Check for failsafes
     117           0 : AP_BattMonitor::Failsafe AP_Generator_IE_650_800::update_failsafes() const
     118             : {
     119             :     // Check if we are in a critical failsafe
     120           0 :     if ((_err_code & fs_crit_mask) != 0) {
     121             :         return  AP_BattMonitor::Failsafe::Critical;
     122             :     }
     123             : 
     124             :     // Check if we are in a low failsafe
     125           0 :     if ((_err_code & fs_low_mask) != 0) {
     126           0 :         return  AP_BattMonitor::Failsafe::Low;
     127             :     }
     128             : 
     129             :     return AP_BattMonitor::Failsafe::None;
     130             : }
     131             : 
     132             : #endif  // AP_GENERATOR_IE_650_800_ENABLED
     133             :