mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
e32d3ceaf7
There is no crankshaft sensor status reported by this EFI. This line is misleading and should be removed. The sensor health bitmask is already logged elsewhere.
391 lines
12 KiB
C++
391 lines
12 KiB
C++
/*
|
|
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_SERIAL_HIRTH_ENABLED
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_EFI/AP_EFI_Serial_Hirth.h>
|
|
#include <AP_SerialManager/AP_SerialManager.h>
|
|
#include <SRV_Channel/SRV_Channel.h>
|
|
#include <AP_ICEngine/AP_ICEngine.h>
|
|
#include <AP_Math/definitions.h>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
#define HIRTH_MAX_PKT_SIZE 100
|
|
#define HIRTH_MAX_RAW_PKT_SIZE 103
|
|
|
|
#define SERIAL_WAIT_TIMEOUT_MS 100
|
|
|
|
#define ENGINE_RUNNING 4
|
|
#define THROTTLE_POSITION_FACTOR 10
|
|
#define INJECTION_TIME_RESOLUTION 0.8
|
|
#define THROTTLE_POSITION_RESOLUTION 0.1
|
|
#define VOLTAGE_RESOLUTION 0.0049 /* 5/1024 */
|
|
#define ADC_CALIBRATION (5.0/1024.0)
|
|
#define MAP_HPA_PER_VOLT_FACTOR 248.2673
|
|
#define HPA_TO_KPA 0.1
|
|
#define TPS_SCALE 0.70
|
|
|
|
// request/response status constants
|
|
#define QUANTITY_REQUEST_STATUS 0x03
|
|
#define QUANTITY_SET_VALUE 0x17
|
|
#define CODE_REQUEST_STATUS_1 0x04
|
|
#define CODE_REQUEST_STATUS_2 0x0B
|
|
#define CODE_REQUEST_STATUS_3 0x0D
|
|
#define CODE_SET_VALUE 0xC9
|
|
#define CHECKSUM_REQUEST_STATUS_1 0xF9
|
|
#define CHECKSUM_REQUEST_STATUS_2 0xF2
|
|
#define CHECKSUM_REQUEST_STATUS_3 0xF0
|
|
#define QUANTITY_RESPONSE_STATUS_1 0x57
|
|
#define QUANTITY_RESPONSE_STATUS_2 0x65
|
|
#define QUANTITY_RESPONSE_STATUS_3 0x67
|
|
#define QUANTITY_ACK_SET_VALUES 0x03
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/**
|
|
* @brief Constructor with port initialization
|
|
*
|
|
* @param _frontend
|
|
*/
|
|
AP_EFI_Serial_Hirth::AP_EFI_Serial_Hirth(AP_EFI &_frontend) :
|
|
AP_EFI_Backend(_frontend)
|
|
{
|
|
port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0);
|
|
set_default_coef1(1.0);
|
|
}
|
|
|
|
/**
|
|
* @brief checks for response from or makes requests to Hirth ECU periodically
|
|
*
|
|
*/
|
|
void AP_EFI_Serial_Hirth::update()
|
|
{
|
|
if (port == nullptr) {
|
|
return;
|
|
}
|
|
|
|
// parse response from Hirth
|
|
check_response();
|
|
|
|
// send request
|
|
send_request();
|
|
}
|
|
|
|
/**
|
|
* @brief Checks if required bytes are available and proceeds with parsing
|
|
*
|
|
*/
|
|
void AP_EFI_Serial_Hirth::check_response()
|
|
{
|
|
const uint32_t now = AP_HAL::millis();
|
|
|
|
// waiting for response
|
|
if (!waiting_response) {
|
|
return;
|
|
}
|
|
|
|
const uint32_t num_bytes = port->available();
|
|
|
|
// if already requested
|
|
if (num_bytes >= expected_bytes) {
|
|
// read data from buffer
|
|
uint8_t computed_checksum = 0;
|
|
computed_checksum += res_data.quantity = port->read();
|
|
computed_checksum += res_data.code = port->read();
|
|
|
|
if (res_data.code == requested_code) {
|
|
for (int i = 0; i < (res_data.quantity - QUANTITY_REQUEST_STATUS); i++) {
|
|
computed_checksum += raw_data[i] = port->read();
|
|
}
|
|
}
|
|
|
|
res_data.checksum = port->read();
|
|
if (res_data.checksum != (256 - computed_checksum)) {
|
|
crc_fail_cnt++;
|
|
port->discard_input();
|
|
} else {
|
|
uptime = now - last_packet_ms;
|
|
last_packet_ms = now;
|
|
internal_state.last_updated_ms = now;
|
|
decode_data();
|
|
copy_to_frontend();
|
|
port->discard_input();
|
|
}
|
|
|
|
waiting_response = false;
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
log_status();
|
|
#endif
|
|
}
|
|
|
|
// reset request if no response for SERIAL_WAIT_TIMEOUT_MS
|
|
if (waiting_response &&
|
|
now - last_request_ms > SERIAL_WAIT_TIMEOUT_MS) {
|
|
waiting_response = false;
|
|
last_request_ms = now;
|
|
|
|
port->discard_input();
|
|
ack_fail_cnt++;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Send Throttle and Telemetry requests to Hirth
|
|
*
|
|
*/
|
|
void AP_EFI_Serial_Hirth::send_request()
|
|
{
|
|
if (waiting_response) {
|
|
return;
|
|
}
|
|
|
|
const uint32_t now = AP_HAL::millis();
|
|
bool request_was_sent;
|
|
|
|
// get new throttle value
|
|
const uint16_t new_throttle = (uint16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
|
|
|
// check for change or timeout for throttle value
|
|
if ((new_throttle != last_throttle) || (now - last_req_send_throttle_ms > 500)) {
|
|
// send new throttle value, only when ARMED
|
|
bool allow_throttle = hal.util->get_soft_armed();
|
|
if (!allow_throttle) {
|
|
#if AP_ICENGINE_ENABLED
|
|
const auto *ice = AP::ice();
|
|
if (ice != nullptr) {
|
|
allow_throttle = ice->allow_throttle_while_disarmed();
|
|
}
|
|
#endif // AP_ICENGINE_ENABLED
|
|
}
|
|
if (allow_throttle) {
|
|
request_was_sent = send_target_values(new_throttle);
|
|
} else {
|
|
request_was_sent = send_target_values(0);
|
|
}
|
|
|
|
last_throttle = new_throttle;
|
|
last_req_send_throttle_ms = now;
|
|
} else {
|
|
// request Status request at the driver update rate if no throttle commands
|
|
request_was_sent = send_request_status();
|
|
}
|
|
|
|
if (request_was_sent) {
|
|
waiting_response = true;
|
|
last_request_ms = now;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief sends the new throttle command to Hirth ECU
|
|
*
|
|
* @param thr - new throttle value given by SRV_Channel::k_throttle
|
|
* @return true - if success
|
|
* @return false - currently not implemented
|
|
*/
|
|
bool AP_EFI_Serial_Hirth::send_target_values(uint16_t thr)
|
|
{
|
|
uint8_t computed_checksum = 0;
|
|
throttle_to_hirth = 0;
|
|
|
|
// clear buffer
|
|
memset(raw_data, 0, ARRAY_SIZE(raw_data));
|
|
|
|
#if AP_EFI_THROTTLE_LINEARISATION_ENABLED
|
|
// linearise throttle input
|
|
thr = linearise_throttle(thr);
|
|
#endif
|
|
|
|
throttle_to_hirth = thr * THROTTLE_POSITION_FACTOR;
|
|
|
|
uint8_t idx = 0;
|
|
|
|
// set Quantity + Code + "20 bytes of records to set" + Checksum
|
|
computed_checksum += raw_data[idx++] = QUANTITY_SET_VALUE;
|
|
computed_checksum += raw_data[idx++] = requested_code = CODE_SET_VALUE;
|
|
computed_checksum += raw_data[idx++] = throttle_to_hirth & 0xFF;
|
|
computed_checksum += raw_data[idx++] = (throttle_to_hirth >> 8) & 0xFF;
|
|
|
|
// checksum calculation
|
|
raw_data[QUANTITY_SET_VALUE - 1] = (256 - computed_checksum);
|
|
|
|
expected_bytes = QUANTITY_ACK_SET_VALUES;
|
|
// write data
|
|
port->write(raw_data, QUANTITY_SET_VALUE);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief cyclically sends different Status requests to Hirth ECU
|
|
*
|
|
* @return true - when successful
|
|
* @return false - not implemented
|
|
*/
|
|
bool AP_EFI_Serial_Hirth::send_request_status() {
|
|
|
|
uint8_t requested_quantity;
|
|
uint8_t requested_checksum;
|
|
|
|
switch (requested_code)
|
|
{
|
|
case CODE_REQUEST_STATUS_1:
|
|
requested_quantity = QUANTITY_REQUEST_STATUS;
|
|
requested_code = CODE_REQUEST_STATUS_2;
|
|
requested_checksum = CHECKSUM_REQUEST_STATUS_2;
|
|
expected_bytes = QUANTITY_RESPONSE_STATUS_2;
|
|
break;
|
|
case CODE_REQUEST_STATUS_2:
|
|
requested_quantity = QUANTITY_REQUEST_STATUS;
|
|
requested_code = CODE_REQUEST_STATUS_3;
|
|
requested_checksum = CHECKSUM_REQUEST_STATUS_3;
|
|
expected_bytes = QUANTITY_RESPONSE_STATUS_3;
|
|
break;
|
|
case CODE_REQUEST_STATUS_3:
|
|
requested_quantity = QUANTITY_REQUEST_STATUS;
|
|
requested_code = CODE_REQUEST_STATUS_1;
|
|
requested_checksum = CHECKSUM_REQUEST_STATUS_1;
|
|
expected_bytes = QUANTITY_RESPONSE_STATUS_1;
|
|
break;
|
|
default:
|
|
requested_quantity = QUANTITY_REQUEST_STATUS;
|
|
requested_code = CODE_REQUEST_STATUS_1;
|
|
requested_checksum = CHECKSUM_REQUEST_STATUS_1;
|
|
expected_bytes = QUANTITY_RESPONSE_STATUS_1;
|
|
break;
|
|
}
|
|
raw_data[0] = requested_quantity;
|
|
raw_data[1] = requested_code;
|
|
raw_data[2] = requested_checksum;
|
|
|
|
port->write(raw_data, QUANTITY_REQUEST_STATUS);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief parses the response from Hirth ECU and updates the internal state instance
|
|
*
|
|
*/
|
|
void AP_EFI_Serial_Hirth::decode_data()
|
|
{
|
|
const uint32_t now = AP_HAL::millis();
|
|
|
|
switch (res_data.code) {
|
|
case CODE_REQUEST_STATUS_1: {
|
|
struct Record1 *record1 = (Record1*)raw_data;
|
|
|
|
internal_state.engine_speed_rpm = record1->rpm;
|
|
internal_state.throttle_out = record1->throttle;
|
|
|
|
// EFI2 log
|
|
internal_state.engine_state = (Engine_State)record1->engine_status;
|
|
|
|
// ECYL log
|
|
internal_state.cylinder_status.injection_time_ms = record1->injection_time * INJECTION_TIME_RESOLUTION;
|
|
internal_state.cylinder_status.ignition_timing_deg = record1->ignition_angle;
|
|
|
|
// EFI3 log
|
|
internal_state.ignition_voltage = record1->battery_voltage * VOLTAGE_RESOLUTION;
|
|
|
|
sensor_status = record1->sensor_ok;
|
|
|
|
// resusing mavlink variables as required for Hirth
|
|
// add in ADC voltage of MAP sensor > convert to MAP in kPa
|
|
internal_state.intake_manifold_pressure_kpa = record1->voltage_int_air_pressure * (ADC_CALIBRATION * MAP_HPA_PER_VOLT_FACTOR * HPA_TO_KPA);
|
|
internal_state.intake_manifold_temperature = C_TO_KELVIN(record1->air_temperature);
|
|
break;
|
|
}
|
|
|
|
case CODE_REQUEST_STATUS_2: {
|
|
struct Record2 *record2 = (Record2*)raw_data;
|
|
|
|
// EFI log
|
|
const float fuel_consumption_rate_lph = record2->fuel_consumption * 0.1;
|
|
|
|
internal_state.fuel_consumption_rate_cm3pm = (fuel_consumption_rate_lph * 1000.0 / 60.0) * get_coef1();
|
|
|
|
if (last_fuel_integration_ms != 0) {
|
|
// estimated_consumed_fuel_volume_cm3 is in cm3/pm
|
|
const float dt_minutes = (now - last_fuel_integration_ms)*(0.001/60);
|
|
internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * dt_minutes;
|
|
}
|
|
last_fuel_integration_ms = now;
|
|
|
|
internal_state.throttle_position_percent = record2->throttle_percent_times_10 * 0.1;
|
|
break;
|
|
}
|
|
|
|
case CODE_REQUEST_STATUS_3: {
|
|
struct Record3 *record3 = (Record3*)raw_data;
|
|
|
|
// EFI3 Log
|
|
error_excess_temperature = record3->error_excess_temperature_bitfield;
|
|
|
|
// ECYL log
|
|
internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(record3->excess_temperature_1);
|
|
internal_state.cylinder_status.cylinder_head_temperature2 = C_TO_KELVIN(record3->excess_temperature_2);
|
|
internal_state.cylinder_status.exhaust_gas_temperature = C_TO_KELVIN(record3->excess_temperature_3);
|
|
internal_state.cylinder_status.exhaust_gas_temperature2 = C_TO_KELVIN(record3->excess_temperature_4);
|
|
break;
|
|
}
|
|
|
|
// case CODE_SET_VALUE:
|
|
// // Do nothing for now
|
|
// break;
|
|
}
|
|
}
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
void AP_EFI_Serial_Hirth::log_status(void)
|
|
{
|
|
// @LoggerMessage: EFIS
|
|
// @Description: Electronic Fuel Injection data - Hirth specific Status information
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: EET: Error Excess Temperature Bitfield
|
|
// @FieldBitmaskEnum: EET: AP_EFI_Serial_Hirth:::Error_Excess_Temp_Bitfield
|
|
// @Field: FLAG: Sensor Status Bitfield
|
|
// @FieldBitmaskEnum: FLAG: AP_EFI_Serial_Hirth:::Sensor_Status_Bitfield
|
|
// @Field: CRF: CRC failure count
|
|
// @Field: AKF: ACK failure count
|
|
// @Field: Up: Uptime between 2 messages
|
|
// @Field: ThO: Throttle output as received by the engine
|
|
// @Field: ThM: Modified throttle_to_hirth output sent to the engine
|
|
AP::logger().WriteStreaming("EFIS",
|
|
"TimeUS,EET,FLAG,CRF,AKF,Up,ThO,ThM",
|
|
"s-------",
|
|
"F-------",
|
|
"QHBIIIfH",
|
|
AP_HAL::micros64(),
|
|
uint16_t(error_excess_temperature),
|
|
uint8_t(sensor_status),
|
|
uint32_t(crc_fail_cnt),
|
|
uint32_t(ack_fail_cnt),
|
|
uint32_t(uptime),
|
|
float(internal_state.throttle_out),
|
|
uint16_t(throttle_to_hirth));
|
|
}
|
|
#endif // HAL_LOGGING_ENABLED
|
|
|
|
#endif // AP_EFI_SERIAL_HIRTH_ENABLED
|