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_MS_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <AP_Math/AP_Math.h>
      22             : #include <AP_SerialManager/AP_SerialManager.h>
      23             : 
      24             : #include "AP_EFI_Serial_MS.h"
      25             : 
      26             : extern const AP_HAL::HAL &hal;
      27             : 
      28           1 : AP_EFI_Serial_MS::AP_EFI_Serial_MS(AP_EFI &_frontend):
      29           1 :     AP_EFI_Backend(_frontend)
      30             : {
      31           1 :     internal_state.estimated_consumed_fuel_volume_cm3 = 0; // Just to be sure
      32           1 :     port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0);
      33           1 : }
      34             : 
      35             : 
      36        1216 : void AP_EFI_Serial_MS::update()
      37             : {
      38        1216 :     if (!port) {
      39           0 :         return;
      40             :     }
      41             : 
      42        1216 :     uint32_t now = AP_HAL::millis();
      43             : 
      44        1216 :     const uint32_t expected_bytes = 2 + (RT_LAST_OFFSET - RT_FIRST_OFFSET) + 4;
      45        1216 :     if (port->available() >= expected_bytes && read_incoming_realtime_data()) {
      46         235 :         copy_to_frontend();
      47             :     }
      48             : 
      49        1216 :     const uint32_t last_request_delta = (now - last_request_ms);
      50        1216 :     const uint32_t available = port->available();
      51        1216 :     if (((last_request_delta > 150) && (available > 0)) || // nothing in our input buffer 150 ms after request
      52         244 :         ((last_request_delta > 90)  && (available == 0))) { // we requested something over 90 ms ago, but didn't get any data
      53         244 :         port->discard_input();
      54         244 :         last_request_ms = now;
      55             :         // Request an update from the realtime table (7).
      56             :         // The data we need start at offset 6 and ends at 129
      57         244 :         send_request(7, RT_FIRST_OFFSET, RT_LAST_OFFSET);
      58             :     }
      59             : }
      60             : 
      61         235 : bool AP_EFI_Serial_MS::read_incoming_realtime_data() 
      62             : {
      63             :     // Data is parsed directly from the buffer, otherwise we would need to allocate
      64             :     // several hundred bytes for the entire realtime data table or request every
      65             :     // value individually
      66         235 :     uint16_t message_length = 0;
      67             : 
      68             :     // reset checksum before reading new data
      69         235 :     checksum = 0;
      70             :     
      71             :     // Message length field begins the message (16 bits, excluded from CRC calculation)
      72             :     // Message length value excludes the message length and CRC bytes 
      73         235 :     message_length = port->read() << 8;
      74         235 :     message_length += port->read();
      75             : 
      76         235 :     if (message_length >= 256) {
      77             :         // don't process invalid messages
      78             :         // hal.console->printf("message_length: %u\n", message_length);
      79           0 :         return false;
      80             :     }
      81             : 
      82             :     // Response Flag (see "response_codes" enum)
      83         235 :     response_flag = read_byte_CRC32();
      84         235 :     if (response_flag != RESPONSE_WRITE_OK) {
      85             :         // abort read if we did not receive the correct response code;
      86           0 :         return false;
      87             :     }
      88             :     
      89             :     // Iterate over the payload bytes 
      90       27495 :     for (uint16_t offset=RT_FIRST_OFFSET; offset < (RT_FIRST_OFFSET + message_length - 1); offset++) {
      91       27260 :         uint8_t data = read_byte_CRC32();
      92             :         float temp_float;
      93       27260 :         switch (offset) {
      94         235 :             case PW1_MSB:
      95         235 :                 internal_state.cylinder_status.injection_time_ms = (float)((data << 8) + read_byte_CRC32())*0.001f;
      96         235 :                 offset++;  // increment the counter because we read a byte in the previous line
      97         235 :                 break;
      98         235 :             case RPM_MSB:
      99             :                 // Read 16 bit RPM
     100         235 :                 internal_state.engine_speed_rpm = (data << 8) + read_byte_CRC32();
     101         235 :                 offset++;
     102         235 :                 break;
     103         235 :             case ADVANCE_MSB:
     104         235 :                 internal_state.cylinder_status.ignition_timing_deg = (float)((data << 8) + read_byte_CRC32())*0.1f;
     105         235 :                 offset++;
     106         235 :                 break;
     107         235 :             case ENGINE_BM:
     108         235 :                 break;
     109         235 :             case BAROMETER_MSB:
     110         235 :                 internal_state.atmospheric_pressure_kpa = (float)((data << 8) + read_byte_CRC32())*0.1f;
     111         235 :                 offset++;
     112         235 :                 break;
     113         235 :             case MAP_MSB:
     114         235 :                 internal_state.intake_manifold_pressure_kpa = (float)((data << 8) + read_byte_CRC32())*0.1f;
     115         235 :                 offset++;
     116         235 :                 break;
     117         235 :             case MAT_MSB:
     118         235 :                 temp_float = (float)((data << 8) + read_byte_CRC32())*0.1f;
     119         235 :                 offset++;
     120         235 :                 internal_state.intake_manifold_temperature = degF_to_Kelvin(temp_float);
     121         235 :                 break;
     122         235 :             case CHT_MSB:
     123         235 :                 temp_float = (float)((data << 8) + read_byte_CRC32())*0.1f;
     124         235 :                 offset++;
     125         235 :                 internal_state.cylinder_status.cylinder_head_temperature = degF_to_Kelvin(temp_float);
     126         235 :                 break;
     127         235 :             case TPS_MSB:
     128         235 :                 temp_float = (float)((data << 8) + read_byte_CRC32())*0.1f;
     129         235 :                 offset++;
     130         235 :                 internal_state.throttle_position_percent = roundf(temp_float);
     131         235 :                 break;
     132         235 :             case AFR1_MSB:
     133         235 :                 temp_float = (float)((data << 8) + read_byte_CRC32())*0.1f;
     134         235 :                 offset++;
     135         235 :                 internal_state.cylinder_status.lambda_coefficient = temp_float;
     136         235 :                 break;
     137         235 :             case DWELL_MSB:
     138         235 :                 temp_float = (float)((data << 8) + read_byte_CRC32())*0.1f;
     139         235 :                 internal_state.spark_dwell_time_ms = temp_float;
     140         235 :                 offset++;
     141         235 :                 break;
     142         235 :             case LOAD:
     143         235 :                 internal_state.engine_load_percent = data;
     144         235 :                 break;
     145           0 :             case FUEL_PRESSURE_MSB:
     146             :                 // MS Fuel Pressure is unitless, store as KPA anyway
     147           0 :                 temp_float = (float)((data << 8) + read_byte_CRC32());
     148           0 :                 internal_state.fuel_pressure = temp_float;
     149           0 :                 offset++;
     150           0 :                 break;   
     151             :                 
     152             :         }
     153             :     }
     154             :     
     155             :     // Read the four CRC bytes
     156             :     uint32_t received_CRC;
     157         235 :     received_CRC = port->read() << 24;
     158         235 :     received_CRC += port->read() << 16;
     159         235 :     received_CRC += port->read() << 8;
     160         235 :     received_CRC += port->read();
     161             :                         
     162         235 :     if (received_CRC != checksum) {
     163             :         // hal.console->printf("EFI CRC: 0x%08x 0x%08x\n", received_CRC, checksum);
     164           0 :         return false;
     165             :     }
     166             : 
     167             :     // Calculate Fuel Consumption 
     168             :     // Duty Cycle (Percent, because that's how HFE gives us the calibration coefficients)
     169         235 :     float duty_cycle = (internal_state.cylinder_status.injection_time_ms * internal_state.engine_speed_rpm)/600.0f;
     170         235 :     uint32_t current_time = AP_HAL::millis();
     171             :     // Super Simplified integration method - Error Analysis TBD
     172             :     // This calculation gives erroneous results when the engine isn't running
     173         235 :     if (internal_state.engine_speed_rpm > RPM_THRESHOLD) {
     174          15 :         internal_state.fuel_consumption_rate_cm3pm = duty_cycle*get_coef1() - get_coef2();
     175          15 :         internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (current_time - internal_state.last_updated_ms)/60000.0f;
     176             :     } else {
     177         220 :         internal_state.fuel_consumption_rate_cm3pm = 0;
     178             :     }
     179         235 :     internal_state.last_updated_ms = current_time;
     180             :     
     181         235 :     return true;
     182             :          
     183             : }
     184             : 
     185         244 : void AP_EFI_Serial_MS::send_request(uint8_t table, uint16_t first_offset, uint16_t last_offset)
     186             : {
     187         244 :     uint16_t length = last_offset - first_offset + 1;
     188             :     // Fixed message size (0x0007)
     189             :     // Command 'r' (0x72)
     190             :     // Null CANid (0x00)
     191         244 :     const uint8_t data[9] = {
     192             :         0x00,
     193             :         0x07,
     194             :         0x72,
     195             :         0x00,
     196             :         (uint8_t)table,
     197         244 :         (uint8_t)(first_offset >> 8),
     198             :         (uint8_t)(first_offset),
     199         244 :         (uint8_t)(length >> 8),
     200             :         (uint8_t)(length)   
     201         244 :     };
     202             :     
     203         244 :     uint32_t crc = 0;
     204             :     
     205             :     // Write the request and calc CRC
     206        2440 :     for (uint8_t i = 0;  i != sizeof(data) ; i++) {
     207             :         // Message size is excluded from CRC
     208        2196 :         if (i > 1) {
     209        1708 :             crc = CRC32_compute_byte(crc, data[i]);
     210             :         }
     211        2196 :         port->write(data[i]);
     212             :     }
     213             :     
     214             :     // Write the CRC32
     215         244 :     port->write((uint8_t)(crc >> 24));
     216         244 :     port->write((uint8_t)(crc >> 16));
     217         244 :     port->write((uint8_t)(crc >> 8));
     218         244 :     port->write((uint8_t)crc);
     219             : 
     220         244 : }
     221             : 
     222       29845 : uint8_t AP_EFI_Serial_MS::read_byte_CRC32()
     223             : {   
     224             :     // Read a byte and update the CRC 
     225       29845 :     uint8_t data = port->read();
     226       29845 :     checksum = CRC32_compute_byte(checksum, data);
     227       29845 :     return data;
     228             : }
     229             : 
     230             : // CRC32 matching MegaSquirt
     231       31553 : uint32_t AP_EFI_Serial_MS::CRC32_compute_byte(uint32_t crc, uint8_t data)
     232             : {
     233       31553 :     crc ^= ~0U;
     234       31553 :     crc = crc_crc32(crc, &data, 1);
     235       31553 :     crc ^= ~0U;
     236       31553 :     return crc;
     237             : }
     238             : 
     239             : #endif  // AP_EFI_SERIAL_MS_ENABLED