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_FuelCell.h"
      19             : 
      20             : #if AP_GENERATOR_IE_ENABLED
      21             : 
      22             : #include <AP_SerialManager/AP_SerialManager.h>
      23             : #include <GCS_MAVLink/GCS.h>
      24             : 
      25             : // Initialize the fuelcell object and prepare it for use
      26          11 : void AP_Generator_IE_FuelCell::init()
      27             : {
      28          11 :     _uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Generator, 0);
      29             : 
      30          11 :     if (_uart == nullptr) {
      31           7 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: No serial port found");
      32           7 :         return;
      33             :     }
      34           4 :     _uart->begin(AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_Generator, 0));
      35           4 :     _health_warn_last_ms = AP_HAL::millis();
      36             : }
      37             : 
      38             : // Update fuelcell, expected to be called at 10hz
      39        3560 : void AP_Generator_IE_FuelCell::update()
      40             : {
      41        3560 :     if (_uart == nullptr) {
      42             :         return;
      43             :     }
      44             : 
      45        3392 :     const uint32_t now = AP_HAL::millis();
      46             : 
      47             :     // Read any available data
      48       28189 :     for (uint8_t i = 0; i < UINT8_MAX; i++) {  // process at most n bytes
      49             :         uint8_t c;
      50       28189 :         if (!_uart->read(c)) {
      51             :             break;
      52             :         }
      53             : 
      54       24797 :         if (!decode(c)) {
      55             :             // Sentence not yet valid, don't assign state and output
      56       24078 :             continue;
      57             :         }
      58             : 
      59             :         // We have a valid sentence, write the parsed values to unit specific measurements
      60         719 :         assign_measurements(now);
      61             :     }
      62             : 
      63        3392 :     _healthy = (now - _last_time_ms) < HEALTHY_TIMEOUT_MS;
      64             : 
      65             :     // Check if we should notify gcs off any change of fuel cell state
      66        3392 :     check_status(now);
      67             : 
      68        3392 :     update_frontend();
      69             : 
      70             : #if HAL_LOGGING_ENABLED
      71        3392 :     log_write();
      72             : #endif
      73             : }
      74             : 
      75             : // Add a single character to the buffer and attempt to decode
      76             : // Returns true if a complete sentence was successfully decoded
      77       24797 : bool AP_Generator_IE_FuelCell::decode(char c)
      78             : {
      79             :     // Start of a string
      80       24797 :     if ((c == '<') || (c == '[')) {
      81         719 :         _start_char = c;
      82         719 :         _sentence_valid = false;
      83         719 :         _data_valid = true;
      84         719 :         _term_number = 0;
      85         719 :         _term_offset = 0;
      86         719 :         _in_string = true;
      87         719 :         _checksum = c;
      88         719 :         return false;
      89             :     }
      90       24078 :     if (!_in_string) {
      91             :         return false;
      92             :     }
      93             : 
      94             :     // End of a string
      95       23359 :     const char end_char = (_start_char == '[') ? ']' : '>';
      96       23359 :     if (c == end_char) {
      97         719 :         decode_latest_term();
      98         719 :         _in_string = false;
      99             : 
     100         719 :         return _sentence_valid;
     101             :     }
     102             : 
     103             :     // End of a term in the string
     104       22640 :     if (c == ',') {
     105        5982 :         decode_latest_term();
     106        5982 :         _checksum += c;
     107        5982 :         return false;
     108             :     }
     109             : 
     110             :     // Otherwise add the char to the current term
     111       16658 :     _term[_term_offset++] = c;
     112       16658 :     _checksum += c;
     113             : 
     114             :     // We have overrun the expected sentence
     115       16658 :     if (_term_offset >TERM_BUFFER) {
     116           0 :         _in_string = false;
     117             :     }
     118             : 
     119             :     return false;
     120             : }
     121             : 
     122             : // Check for arming
     123         218 : bool AP_Generator_IE_FuelCell::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
     124             : {
     125             :     // Refuse arming if not healthy
     126         218 :     if (!healthy()) {
     127          36 :         strncpy(failmsg, "Not healthy", failmsg_len);
     128          36 :         return false;
     129             :     }
     130             : 
     131             :     // Refuse arming if not in running state
     132         182 :     if (!is_running()) {
     133          14 :         strncpy(failmsg, "Status not running", failmsg_len);
     134          14 :         return false;
     135             :     }
     136             : 
     137             :     // Check for error codes
     138         168 :     if (check_for_err_code(failmsg, failmsg_len)) {
     139           2 :         return false;
     140             :     }
     141             : 
     142             :     return true;
     143             : }
     144             : 
     145             : // Lookup table for running state.  State code is the same for all IE units.
     146             : const AP_Generator_IE_FuelCell::Lookup_State AP_Generator_IE_FuelCell::lookup_state[] = {
     147             :     { State::STARTING,"Starting"},
     148             :     { State::READY,"Ready"},
     149             :     { State::RUNNING,"Running"},
     150             :     { State::FAULT,"Fault"},
     151             :     { State::BATTERY_ONLY,"Battery Only"},
     152             : };
     153             : 
     154             : // Check for any change in error state or status and report to gcs
     155        3392 : void AP_Generator_IE_FuelCell::check_status(const uint32_t now)
     156             : {
     157             :     // Check driver health
     158        3392 :     if (!healthy() && (!_health_warn_last_ms || (now - _health_warn_last_ms >= 20000))) {
     159             :         // Don't spam GCS with unhealthy message
     160           4 :         _health_warn_last_ms = now;
     161           4 :         GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Generator: Not healthy");
     162             : 
     163        3388 :     } else if (healthy()) {
     164        3262 :         _health_warn_last_ms = 0;
     165             :     }
     166             : 
     167             :     // If fuel cell state has changed send gcs message
     168        3392 :     update_state_msg();
     169             : 
     170             :     // Check error codes
     171        3392 :     check_for_err_code_if_changed();
     172        3392 : }
     173             : 
     174             : // Check error codes and populate message with error code
     175        3392 : void AP_Generator_IE_FuelCell::check_for_err_code_if_changed()
     176             : {
     177             :     // Only check if there has been a change in error code
     178        3392 :     if ((_err_code == _last_err_code) && (_sub_err_code == _last_sub_err_code)) {
     179        3388 :         return;
     180             :     }
     181             : 
     182             : #if HAL_GCS_ENABLED
     183             :     char msg_txt[64];
     184           4 :     if (check_for_err_code(msg_txt, sizeof(msg_txt)) || check_for_warning_code(msg_txt, sizeof(msg_txt))) {
     185           2 :         GCS_SEND_TEXT(get_mav_severity(_err_code), "%s", msg_txt);
     186             : 
     187           2 :     } else if ((_err_code == 0) && (_sub_err_code == 0)) {
     188           2 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fuel cell error cleared");
     189             : 
     190             :     }
     191             : #endif
     192             : 
     193           4 :     _last_err_code = _err_code;
     194           4 :     _last_sub_err_code = _sub_err_code;
     195             : 
     196             : }
     197             : 
     198             : // Return true is fuel cell is in running state suitable for arming
     199          82 : bool AP_Generator_IE_FuelCell::is_running() const
     200             : {
     201          82 :     return _state == State::RUNNING;
     202             : }
     203             : 
     204             : // Print msg to user updating on state change
     205        1516 : void AP_Generator_IE_FuelCell::update_state_msg()
     206             : {
     207             :     // If fuel cell state has changed send gcs message
     208        1516 :     if (_state != _last_state) {
     209          14 :         for (const struct Lookup_State entry : lookup_state) {
     210          14 :             if (_state == entry.option) {
     211           4 :                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt);
     212           4 :                 break;
     213             :             }
     214             :         }
     215           4 :         _last_state = _state;
     216             :     }
     217        1516 : }
     218             : 
     219             : #endif  // AP_GENERATOR_IE_ENABLED