From afc019e1eaa116adf68ff0de69902578521d14d6 Mon Sep 17 00:00:00 2001 From: Max-001 <80035972+Max-001@users.noreply.github.com> Date: Tue, 27 Jun 2023 10:02:59 +0200 Subject: [PATCH] EFI: added efi MavLink class --- libraries/AP_EFI/AP_EFI.cpp | 14 ++++++- libraries/AP_EFI/AP_EFI.h | 3 ++ libraries/AP_EFI/AP_EFI_Backend.h | 3 ++ libraries/AP_EFI/AP_EFI_MAV.cpp | 58 ++++++++++++++++++++++++++++ libraries/AP_EFI/AP_EFI_MAV.h | 35 +++++++++++++++++ libraries/AP_EFI/AP_EFI_config.h | 4 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 9 +++++ 7 files changed, 125 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_EFI/AP_EFI_MAV.cpp create mode 100644 libraries/AP_EFI/AP_EFI_MAV.h diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index 72e7897df8..b52756d09f 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -23,6 +23,7 @@ #include "AP_EFI_DroneCAN.h" #include "AP_EFI_Currawong_ECU.h" #include "AP_EFI_Scripting.h" +#include "AP_EFI_MAV.h" #include #include @@ -38,7 +39,7 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _TYPE // @DisplayName: EFI communication type // @Description: What method of communication is used for EFI #1 - // @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan,5:DroneCAN,6:Currawong-ECU,7:Scripting + // @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan,5:DroneCAN,6:Currawong-ECU,7:Scripting,9:MAV // @User: Advanced // @RebootRequired: True AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE), @@ -116,6 +117,11 @@ void AP_EFI::init(void) case Type::SCRIPTING: backend = new AP_EFI_Scripting(*this); break; +#endif +#if AP_EFI_MAV_ENABLED + case Type::MAV: + backend = new AP_EFI_MAV(*this); + break; #endif default: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unknown EFI type"); @@ -302,6 +308,12 @@ void AP_EFI::get_state(EFI_State &_state) _state = state; } +void AP_EFI::handle_EFI_message(const mavlink_message_t &msg) { + if (backend != nullptr) { + backend->handle_EFI_message(msg); + } +} + namespace AP { AP_EFI *EFI() { diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index 756273ca6b..36db32e07b 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -97,6 +97,7 @@ public: SCRIPTING = 7, #endif // Hirth = 8 /* Reserved for future implementation */ + MAV = 9, }; static AP_EFI *get_singleton(void) { @@ -110,6 +111,8 @@ public: AP_EFI_Backend* get_backend(uint8_t idx) { return idx==0?backend:nullptr; } #endif + void handle_EFI_message(const mavlink_message_t &msg); + protected: // Back end Parameters diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h index 16029a9496..dea30f8c44 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -18,6 +18,7 @@ #include "AP_EFI_State.h" #include #include +#include class AP_EFI; //forward declaration @@ -32,6 +33,8 @@ public: // Update the state structure virtual void update() = 0; + virtual void handle_EFI_message(const mavlink_message_t &msg) {}; + #if AP_SCRIPTING_ENABLED virtual bool handle_scripting(const EFI_State &efi_state) { return false; } #endif diff --git a/libraries/AP_EFI/AP_EFI_MAV.cpp b/libraries/AP_EFI/AP_EFI_MAV.cpp new file mode 100644 index 0000000000..5ae7121810 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_MAV.cpp @@ -0,0 +1,58 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_EFI_config.h" + +#if AP_EFI_MAV_ENABLED + +#include "AP_EFI_MAV.h" +#include + +//Called from frontend to update with the readings received by handler +void AP_EFI_MAV::update() +{ + if (receivedNewData) { + copy_to_frontend(); + receivedNewData = false; + } +} + +//Decode MavLink message +void AP_EFI_MAV::handle_EFI_message(const mavlink_message_t &msg) +{ + mavlink_efi_status_t state; + mavlink_msg_efi_status_decode(&msg, &state); + + internal_state.ecu_index = state.ecu_index; + internal_state.engine_speed_rpm = state.rpm; + internal_state.estimated_consumed_fuel_volume_cm3 = state.fuel_consumed; + internal_state.fuel_consumption_rate_cm3pm = state.fuel_flow; + internal_state.engine_load_percent = state.engine_load; + internal_state.throttle_position_percent = state.throttle_position; + internal_state.spark_dwell_time_ms = state.spark_dwell_time; + internal_state.atmospheric_pressure_kpa = state.barometric_pressure; + internal_state.intake_manifold_pressure_kpa = state.intake_manifold_pressure; + internal_state.intake_manifold_temperature = C_TO_KELVIN(state.intake_manifold_temperature); + internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(state.cylinder_head_temperature); + internal_state.cylinder_status.ignition_timing_deg = state.ignition_timing; + internal_state.cylinder_status.injection_time_ms = state.injection_time; + internal_state.cylinder_status.exhaust_gas_temperature = C_TO_KELVIN(state.exhaust_gas_temperature); + internal_state.throttle_out = state.throttle_out; + internal_state.pt_compensation = state.pt_compensation; + //internal_state.??? = state.health; + internal_state.ignition_voltage = state.ignition_voltage; + + receivedNewData = true; +} + +#endif // AP_EFI_MAV_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_MAV.h b/libraries/AP_EFI/AP_EFI_MAV.h new file mode 100644 index 0000000000..f3e5c1f1d9 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_MAV.h @@ -0,0 +1,35 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "AP_EFI_config.h" + +#if AP_EFI_MAV_ENABLED + +#include "AP_EFI.h" +#include "AP_EFI_Backend.h" + +class AP_EFI_MAV : public AP_EFI_Backend { +public: + using AP_EFI_Backend::AP_EFI_Backend; + + void update() override; + + void handle_EFI_message(const mavlink_message_t &msg) override; + +private: + bool receivedNewData; +}; + +#endif // AP_EFI_MAV_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_config.h b/libraries/AP_EFI/AP_EFI_config.h index 65e5ee31ce..00653cc38c 100644 --- a/libraries/AP_EFI/AP_EFI_config.h +++ b/libraries/AP_EFI/AP_EFI_config.h @@ -19,6 +19,10 @@ #define AP_EFI_DRONECAN_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS #endif +#ifndef AP_EFI_MAV_ENABLED +#define AP_EFI_MAV_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_EFI_NWPWU_ENABLED #define AP_EFI_NWPWU_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED && HAL_MAX_CAN_PROTOCOL_DRIVERS #endif diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 83ca5e7469..d8997dc48b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4094,6 +4094,15 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) AP_CheckFirmware::handle_msg(chan, msg); break; #endif + +#if AP_EFI_MAV_ENABLED + case MAVLINK_MSG_ID_EFI_STATUS: + AP_EFI *efi = AP::EFI(); + if (efi) { + efi->handle_EFI_message(msg); + } + break; +#endif } }