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_EFI_config.h"
      17             : 
      18             : #if AP_EFI_SERIAL_LUTAN_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <AP_HAL/utility/sparse-endian.h>
      22             : #include <AP_Math/AP_Math.h>
      23             : #include <AP_SerialManager/AP_SerialManager.h>
      24             : 
      25             : #include "AP_EFI_Serial_Lutan.h"
      26             : 
      27             : #include <stdio.h>
      28             : 
      29             : // RPM Threshold for fuel consumption estimator
      30             : #define RPM_THRESHOLD                100
      31             : 
      32             : extern const AP_HAL::HAL &hal;
      33             : 
      34           0 : AP_EFI_Serial_Lutan::AP_EFI_Serial_Lutan(AP_EFI &_frontend):
      35           0 :     AP_EFI_Backend(_frontend)
      36             : {
      37           0 :     port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0);
      38           0 : }
      39             : 
      40             : 
      41           0 : void AP_EFI_Serial_Lutan::update()
      42             : {
      43           0 :     if (port == nullptr) {
      44           0 :         return;
      45             :     }
      46             : 
      47           0 :     const uint32_t now = AP_HAL::millis();
      48           0 :     if (pkt_nbytes > 0 && now - last_recv_ms > 100) {
      49           0 :         pkt_nbytes = 0;
      50             :     }
      51           0 :     const uint32_t n = MIN(sizeof(pkt), port->available());
      52           0 :     if (n > 0) {
      53           0 :         last_recv_ms = now;
      54             :     }
      55           0 :     if (n + pkt_nbytes > sizeof(pkt)) {
      56           0 :         pkt_nbytes = 0;
      57             :     }
      58           0 :     ssize_t nread = port->read(&pkt[pkt_nbytes], n);
      59           0 :     if (nread < 0) {
      60           0 :         nread = 0;
      61             :     }
      62           0 :     pkt_nbytes += nread;
      63           0 :     if (pkt_nbytes > 2) {
      64           0 :         const uint16_t length = be16toh(data.length);
      65           0 :         if (length+6 == pkt_nbytes) {
      66             :             // got a pkt of right length
      67           0 :             const uint32_t crc = be32toh_ptr(&pkt[length+2]);
      68           0 :             const uint32_t crc2 = ~crc_crc32(~0U, &pkt[2], length);
      69           0 :             if (crc == crc2) {
      70             :                 // valid data
      71           0 :                 internal_state.spark_dwell_time_ms = int16_t(be16toh(data.dwell))*0.1;
      72           0 :                 internal_state.cylinder_status.injection_time_ms = be16toh(data.pulseWidth1)*0.00666;
      73           0 :                 internal_state.engine_speed_rpm = be16toh(data.rpm);
      74           0 :                 internal_state.atmospheric_pressure_kpa = int16_t(be16toh(data.barometer))*0.1;
      75           0 :                 internal_state.intake_manifold_pressure_kpa = int16_t(be16toh(data.map))*0.1;
      76           0 :                 internal_state.intake_manifold_temperature = degF_to_Kelvin(int16_t(be16toh(data.mat))*0.1);
      77           0 :                 internal_state.coolant_temperature = degF_to_Kelvin(int16_t(be16toh(data.coolant))*0.1);
      78             :                 // CHT is in coolant field
      79           0 :                 internal_state.cylinder_status.cylinder_head_temperature = internal_state.coolant_temperature;
      80           0 :                 internal_state.throttle_position_percent = int16_t(be16toh(data.tps))*0.1;
      81             : 
      82             :                 // integrate fuel consumption
      83           0 :                 if (internal_state.engine_speed_rpm > RPM_THRESHOLD) {
      84           0 :                     const float duty_cycle = (internal_state.cylinder_status.injection_time_ms * internal_state.engine_speed_rpm)/600.0f;
      85           0 :                     internal_state.fuel_consumption_rate_cm3pm = duty_cycle*get_coef1() - get_coef2();
      86           0 :                     internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (now - internal_state.last_updated_ms)/60000.0f;
      87             :                 } else {
      88           0 :                     internal_state.fuel_consumption_rate_cm3pm = 0;
      89             :                 }
      90           0 :                 internal_state.last_updated_ms = now;
      91           0 :                 copy_to_frontend();
      92             :             }
      93           0 :             pkt_nbytes = 0;
      94             :         }
      95             :     }
      96           0 :     if (now - last_request_ms > 200) {
      97           0 :         last_request_ms = now;
      98           0 :         port->discard_input();
      99           0 :         send_request();
     100             :     }
     101             : }
     102             : 
     103           0 : void AP_EFI_Serial_Lutan::send_request(void)
     104             : {
     105             :     static const uint8_t d[] = { 0, 1, 0x41 };
     106           0 :     const uint32_t crc = ~crc_crc32(~0U, &d[2], sizeof(d)-2);
     107           0 :     const uint32_t crc2 = htobe32(crc);
     108           0 :     port->write(d, sizeof(d));
     109           0 :     port->write((const uint8_t *)&crc2, sizeof(crc2));
     110           0 : }
     111             : 
     112             : #endif  // AP_EFI_SERIAL_LUTAN_ENABLED