EFI: added efi MavLink class

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 <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
@@ -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()
 {

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

libraries/AP_EFI/AP_EFI_Backend.h

@@ -18,6 +18,7 @@
 #include "AP_EFI_State.h"
 #include <AP_HAL/Semaphores.h>
 #include <AP_Scripting/AP_Scripting_config.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
 
 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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_EFI_config.h"
+
+#if AP_EFI_MAV_ENABLED
+
+#include "AP_EFI_MAV.h"
+#include <AP_Math/AP_Math.h>
+
+//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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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

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