AP_EFI: Add NWPMU CAN driver

libraries/AP_EFI/AP_EFI.cpp

@@ -18,8 +18,13 @@
 #if EFI_ENABLED
 
 #include "AP_EFI_Serial_MS.h"
+#include "AP_EFI_NWPMU.h"
 #include <AP_Logger/AP_Logger.h>
 
+#if HAL_MAX_CAN_PROTOCOL_DRIVERS
+#include <AP_CANManager/AP_CANManager.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
@@ -27,7 +32,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
+    // @Values: 0:None,1:Serial-MS,2:NWPMU
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE),
@@ -68,8 +73,17 @@ void AP_EFI::init(void)
         return;
     }
     // Check for MegaSquirt Serial EFI
-    if (type == EFI_COMMUNICATION_TYPE_SERIAL_MS) {
+    switch (type) {
-        backend = new AP_EFI_Serial_MS(*this);
+        case EFI_COMMUNICATION_TYPE_NONE:
+            break;
+        case EFI_COMMUNICATION_TYPE_SERIAL_MS:
+            backend = new AP_EFI_Serial_MS(*this);
+            break;
+        case EFI_COMMUNICATION_TYPE_NWPMU:
+#if HAL_EFI_NWPWU_ENABLED
+            backend = new AP_EFI_NWPMU(*this);
+#endif
+            break;
     }
 }
 

libraries/AP_EFI/AP_EFI.h

@@ -70,7 +70,8 @@ public:
     // Backend driver types
     enum EFI_Communication_Type {
         EFI_COMMUNICATION_TYPE_NONE      = 0,
-        EFI_COMMUNICATION_TYPE_SERIAL_MS = 1
+        EFI_COMMUNICATION_TYPE_SERIAL_MS = 1,
+        EFI_COMMUNICATION_TYPE_NWPMU     = 2,
     };
 
     static AP_EFI *get_singleton(void) {

libraries/AP_EFI/AP_EFI_NWPMU.cpp

@@ -0,0 +1,127 @@
+/*
+   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_HAL/AP_HAL.h>
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/utility/sparse-endian.h>
+#include <AP_Math/AP_Math.h>
+
+#include "AP_EFI_NWPMU.h"
+
+#if HAL_EFI_NWPWU_ENABLED
+
+extern const AP_HAL::HAL& hal;
+
+AP_EFI_NWPMU::AP_EFI_NWPMU(AP_EFI &_frontend) :
+    CANSensor("NWPMU", AP_CANManager::Driver_Type_EFI_NWPMU),
+    AP_EFI_Backend(_frontend)
+{
+}
+
+void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame)
+{
+    const uint32_t id =  frame.id & AP_HAL::CANFrame::MaskExtID;
+
+    WITH_SEMAPHORE(sem);
+
+    switch ((NWPMU_ID)id) {
+    case NWPMU_ID::ECU_1: {
+        internal_state.last_updated_ms = AP_HAL::millis();
+        struct ecu_1 *data = (struct ecu_1 *)frame.data;
+        internal_state.engine_speed_rpm = data->rpm;
+        internal_state.throttle_position_percent = data->tps * 0.1f;
+        internal_state.cylinder_status[0].ignition_timing_deg = data->ignition_angle * 0.1f;
+        break;
+    }
+
+    case NWPMU_ID::ECU_2: {
+        internal_state.last_updated_ms = AP_HAL::millis();
+        struct ecu_2 *data = (struct ecu_2 *)frame.data;
+        switch ((NWPMU_PRESSURE_TYPE)data->pressure_type) {
+        case NWPMU_PRESSURE_TYPE::kPa:
+            internal_state.atmospheric_pressure_kpa = data->baro * 0.01f;
+            internal_state.intake_manifold_pressure_kpa = data->baro * 0.01f;
+            break;
+        case NWPMU_PRESSURE_TYPE::psi:
+            internal_state.atmospheric_pressure_kpa = data->baro * 0.0689476f;
+            internal_state.intake_manifold_pressure_kpa = data->baro * 0.0689476f;
+            break;
+        default:
+            break;
+        }
+        internal_state.cylinder_status[0].lambda_coefficient = data->lambda * 0.01f;
+        break;
+    }
+
+    case NWPMU_ID::ECU_4: {
+        internal_state.last_updated_ms = AP_HAL::millis();
+        struct ecu_4 *data = (struct ecu_4 *)frame.data;
+        // remap the analog input for fuel pressure, 0.5 V == 0 PSI, 4.5V == 100 PSI
+        internal_state.fuel_pressure = linear_interpolate(0, 689.476,
+                                                          data->analog_fuel_pres * 0.001,
+                                                          0.5f,4.5f);
+        break;
+    }
+
+    case NWPMU_ID::ECU_5: {
+        internal_state.last_updated_ms = AP_HAL::millis();
+        struct ecu_5 *data = (struct ecu_5 *)frame.data;
+        switch((NWPMU_TEMPERATURE_TYPE)data->temp_type) {
+        case NWPMU_TEMPERATURE_TYPE::C:
+            internal_state.coolant_temperature = data->coolant_temp * 0.1f + C_TO_KELVIN;
+            internal_state.cylinder_status[0].cylinder_head_temperature = data->coolant_temp * 0.1f + C_TO_KELVIN;
+            break;
+        case NWPMU_TEMPERATURE_TYPE::F:
+            internal_state.coolant_temperature = ((data->coolant_temp * 0.1f) - 32 * 5/9) + C_TO_KELVIN;
+            internal_state.cylinder_status[0].cylinder_head_temperature = ((data->coolant_temp * 0.1f) - 32 * 5/9) + C_TO_KELVIN;
+            break;
+        default:
+            break;
+        }
+        break;
+    }
+
+    case NWPMU_ID::ECU_6: {
+        internal_state.last_updated_ms = AP_HAL::millis();
+        struct ecu_6 *data = (struct ecu_6 *)frame.data;
+        if (!_emitted_version && (AP_HAL::millis() > 10000)) { // don't emit a version early in the boot process
+            gcs().send_text(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d",
+                            data->firmware_major,
+                            data->firmware_minor,
+                            data->firmware_build);
+            _emitted_version = true;
+        }
+        break;
+    }
+    case NWPMU_ID::GCU:
+    case NWPMU_ID::ECU_3:
+    case NWPMU_ID::ECU_7:
+    case NWPMU_ID::ECU_8:
+    case NWPMU_ID::ECU_9:
+    case NWPMU_ID::ECU_10:
+    case NWPMU_ID::ECU_11:
+    case NWPMU_ID::ECU_12:
+        break;
+    }
+}
+
+void AP_EFI_NWPMU::update()
+{
+    // copy the data to the front end
+    copy_to_frontend();
+}
+
+#endif // HAL_EFI_NWPWU_ENABLED

libraries/AP_EFI/AP_EFI_NWPMU.h

@@ -0,0 +1,115 @@
+/*
+   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/>.
+ */
+/*
+ * AP_EFI_NWPMU.h
+ *
+ *      Author: Michael Du Breuil
+ */
+ 
+#pragma once
+
+#include "AP_EFI.h"
+#include "AP_EFI_Backend.h"
+#include <AP_CANManager/AP_CANSensor.h>
+
+#ifndef HAL_EFI_NWPWU_ENABLED
+#define HAL_EFI_NWPWU_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && BOARD_FLASH_SIZE > 1024
+#endif
+
+#if HAL_EFI_NWPWU_ENABLED
+
+class AP_EFI_NWPMU : public CANSensor, public AP_EFI_Backend {
+public:
+    AP_EFI_NWPMU(AP_EFI &_frontend);
+    
+    void update() override;
+
+private:
+    void handle_frame(AP_HAL::CANFrame &frame) override;
+
+    bool _emitted_version;
+
+    enum class NWPMU_ID {
+        GCU    = 0x0006C000, // output voltage and current consumption
+        ECU_1  = 0x0CFFF048, // rpm, tps, fuel open, ignition angle
+        ECU_2  = 0x0CFFF148, // baro, MAP, lambda, pressure type
+        ECU_3  = 0x0CFFF248, // analog input 1-4
+        ECU_4  = 0x0CFFF348, // analog input 5-8
+        ECU_5  = 0x0CFFF548, // battery voltage, air temp, coolant temp, temp type
+        ECU_6  = 0x0CFFF648, // analog input 5, 7, version
+        ECU_7  = 0x0CFFF848, // measured lambda 1-2, target lambda
+        ECU_8  = 0x0CFFF948, // PWM ACHT, Blank, Fuel
+        ECU_9  = 0x0CFFFB48, // ignition compensation, cut precent
+        ECU_10 = 0x0CFFFD48, // Fuel comp (accel, starting, air temp, coolant temp)
+        ECU_11 = 0x0CFFFE48, // Fuel comp (baro, MAP)
+        ECU_12 = 0x0CFFD048, // Ignition comp (air temp, cooland temp, MAP)
+    };
+
+    enum class NWPMU_PRESSURE_TYPE {
+        psi = 0,
+        kPa = 1,
+    };
+
+    enum class NWPMU_TEMPERATURE_TYPE {
+        F = 0,
+        C = 1,
+    };
+
+    struct ecu_1 {
+        uint16_t rpm;
+        uint16_t tps;
+        uint16_t fuel_open_time; // 0.01 per bit (ms)
+        uint16_t ignition_angle; // 0.01 per bit (deg)
+    };
+
+    struct ecu_2 {
+        uint16_t baro;   // 0.01 per bit
+        uint16_t map;    // 0.01 per bit
+        uint16_t lambda; // 0.01 per bit
+        uint8_t pressure_type; // bit 1 is set if kPa, otherwise is PSI
+    };
+
+    struct ecu_3 {
+        uint16_t analog_1; // 0.001 per bit (v)
+        uint16_t analog_2; // 0.001 per bit (v)
+        uint16_t analog_afr; // 0.001 per bit (v)
+        uint16_t analog_blank; // 0.001 per bit (v)
+    };
+
+    struct ecu_4 {
+        uint16_t analog_gen_temp;   // 0.001 per bit
+        uint16_t analog_fuel_pres;  // 0.001 per bit
+        uint16_t analog_fuel_level; // 0.001 per bit
+        uint16_t analog_baro_pres;  // 0.001 per bit
+    };
+
+    struct ecu_5 {
+        uint16_t batt_volt; // 0.01 per bit (v)
+        uint16_t air_temp;  // 0.1 per bit
+        uint16_t coolant_temp; // 0.1 per bit
+        uint8_t  temp_type; // 0 F, 1 C
+    };
+
+    struct ecu_6 {
+        uint16_t analog_5; // 0.1 per bit
+        uint16_t analog_7; // 0.1 per bit
+        uint8_t firmware_major;
+        uint8_t firmware_minor;
+        uint8_t firmware_build;
+    };
+};
+
+#endif // HAL_EFI_NWPWU_ENABLED
+