diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index e8b741821f..294d09bae2 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/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 +#if HAL_MAX_CAN_PROTOCOL_DRIVERS +#include +#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) { - backend = new AP_EFI_Serial_MS(*this); + switch (type) { + 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; } } diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index 22329768e9..d01913a0ad 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/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) { diff --git a/libraries/AP_EFI/AP_EFI_NWPMU.cpp b/libraries/AP_EFI/AP_EFI_NWPMU.cpp new file mode 100644 index 0000000000..df541a8463 --- /dev/null +++ b/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 . + */ + +#include + +#include +#include +#include + +#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 diff --git a/libraries/AP_EFI/AP_EFI_NWPMU.h b/libraries/AP_EFI/AP_EFI_NWPMU.h new file mode 100644 index 0000000000..ce3677ccf9 --- /dev/null +++ b/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 . + */ +/* + * AP_EFI_NWPMU.h + * + * Author: Michael Du Breuil + */ + +#pragma once + +#include "AP_EFI.h" +#include "AP_EFI_Backend.h" +#include + +#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 +