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             : 
      18             : #include "AP_EFI_config.h"
      19             : 
      20             : #if AP_EFI_SERIAL_HIRTH_ENABLED
      21             : #include <AP_HAL/AP_HAL.h>
      22             : #include <AP_EFI/AP_EFI_Serial_Hirth.h>
      23             : #include <AP_SerialManager/AP_SerialManager.h>
      24             : #include <SRV_Channel/SRV_Channel.h>
      25             : #include <AP_ICEngine/AP_ICEngine.h>
      26             : #include <AP_Math/definitions.h>
      27             : #include <AP_Logger/AP_Logger.h>
      28             : 
      29             : #define HIRTH_MAX_PKT_SIZE 100
      30             : #define HIRTH_MAX_RAW_PKT_SIZE 103
      31             : 
      32             : #define SERIAL_WAIT_TIMEOUT_MS 100
      33             : 
      34             : #define ENGINE_RUNNING 4
      35             : #define THROTTLE_POSITION_FACTOR 10
      36             : #define INJECTION_TIME_RESOLUTION 0.8
      37             : #define THROTTLE_POSITION_RESOLUTION 0.1
      38             : #define VOLTAGE_RESOLUTION 0.0049       /* 5/1024 */
      39             : #define ADC_CALIBRATION (5.0/1024.0)
      40             : #define MAP_HPA_PER_VOLT_FACTOR 248.2673
      41             : #define HPA_TO_KPA 0.1
      42             : #define TPS_SCALE 0.70
      43             : 
      44             : // request/response status constants
      45             : #define QUANTITY_REQUEST_STATUS    0x03
      46             : #define QUANTITY_SET_VALUE         0x17
      47             : #define CODE_REQUEST_STATUS_1      0x04
      48             : #define CODE_REQUEST_STATUS_2      0x0B
      49             : #define CODE_REQUEST_STATUS_3      0x0D
      50             : #define CODE_SET_VALUE             0xC9
      51             : #define CHECKSUM_REQUEST_STATUS_1  0xF9
      52             : #define CHECKSUM_REQUEST_STATUS_2  0xF2
      53             : #define CHECKSUM_REQUEST_STATUS_3  0xF0
      54             : #define QUANTITY_RESPONSE_STATUS_1 0x57
      55             : #define QUANTITY_RESPONSE_STATUS_2 0x65
      56             : #define QUANTITY_RESPONSE_STATUS_3 0x67
      57             : #define QUANTITY_ACK_SET_VALUES    0x03
      58             : 
      59             : extern const AP_HAL::HAL& hal;
      60             : 
      61             : /**
      62             :  * @brief Constructor with port initialization
      63             :  * 
      64             :  * @param _frontend 
      65             :  */
      66           1 : AP_EFI_Serial_Hirth::AP_EFI_Serial_Hirth(AP_EFI &_frontend) :
      67           1 :     AP_EFI_Backend(_frontend)
      68             : {
      69           1 :     port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0);
      70           1 :     set_default_coef1(1.0);
      71           1 : }
      72             : 
      73             : /**
      74             :  * @brief checks for response from or makes requests to Hirth ECU periodically
      75             :  * 
      76             :  */
      77        1216 : void AP_EFI_Serial_Hirth::update()
      78             : {
      79        1216 :     if (port == nullptr) {
      80           0 :         return;
      81             :     }
      82             : 
      83             :     // parse response from Hirth
      84        1216 :     check_response();
      85             : 
      86             :     // send request
      87        1216 :     send_request();
      88             : }
      89             : 
      90             : /**
      91             :  * @brief Checks if required bytes are available and proceeds with parsing
      92             :  * 
      93             :  */
      94        1216 : void AP_EFI_Serial_Hirth::check_response()
      95             : {
      96        1216 :     const uint32_t now = AP_HAL::millis();
      97             : 
      98             :     // waiting for response
      99        1216 :     if (!waiting_response) {
     100           1 :         return;
     101             :     }
     102             : 
     103        1215 :     const uint32_t num_bytes = port->available();
     104             : 
     105             :     // if already requested
     106        1215 :     if (num_bytes >= expected_bytes) {
     107             :         // read data from buffer
     108         609 :         uint8_t computed_checksum = 0;
     109         609 :         computed_checksum += res_data.quantity = port->read();
     110         609 :         computed_checksum += res_data.code = port->read();
     111             : 
     112         609 :         if (res_data.code == requested_code) {
     113       53195 :             for (int i = 0; i < (res_data.quantity - QUANTITY_REQUEST_STATUS); i++) {
     114       52586 :                 computed_checksum += raw_data[i] = port->read();
     115             :             }
     116             :         }
     117             : 
     118         609 :         res_data.checksum = port->read();
     119         609 :         if (res_data.checksum != (256 - computed_checksum)) {
     120          46 :             crc_fail_cnt++;
     121          46 :             port->discard_input();
     122             :         } else {
     123         563 :             uptime = now - last_packet_ms;
     124         563 :             last_packet_ms = now;
     125         563 :             internal_state.last_updated_ms = now;
     126         563 :             decode_data();
     127         563 :             copy_to_frontend();
     128         563 :             port->discard_input();
     129             :         }
     130             : 
     131         609 :         waiting_response = false;
     132             : 
     133             : #if HAL_LOGGING_ENABLED
     134         609 :         log_status();
     135             : #endif
     136             :     }
     137             : 
     138             :     // reset request if no response for SERIAL_WAIT_TIMEOUT_MS
     139        1215 :     if (waiting_response &&
     140         606 :         now - last_request_ms > SERIAL_WAIT_TIMEOUT_MS) {
     141           7 :         waiting_response = false;
     142           7 :         last_request_ms = now;
     143             : 
     144           7 :         port->discard_input();
     145           7 :         ack_fail_cnt++;
     146             :     }
     147             : }
     148             : 
     149             : /**
     150             :  * @brief Send Throttle and Telemetry requests to Hirth
     151             :  * 
     152             :  */
     153        1216 : void AP_EFI_Serial_Hirth::send_request()
     154             : {
     155        1216 :     if (waiting_response) {
     156         599 :         return;
     157             :     }
     158             : 
     159         617 :     const uint32_t now = AP_HAL::millis();
     160             :     bool request_was_sent;
     161             : 
     162             :     // get new throttle value
     163         617 :     const uint16_t new_throttle = (uint16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
     164             : 
     165             :     // check for change or timeout for throttle value
     166         617 :     if ((new_throttle != last_throttle) || (now - last_req_send_throttle_ms > 500)) {
     167             :         // send new throttle value, only when ARMED
     168          48 :         bool allow_throttle = hal.util->get_soft_armed();
     169          48 :         if (!allow_throttle) {
     170             : #if AP_ICENGINE_ENABLED
     171          44 :             const auto *ice = AP::ice();
     172          44 :             if (ice != nullptr) {
     173          44 :                 allow_throttle = ice->allow_throttle_while_disarmed();
     174             :             }
     175             : #endif  // AP_ICENGINE_ENABLED
     176             :         }
     177          48 :         if (allow_throttle) {
     178           4 :             request_was_sent = send_target_values(new_throttle);
     179             :         } else {
     180          44 :             request_was_sent = send_target_values(0);
     181             :         }
     182             : 
     183          48 :         last_throttle = new_throttle;
     184          48 :         last_req_send_throttle_ms = now;
     185          48 :     } else {
     186             :         // request Status request at the driver update rate if no throttle commands
     187         569 :         request_was_sent = send_request_status();
     188             :     }
     189             : 
     190         617 :     if (request_was_sent) {
     191         617 :         waiting_response = true;
     192         617 :         last_request_ms = now;
     193             :     }
     194             : }
     195             : 
     196             : 
     197             : /**
     198             :  * @brief sends the new throttle command to Hirth ECU
     199             :  * 
     200             :  * @param thr - new throttle value given by SRV_Channel::k_throttle
     201             :  * @return true - if success
     202             :  * @return false - currently not implemented
     203             :  */
     204          48 : bool AP_EFI_Serial_Hirth::send_target_values(uint16_t thr)
     205             : {
     206          48 :     uint8_t computed_checksum = 0;
     207          48 :     throttle_to_hirth = 0;
     208             : 
     209             :     // clear buffer
     210          48 :     memset(raw_data, 0, ARRAY_SIZE(raw_data));
     211             : 
     212             : #if AP_EFI_THROTTLE_LINEARISATION_ENABLED
     213             :     // linearise throttle input
     214          48 :     thr = linearise_throttle(thr);
     215             : #endif
     216             : 
     217          48 :     throttle_to_hirth = thr * THROTTLE_POSITION_FACTOR;
     218             : 
     219          48 :     uint8_t idx = 0;
     220             : 
     221             :     // set Quantity + Code + "20 bytes of records to set" + Checksum
     222          48 :     computed_checksum += raw_data[idx++] = QUANTITY_SET_VALUE;
     223          48 :     computed_checksum += raw_data[idx++] = requested_code = CODE_SET_VALUE;
     224          48 :     computed_checksum += raw_data[idx++] = throttle_to_hirth & 0xFF;
     225          48 :     computed_checksum += raw_data[idx++] = (throttle_to_hirth >> 8) & 0xFF;
     226             : 
     227             :     // checksum calculation
     228          48 :     raw_data[QUANTITY_SET_VALUE - 1] = (256 - computed_checksum);
     229             :     
     230          48 :     expected_bytes = QUANTITY_ACK_SET_VALUES;
     231             :     // write data
     232          48 :     port->write(raw_data, QUANTITY_SET_VALUE);
     233             : 
     234          48 :     return true;
     235             : }
     236             : 
     237             : 
     238             : /**
     239             :  * @brief cyclically sends different Status requests to Hirth ECU
     240             :  * 
     241             :  * @return true - when successful
     242             :  * @return false  - not implemented
     243             :  */
     244         569 : bool AP_EFI_Serial_Hirth::send_request_status() {
     245             : 
     246             :     uint8_t requested_quantity;
     247             :     uint8_t requested_checksum;
     248             : 
     249         569 :     switch (requested_code)
     250             :     {
     251         179 :     case CODE_REQUEST_STATUS_1:
     252         179 :         requested_quantity = QUANTITY_REQUEST_STATUS;
     253         179 :         requested_code = CODE_REQUEST_STATUS_2;
     254         179 :         requested_checksum = CHECKSUM_REQUEST_STATUS_2;
     255         179 :         expected_bytes = QUANTITY_RESPONSE_STATUS_2;
     256         179 :         break;
     257         177 :     case CODE_REQUEST_STATUS_2:
     258         177 :         requested_quantity = QUANTITY_REQUEST_STATUS;
     259         177 :         requested_code = CODE_REQUEST_STATUS_3;
     260         177 :         requested_checksum = CHECKSUM_REQUEST_STATUS_3;
     261         177 :         expected_bytes = QUANTITY_RESPONSE_STATUS_3;
     262         177 :         break;
     263         166 :     case CODE_REQUEST_STATUS_3:
     264         166 :         requested_quantity = QUANTITY_REQUEST_STATUS;
     265         166 :         requested_code = CODE_REQUEST_STATUS_1;
     266         166 :         requested_checksum = CHECKSUM_REQUEST_STATUS_1;
     267         166 :         expected_bytes = QUANTITY_RESPONSE_STATUS_1;
     268         166 :         break;
     269          47 :     default:
     270          47 :         requested_quantity = QUANTITY_REQUEST_STATUS;
     271          47 :         requested_code = CODE_REQUEST_STATUS_1;
     272          47 :         requested_checksum = CHECKSUM_REQUEST_STATUS_1;
     273          47 :         expected_bytes = QUANTITY_RESPONSE_STATUS_1;
     274          47 :         break;
     275             :     }
     276         569 :     raw_data[0] = requested_quantity;
     277         569 :     raw_data[1] = requested_code;
     278         569 :     raw_data[2] = requested_checksum;
     279             : 
     280         569 :     port->write(raw_data, QUANTITY_REQUEST_STATUS);
     281             : 
     282         569 :     return true;
     283             : }
     284             : 
     285             : 
     286             : /**
     287             :  * @brief parses the response from Hirth ECU and updates the internal state instance
     288             :  * 
     289             :  */
     290         563 : void AP_EFI_Serial_Hirth::decode_data()
     291             : {
     292         563 :     const uint32_t now = AP_HAL::millis();
     293             : 
     294         563 :     switch (res_data.code) {
     295         210 :     case CODE_REQUEST_STATUS_1: {
     296         210 :         struct Record1 *record1 = (Record1*)raw_data;
     297             : 
     298         210 :         internal_state.engine_speed_rpm = record1->rpm;
     299         210 :         internal_state.throttle_out = record1->throttle;
     300             : 
     301             :         // EFI2 log
     302         210 :         internal_state.engine_state = (Engine_State)record1->engine_status;
     303             : 
     304             :         // ECYL log
     305         210 :         internal_state.cylinder_status.injection_time_ms = record1->injection_time * INJECTION_TIME_RESOLUTION;
     306         210 :         internal_state.cylinder_status.ignition_timing_deg = record1->ignition_angle;
     307             : 
     308             :         // EFI3 log
     309         210 :         internal_state.ignition_voltage = record1->battery_voltage * VOLTAGE_RESOLUTION;
     310             : 
     311         210 :         sensor_status = record1->sensor_ok;
     312             : 
     313             :         // resusing mavlink variables as required for Hirth
     314             :         // add in ADC voltage of MAP sensor > convert to MAP in kPa
     315         210 :         internal_state.intake_manifold_pressure_kpa = record1->voltage_int_air_pressure * (ADC_CALIBRATION * MAP_HPA_PER_VOLT_FACTOR * HPA_TO_KPA);
     316         210 :         internal_state.intake_manifold_temperature = C_TO_KELVIN(record1->air_temperature);
     317         210 :         break;
     318             :     }
     319             : 
     320         177 :     case CODE_REQUEST_STATUS_2: {
     321         177 :         struct Record2 *record2 = (Record2*)raw_data;
     322             : 
     323             :         // EFI log
     324         177 :         const float fuel_consumption_rate_lph = record2->fuel_consumption * 0.1;
     325             : 
     326         177 :         internal_state.fuel_consumption_rate_cm3pm = (fuel_consumption_rate_lph * 1000.0 / 60.0) * get_coef1();
     327             : 
     328         177 :         if (last_fuel_integration_ms != 0) {
     329             :             // estimated_consumed_fuel_volume_cm3 is in cm3/pm
     330         176 :             const float dt_minutes = (now - last_fuel_integration_ms)*(0.001/60);
     331         176 :             internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * dt_minutes;
     332             :         }
     333         177 :         last_fuel_integration_ms = now;
     334             : 
     335         177 :         internal_state.throttle_position_percent = record2->throttle_percent_times_10 * 0.1;
     336         177 :         break;
     337             :     }
     338             : 
     339         176 :     case CODE_REQUEST_STATUS_3: {
     340         176 :         struct Record3 *record3 = (Record3*)raw_data;
     341             : 
     342             :         // EFI3 Log
     343         176 :         error_excess_temperature = record3->error_excess_temperature_bitfield;
     344             : 
     345             :         // ECYL log
     346         176 :         internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(record3->excess_temperature_1);
     347         176 :         internal_state.cylinder_status.cylinder_head_temperature2 = C_TO_KELVIN(record3->excess_temperature_2);
     348         176 :         internal_state.cylinder_status.exhaust_gas_temperature = C_TO_KELVIN(record3->excess_temperature_3);
     349         176 :         internal_state.cylinder_status.exhaust_gas_temperature2 = C_TO_KELVIN(record3->excess_temperature_4);
     350         176 :         break;
     351             :     }
     352             :         
     353             :     // case CODE_SET_VALUE:
     354             :     //     // Do nothing for now
     355             :     //     break;
     356             :     }
     357         563 : }
     358             : 
     359             : #if HAL_LOGGING_ENABLED
     360         609 : void AP_EFI_Serial_Hirth::log_status(void)
     361             : {
     362             :     // @LoggerMessage: EFIS
     363             :     // @Description: Electronic Fuel Injection data - Hirth specific Status information
     364             :     // @Field: TimeUS: Time since system startup
     365             :     // @Field: EET: Error Excess Temperature Bitfield
     366             :     // @FieldBitmaskEnum: EET: AP_EFI_Serial_Hirth:::Error_Excess_Temp_Bitfield
     367             :     // @Field: FLAG: Sensor Status Bitfield
     368             :     // @FieldBitmaskEnum: FLAG: AP_EFI_Serial_Hirth:::Sensor_Status_Bitfield
     369             :     // @Field: CRF: CRC failure count
     370             :     // @Field: AKF: ACK failure count
     371             :     // @Field: Up: Uptime between 2 messages
     372             :     // @Field: ThO: Throttle output as received by the engine
     373             :     // @Field: ThM: Modified throttle_to_hirth output sent to the engine
     374        1218 :     AP::logger().WriteStreaming("EFIS",
     375             :                                 "TimeUS,EET,FLAG,CRF,AKF,Up,ThO,ThM",
     376             :                                 "s-------",
     377             :                                 "F-------",
     378             :                                 "QHBIIIfH",
     379             :                                 AP_HAL::micros64(),
     380         609 :                                 uint16_t(error_excess_temperature),
     381         609 :                                 uint8_t(sensor_status),
     382         609 :                                 uint32_t(crc_fail_cnt),
     383         609 :                                 uint32_t(ack_fail_cnt),
     384         609 :                                 uint32_t(uptime),
     385         609 :                                 float(internal_state.throttle_out),
     386         609 :                                 uint16_t(throttle_to_hirth));
     387         609 : }
     388             : #endif // HAL_LOGGING_ENABLED
     389             : 
     390             : #endif // AP_EFI_SERIAL_HIRTH_ENABLED