Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_EFI: Add Currawong ECU packet decoding

This commit is contained in:
Reilly Callaway 2022-03-15 12:14:28 +11:00 committed by Randy Mackay
parent 47776cfa0a
commit c4699d0aba
6 changed files with 67 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_EFI/AP_EFI.cpp
View File

@ -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
};

2
libraries/AP_EFI/AP_EFI.h
View File

@ -99,7 +99,7 @@ protected:
AP_Float coef1;
AP_Float coef2;
AP_Float ecu_dn;
AP_Float ecu_fuel_density;
EFI_State state;

4
libraries/AP_EFI/AP_EFI_Backend.cpp
View File

@ -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

2
libraries/AP_EFI/AP_EFI_Backend.h
View File

@ -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);

75
libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp
View File

@ -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

12
libraries/AP_EFI/AP_EFI_Currawong_ECU.h
View File

@ -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