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             :  * AP_EFI_Currawong_ECU.cpp
      18             :  *
      19             :  *      Author: Reilly Callaway / Currawong Engineering Pty Ltd
      20             :  */
      21             : 
      22             : #include "AP_EFI_Currawong_ECU.h"
      23             : 
      24             : #if AP_EFI_CURRAWONG_ECU_ENABLED
      25             : 
      26             : #include <AP_Param/AP_Param.h>
      27             : #include <AP_PiccoloCAN/piccolo_protocol/ECUPackets.h>
      28             : #include <AP_Math/definitions.h>
      29             : 
      30             : AP_EFI_Currawong_ECU* AP_EFI_Currawong_ECU::_singleton;
      31             : 
      32           0 : AP_EFI_Currawong_ECU::AP_EFI_Currawong_ECU(AP_EFI &_frontend) :
      33           0 :     AP_EFI_Backend(_frontend)
      34             : {
      35           0 :     _singleton = this;
      36             : 
      37             :     // Indicate that temperature and fuel pressure are supported
      38           0 :     internal_state.fuel_pressure_status = Fuel_Pressure_Status::OK;
      39           0 :     internal_state.temperature_status = Temperature_Status::OK;
      40             : 
      41             :     // Currawong ECU does not report EGT 
      42           0 :     internal_state.cylinder_status.exhaust_gas_temperature = NAN;
      43             : 
      44           0 : }
      45             : 
      46           0 : void AP_EFI_Currawong_ECU::update()
      47             : {
      48             :     // copy the data to the front end
      49           0 :     copy_to_frontend();
      50           0 : }
      51             : 
      52           0 : bool AP_EFI_Currawong_ECU::handle_message(AP_HAL::CANFrame &frame)
      53             : {
      54           0 :     bool valid  = true;
      55             : 
      56             :     // There are differences between Ardupilot EFI_State and types/scaling of Piccolo packets.
      57             :     // First decode to Piccolo structs, and then store the data we need in internal_state with any scaling required.
      58             : 
      59             :     // Structs to decode Piccolo messages into
      60             :     ECU_TelemetryFast_t telemetry_fast;
      61             :     ECU_TelemetrySlow0_t telemetry_slow0;
      62             :     ECU_TelemetrySlow1_t telemetry_slow1;
      63             :     ECU_TelemetrySlow2_t telemetry_slow2;
      64             : 
      65             :     // Throw the message at the decoding functions
      66           0 :     if (decodeECU_TelemetryFastPacketStructure(&frame, &telemetry_fast)) {
      67           0 :         internal_state.throttle_position_percent = static_cast<uint8_t>(telemetry_fast.throttle);
      68           0 :         internal_state.engine_load_percent = static_cast<uint8_t>(telemetry_fast.throttle);
      69           0 :         internal_state.engine_speed_rpm = static_cast<uint32_t>(telemetry_fast.rpm);
      70             : 
      71           0 :         if (internal_state.engine_speed_rpm > 0) {
      72           0 :             internal_state.engine_state = Engine_State::RUNNING;
      73             :         } else {
      74           0 :             internal_state.engine_state = Engine_State::STOPPED;
      75             :         }
      76             : 
      77             :         // Prevent div by zero
      78           0 :         if (get_ecu_fuel_density() > 0.01) {
      79           0 :             internal_state.estimated_consumed_fuel_volume_cm3 = static_cast<float>(telemetry_fast.fuelUsed) / KG_PER_M3_TO_G_PER_CM3(get_ecu_fuel_density());
      80             :         } else {
      81             :             // If no (reasonable) density is provided
      82           0 :             internal_state.estimated_consumed_fuel_volume_cm3 = 0.;
      83             :         }
      84             : 
      85           0 :         internal_state.general_error = telemetry_fast.ecuStatusBits.errorIndicator;
      86           0 :         if (!telemetry_fast.ecuStatusBits.enabled) {
      87           0 :             internal_state.engine_state = Engine_State::STOPPED;
      88             :         }
      89           0 :     } else if (decodeECU_TelemetrySlow0PacketStructure(&frame, &telemetry_slow0)) {
      90           0 :         internal_state.intake_manifold_pressure_kpa = telemetry_slow0.map;
      91           0 :         internal_state.atmospheric_pressure_kpa = telemetry_slow0.baro;
      92           0 :         internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(telemetry_slow0.cht);
      93           0 :     } else if (decodeECU_TelemetrySlow1PacketStructure(&frame, &telemetry_slow1)) {
      94           0 :         internal_state.intake_manifold_temperature = C_TO_KELVIN(telemetry_slow1.mat);
      95           0 :         internal_state.fuel_pressure = telemetry_slow1.fuelPressure;
      96           0 :     } else if (decodeECU_TelemetrySlow2PacketStructure(&frame, &telemetry_slow2)) {
      97           0 :         internal_state.cylinder_status.ignition_timing_deg = telemetry_slow2.ignAngle1;
      98             : 
      99           0 :         internal_state.fuel_consumption_rate_cm3pm = telemetry_slow2.flowRate / KG_PER_M3_TO_G_PER_CM3(get_ecu_fuel_density());
     100             :     } else {
     101           0 :         valid = false;
     102             :     }
     103             : 
     104           0 :     if (valid) {
     105           0 :         internal_state.last_updated_ms = AP_HAL::millis();
     106             :     }
     107             : 
     108           0 :     return valid;
     109             : }
     110             : 
     111             : #endif // AP_EFI_CURRAWONG_ECU_ENABLED