mirror of https://github.com/ArduPilot/ardupilot
AP_EFI: Add Currawong ECU packet decoding
This commit is contained in:
Reilly Callaway
Peter Barker
parent
21ac8d801c
commit
1cff0125ae
|
|
@ -17,7 +17,6 @@
|
|||
|
||||
#if HAL_EFI_ENABLED
|
||||
|
||||
#define HAL_EFI_CURRAWONG_ECU_ENABLED 1
|
||||
#include "AP_EFI_Serial_MS.h"
|
||||
#include "AP_EFI_Serial_Lutan.h"
|
||||
#include "AP_EFI_NWPMU.h"
|
||||
|
|
@ -55,13 +54,13 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = {
|
|||
// @User: Advanced
|
||||
AP_GROUPINFO("_COEF2", 3, AP_EFI, coef2, 0),
|
||||
|
||||
// @Param: ECU_DN
|
||||
// @Param: _FUEL_DENS
|
||||
// @DisplayName: ECU Fuel Density
|
||||
// @Description: Used to calculate fuel consumption
|
||||
// @Units: kg/m/m/m
|
||||
// @Range: 0 10000
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_ECU_DN", 4, AP_EFI, ecu_dn, 0.),
|
||||
AP_GROUPINFO("_FUEL_DENS", 4, AP_EFI, ecu_fuel_density, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ protected:
|
|||
AP_Float coef1;
|
||||
AP_Float coef2;
|
||||
|
||||
AP_Float ecu_dn;
|
||||
AP_Float ecu_fuel_density;
|
||||
|
||||
EFI_State state;
|
||||
|
||||
|
|
|
|||
|
|
@ -47,8 +47,8 @@ HAL_Semaphore &AP_EFI_Backend::get_sem(void)
|
|||
return frontend.sem;
|
||||
}
|
||||
|
||||
float AP_EFI_Backend::get_ecu_dn(void) const
|
||||
float AP_EFI_Backend::get_ecu_fuel_density(void) const
|
||||
{
|
||||
return frontend.ecu_dn;
|
||||
return frontend.ecu_fuel_density;
|
||||
}
|
||||
#endif // HAL_EFI_ENABLED
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ protected:
|
|||
int8_t get_uavcan_node_id(void) const;
|
||||
float get_coef1(void) const;
|
||||
float get_coef2(void) const;
|
||||
float get_ecu_dn(void) const;
|
||||
float get_ecu_fuel_density(void) const;
|
||||
|
||||
HAL_Semaphore &get_sem(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -16,26 +16,26 @@
|
|||
/*
|
||||
* AP_EFI_Currawong_ECU.cpp
|
||||
*
|
||||
* Author: Reilly Callaway
|
||||
* Author: Reilly Callaway / Currawong Engineering Pty Ltd
|
||||
*/
|
||||
|
||||
#include <AP_PiccoloCAN/piccolo_protocol/ECUPackets.h>
|
||||
#include <AP_Math/definitions.h>
|
||||
#include "AP_EFI_Currawong_ECU.h"
|
||||
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_PiccoloCAN/piccolo_protocol/ECUPackets.h>
|
||||
#include <AP_Math/definitions.h>
|
||||
|
||||
AP_EFI_Currawong_ECU* AP_EFI_Currawong_ECU::singleton;
|
||||
AP_EFI_Currawong_ECU* AP_EFI_Currawong_ECU::_singleton;
|
||||
|
||||
AP_EFI_Currawong_ECU::AP_EFI_Currawong_ECU(AP_EFI &_frontend) :
|
||||
AP_EFI_Backend(_frontend)
|
||||
{
|
||||
singleton = this;
|
||||
|
||||
internal_state.oil_pressure_status = Oil_Pressure_Status::OIL_PRESSURE_STATUS_NOT_SUPPORTED;
|
||||
internal_state.debris_status = Debris_Status::NOT_SUPPORTED;
|
||||
internal_state.misfire_status = Misfire_Status::NOT_SUPPORTED;
|
||||
_singleton = this;
|
||||
// Indicate that temperature and fuel pressure are supported
|
||||
internal_state.fuel_pressure_status = Fuel_Pressure_Status::OK;
|
||||
internal_state.temperature_status = Temperature_Status::OK;
|
||||
}
|
||||
|
||||
void AP_EFI_Currawong_ECU::update()
|
||||
|
|
@ -49,24 +49,61 @@ bool AP_EFI_Currawong_ECU::handle_message(AP_HAL::CANFrame &frame)
|
|||
bool valid = true;
|
||||
|
||||
// There are differences between Ardupilot EFI_State and types/scaling of Piccolo packets.
|
||||
// So we first decode to Piccolo structs, and then store the data we need in EFI_State internal_state with any scaling required.
|
||||
// First decode to Piccolo structs, and then store the data we need in internal_state with any scaling required.
|
||||
|
||||
// Structs to decode Piccolo messages into
|
||||
ECU_TelemetryFast_t telemetryFast;
|
||||
ECU_TelemetrySlow0_t telemetrySlow0;
|
||||
ECU_TelemetrySlow1_t telemetrySlow1;
|
||||
ECU_TelemetrySlow2_t telemetrySlow2;
|
||||
ECU_Errors_t errors;
|
||||
ECU_TelemetryFast_t telemetry_fast;
|
||||
ECU_TelemetrySlow0_t telemetry_slow0;
|
||||
ECU_TelemetrySlow1_t telemetry_slow1;
|
||||
ECU_TelemetrySlow2_t telemetry_slow2;
|
||||
|
||||
// Throw the message at the decoding functions
|
||||
if (decodeECU_TelemetryFastPacketStructure(&frame, &telemetry_fast)) {
|
||||
internal_state.throttle_position_percent = static_cast<uint8_t>(telemetry_fast.throttle);
|
||||
internal_state.engine_load_percent = static_cast<uint8_t>(telemetry_fast.throttle);
|
||||
internal_state.engine_speed_rpm = static_cast<uint32_t>(telemetry_fast.rpm);
|
||||
|
||||
if (internal_state.engine_speed_rpm > 0) {
|
||||
internal_state.engine_state = Engine_State::RUNNING;
|
||||
} else {
|
||||
internal_state.engine_state = Engine_State::STOPPED;
|
||||
}
|
||||
|
||||
if (valid)
|
||||
{
|
||||
// Prevent div by zero
|
||||
if (get_ecu_fuel_density() > 0.01) {
|
||||
internal_state.estimated_consumed_fuel_volume_cm3 = static_cast<float>(telemetry_fast.fuelUsed) / KG_PER_M3_TO_G_PER_CM3(get_ecu_fuel_density());
|
||||
} else {
|
||||
// If no (reasonable) density is provided
|
||||
internal_state.estimated_consumed_fuel_volume_cm3 = 0.;
|
||||
}
|
||||
|
||||
internal_state.general_error = telemetry_fast.ecuStatusBits.errorIndicator;
|
||||
if (!telemetry_fast.ecuStatusBits.enabled) {
|
||||
internal_state.engine_state = Engine_State::STOPPED;
|
||||
}
|
||||
} else if (decodeECU_TelemetrySlow0PacketStructure(&frame, &telemetry_slow0)) {
|
||||
internal_state.intake_manifold_pressure_kpa = telemetry_slow0.map;
|
||||
internal_state.atmospheric_pressure_kpa = telemetry_slow0.baro;
|
||||
internal_state.cylinder_status[0].cylinder_head_temperature = C_TO_KELVIN(telemetry_slow0.cht);
|
||||
} else if (decodeECU_TelemetrySlow1PacketStructure(&frame, &telemetry_slow1)) {
|
||||
internal_state.intake_manifold_temperature = C_TO_KELVIN(telemetry_slow1.mat);
|
||||
internal_state.fuel_pressure = telemetry_slow1.fuelPressure;
|
||||
} else if (decodeECU_TelemetrySlow2PacketStructure(&frame, &telemetry_slow2)) {
|
||||
internal_state.cylinder_status[0].ignition_timing_deg = telemetry_slow2.ignAngle1;
|
||||
if (ENGINE_MAX_CYLINDERS > 1) {
|
||||
internal_state.cylinder_status[1].ignition_timing_deg = telemetry_slow2.ignAngle2;
|
||||
}
|
||||
|
||||
internal_state.fuel_consumption_rate_cm3pm = telemetry_slow2.flowRate / KG_PER_M3_TO_G_PER_CM3(get_ecu_fuel_density());
|
||||
} else {
|
||||
valid = false;
|
||||
}
|
||||
|
||||
if (valid) {
|
||||
internal_state.last_updated_ms = AP_HAL::millis();
|
||||
}
|
||||
|
||||
return valid;
|
||||
}
|
||||
|
||||
#endif // HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
#endif // HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
/*
|
||||
* AP_EFI_Currawong_ECU.h
|
||||
*
|
||||
* Author: Reilly Callaway
|
||||
* Author: Reilly Callaway / Currawong Engineering Pty Ltd
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -25,13 +25,11 @@
|
|||
#include "AP_EFI_Backend.h"
|
||||
|
||||
#ifndef HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
#define HAL_EFI_CURRAWONG_ECU_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && BOARD_FLASH_SIZE > 1024
|
||||
#define HAL_EFI_CURRAWONG_ECU_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && (BOARD_FLASH_SIZE > 1024)
|
||||
#endif
|
||||
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
|
||||
|
||||
|
||||
class AP_EFI_Currawong_ECU : public AP_EFI_Backend {
|
||||
public:
|
||||
AP_EFI_Currawong_ECU(AP_EFI &_frontend);
|
||||
|
|
@ -40,16 +38,16 @@ public:
|
|||
|
||||
static AP_EFI_Currawong_ECU* get_instance(void)
|
||||
{
|
||||
return singleton;
|
||||
return _singleton;
|
||||
}
|
||||
|
||||
private:
|
||||
bool handle_message(AP_HAL::CANFrame &frame);
|
||||
|
||||
static AP_EFI_Currawong_ECU *singleton;
|
||||
static AP_EFI_Currawong_ECU* _singleton;
|
||||
|
||||
friend class AP_PiccoloCAN;
|
||||
};
|
||||
|
||||
#endif // HAL_EFI_NWPWU_ENABLED
|
||||
#endif // HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue