From 48abb57a8afae2865d971032a6d01abc4a4536e6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 12:32:33 +1100 Subject: [PATCH] AP_EFI: added support for Lutan EFI system --- libraries/AP_EFI/AP_EFI.cpp | 6 +- libraries/AP_EFI/AP_EFI.h | 1 + libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp | 95 ++++++++++++++++++++++++ libraries/AP_EFI/AP_EFI_Serial_Lutan.h | 83 +++++++++++++++++++++ libraries/AP_EFI/AP_EFI_Serial_MS.cpp | 2 +- 5 files changed, 185 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp create mode 100644 libraries/AP_EFI/AP_EFI_Serial_Lutan.h diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index 5b7cc12de5..5e84353aa7 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -18,6 +18,7 @@ #if HAL_EFI_ENABLED #include "AP_EFI_Serial_MS.h" +#include "AP_EFI_Serial_Lutan.h" #include "AP_EFI_NWPMU.h" #include @@ -32,7 +33,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 + // @Values: 0:None,1:Serial-MS,2:NWPMU,3:Serial-Lutan // @User: Advanced // @RebootRequired: True AP_GROUPINFO_FLAGS("_TYPE", 1, AP_EFI, type, 0, AP_PARAM_FLAG_ENABLE), @@ -76,6 +77,9 @@ void AP_EFI::init(void) case Type::MegaSquirt: backend = new AP_EFI_Serial_MS(*this); break; + case Type::Lutan: + backend = new AP_EFI_Serial_Lutan(*this); + break; case Type::NWPMU: #if HAL_EFI_NWPWU_ENABLED backend = new AP_EFI_NWPMU(*this); diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index 96cc0020d6..3b2451b281 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -74,6 +74,7 @@ public: NONE = 0, MegaSquirt = 1, NWPMU = 2, + Lutan = 3, }; static AP_EFI *get_singleton(void) { diff --git a/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp b/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp new file mode 100644 index 0000000000..1d0141d6be --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp @@ -0,0 +1,95 @@ +/* + 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 "AP_EFI_Serial_Lutan.h" +#include +#include + +#if HAL_EFI_ENABLED +#include + +extern const AP_HAL::HAL &hal; + +AP_EFI_Serial_Lutan::AP_EFI_Serial_Lutan(AP_EFI &_frontend): + AP_EFI_Backend(_frontend) +{ + port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0); +} + + +void AP_EFI_Serial_Lutan::update() +{ + if (port == nullptr) { + return; + } + + const uint32_t now = AP_HAL::millis(); + if (pkt_nbytes > 0 && now - last_recv_ms > 100) { + pkt_nbytes = 0; + } + const uint32_t n = MIN(sizeof(pkt), port->available()); + if (n > 0) { + last_recv_ms = now; + } + if (n + pkt_nbytes > sizeof(pkt)) { + pkt_nbytes = 0; + } + const ssize_t nread = port->read(&pkt[pkt_nbytes], n); + if (nread <= 0) { + return; + } + pkt_nbytes += nread; + if (pkt_nbytes > 2) { + const uint16_t length = be16toh(data.length); + if (length+6 == pkt_nbytes) { + // got a pkt of right length + const uint32_t crc = be32toh_ptr(&pkt[length+2]); + const uint32_t crc2 = ~crc_crc32(~0U, &pkt[2], length); + if (crc == crc2) { + // valid data + internal_state.last_updated_ms = now; + internal_state.spark_dwell_time_ms = int16_t(be16toh(data.dwell))*0.1; + internal_state.cylinder_status[0].injection_time_ms = be16toh(data.pulseWidth1)*0.00666; + internal_state.engine_speed_rpm = be16toh(data.rpm); + internal_state.atmospheric_pressure_kpa = int16_t(be16toh(data.barometer))*0.1; + internal_state.intake_manifold_pressure_kpa = int16_t(be16toh(data.map))*0.1; + internal_state.intake_manifold_temperature = degF_to_Kelvin(int16_t(be16toh(data.mat))*0.1); + internal_state.coolant_temperature = degF_to_Kelvin(int16_t(be16toh(data.coolant))*0.1); + // CHT is in coolant field + internal_state.cylinder_status[0].cylinder_head_temperature = internal_state.coolant_temperature; + internal_state.throttle_position_percent = int16_t(be16toh(data.tps))*0.1; + copy_to_frontend(); + } + pkt_nbytes = 0; + } + } + if (n == 0 || now - last_request_ms > 200) { + last_request_ms = now; + port->discard_input(); + send_request(); + } +} + +void AP_EFI_Serial_Lutan::send_request(void) +{ + static const uint8_t d[] = { 0, 1, 0x41 }; + const uint32_t crc = ~crc_crc32(~0U, &d[2], sizeof(d)-2); + const uint32_t crc2 = htobe32(crc); + port->write(d, sizeof(d)); + port->write((const uint8_t *)&crc2, sizeof(crc2)); +} + +#endif // HAL_EFI_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Serial_Lutan.h b/libraries/AP_EFI/AP_EFI_Serial_Lutan.h new file mode 100644 index 0000000000..51832d65b6 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Lutan.h @@ -0,0 +1,83 @@ +/* + 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 . + */ +/* + support for Lutan serial EFI + */ +#pragma once + +#include "AP_EFI.h" +#include "AP_EFI_Backend.h" + +class AP_EFI_Serial_Lutan: public AP_EFI_Backend { + +public: + // Constructor with initialization + AP_EFI_Serial_Lutan(AP_EFI &_frontend); + + // Update the state structure + void update() override; + +private: + AP_HAL::UARTDriver *port; + void send_request(void); + + union { + struct PACKED { + // fields from channels in LUTAN-MS2.ini + uint16_t length; + uint8_t flags; + uint16_t seconds; + uint16_t pulseWidth1; // ms, scale 0.000666 + uint16_t pulseWidth2; // ms, scale 0.000666 + uint16_t rpm; + int16_t advance; // deg, scale 0.1 + uint8_t squirt_flags; + uint8_t engine_flags; + uint8_t afrtgt1; + uint8_t afrtgt2; + uint8_t wbo2_en1; + uint8_t wbo2_en2; + int16_t barometer; // kPa, scale 0.1 + int16_t map; // kPa, scale 0.1 + int16_t mat; // degF, scale 0.1 + int16_t coolant; // degF, scale 0.1 + int16_t tps; // %, scale 0.1 + int16_t batteryVoltage; // V, scale 0.1 + int16_t afr1; // scale 0.1 + int16_t afr2; // scale 0.1 + uint16_t knock; // %, scale 0.1 + int16_t egoCorrection1; // %, scale 0.1 + int16_t egoCorrection2; // %, scale 0.1 + int16_t airCorrection; // %, scale 0.1 + int16_t warmupEnrich; // %, scale 0.1 + int16_t accelEnrich; // ms, scale 0.1 + int16_t tpsfuelcut; // % + int16_t baroCorrection; // %, scale 0.1 + int16_t gammaEnrich; // % + int16_t veCurr1; // %, scale 0.1 + int16_t veCurr2; // %, scale 0.1 + int16_t iacstep; + int16_t idleDC; // scale 0.392 + int16_t coldAdvDeg; // deg, scale 0.1 + int16_t TPSdot; // %/s, scale 0.1 + int16_t MAPdot; // kPa/s + int16_t dwell; // ms, scale 0.0666 + } data; + uint8_t pkt[400]; + }; + uint16_t pkt_nbytes; + uint32_t last_request_ms; + uint32_t last_recv_ms; +}; diff --git a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp index 4f70b2466c..f5820fa272 100644 --- a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp +++ b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp @@ -25,7 +25,7 @@ AP_EFI_Serial_MS::AP_EFI_Serial_MS(AP_EFI &_frontend): AP_EFI_Backend(_frontend) { internal_state.estimated_consumed_fuel_volume_cm3 = 0; // Just to be sure - port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI_MS, 0); + port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0); }