Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp

391 lines
12 KiB
C++
Raw Permalink Blame History

/*
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
Reference in New Issue View Git Blame Copy Permalink