diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 5cf2e7fd31..ba93746080 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -185,6 +185,17 @@ void AP_Periph_FW::init() adsb_init(); #endif +#ifdef HAL_PERIPH_ENABLE_EFI + if (efi.enabled() && g.efi_port >= 0) { + auto *uart = hal.serial(g.efi_port); + if (uart != nullptr) { + uart->begin(g.efi_baudrate); + serial_manager.set_protocol_and_baud(g.efi_port, AP_SerialManager::SerialProtocol_EFI, g.efi_baudrate); + efi.init(); + } + } +#endif + #ifdef HAL_PERIPH_ENABLE_AIRSPEED if (airspeed.enabled()){ #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 8ab1bfd742..9ea2763a4c 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include "../AP_Bootloader/app_comms.h" @@ -92,6 +93,10 @@ public: void prepare_reboot(); bool canfdout() const { return (g.can_fdmode == 1); } +#ifdef HAL_PERIPH_ENABLE_EFI + void can_efi_update(); +#endif + #ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT void check_for_serial_reboot_cmd(const int8_t serial_index); #endif @@ -195,6 +200,11 @@ public: void hwesc_telem_update(); #endif +#ifdef HAL_PERIPH_ENABLE_EFI + AP_EFI efi; + uint32_t efi_update_ms; +#endif + #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM AP_ESC_Telem esc_telem; diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 240769eaaa..44c1bb6f81 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -39,6 +39,14 @@ extern const AP_HAL::HAL &hal; #define AP_PERIPH_BARO_ENABLE_DEFAULT 1 #endif +#ifndef AP_PERIPH_EFI_PORT_DEFAULT +#define AP_PERIPH_EFI_PORT_DEFAULT 3 +#endif + +#ifndef HAL_PERIPH_EFI_BAUDRATE_DEFAULT +#define HAL_PERIPH_EFI_BAUDRATE_DEFAULT 115200 +#endif + #ifndef HAL_DEFAULT_MAV_SYSTEM_ID #define MAV_SYSTEM_ID 3 #else @@ -418,6 +426,31 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(node_stats, "STAT", AP_Stats), #endif +#ifdef HAL_PERIPH_ENABLE_EFI + // @Param: EFI_BAUDRATE + // @DisplayName: EFI serial baudrate + // @Description: EFI serial baudrate. + // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,230:230400,256:256000,460:460800,500:500000,921:921600,1500:1500000 + // @Increment: 1 + // @User: Standard + // @RebootRequired: True + GSCALAR(efi_baudrate, "EFI_BAUDRATE", HAL_PERIPH_EFI_BAUDRATE_DEFAULT), + + // @Param: EFI_PORT + // @DisplayName: EFI Serial Port + // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to EFI. + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + GSCALAR(efi_port, "EFI_PORT", AP_PERIPH_EFI_PORT_DEFAULT), + + // EFI driver + // @Group: EFI + // @Path: ../libraries/AP_EFI/AP_EFI.cpp + GOBJECT(efi, "EFI", AP_EFI), +#endif + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 52d45f0b77..092b4106ad 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -56,6 +56,9 @@ public: k_param_can_fdbaudrate1, k_param_node_stats, k_param_rangefinder_max_rate, + k_param_efi, + k_param_efi_port, + k_param_efi_baudrate, }; AP_Int16 format_version; @@ -129,6 +132,11 @@ public: AP_Int16 sysid_this_mav; #endif +#ifdef HAL_PERIPH_ENABLE_EFI + AP_Int32 efi_baudrate; + AP_Int8 efi_port; +#endif + #if HAL_CANFD_SUPPORTED AP_Int8 can_fdmode; AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES]; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index b3b626df55..a76f023ee3 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1682,6 +1682,9 @@ void AP_Periph_FW::can_update() #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT rcout_update(); +#endif +#ifdef HAL_PERIPH_ENABLE_EFI + can_efi_update(); #endif const uint32_t now_us = AP_HAL::micros(); while ((AP_HAL::micros() - now_us) < 1000) { @@ -2363,6 +2366,193 @@ void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg) } #endif // HAL_PERIPH_ENABLE_ADSB + +#ifdef HAL_PERIPH_ENABLE_EFI +/* + update CAN EFI + */ +void AP_Periph_FW::can_efi_update(void) +{ + if (!efi.enabled()) { + return; + } + efi.update(); + const uint32_t update_ms = efi.get_last_update_ms(); + if (!efi.is_healthy() || efi_update_ms == update_ms) { + return; + } + efi_update_ms = update_ms; + EFI_State state; + efi.get_state(state); + + { + /* + send status packet + */ + uavcan_equipment_ice_reciprocating_Status pkt {}; + + // state maps 1:1 from Engine_State + pkt.state = uint8_t(state.engine_state); + + switch (state.crankshaft_sensor_status) { + case Crankshaft_Sensor_Status::NOT_SUPPORTED: + break; + case Crankshaft_Sensor_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED; + break; + case Crankshaft_Sensor_Status::ERROR: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR; + break; + } + + switch (state.temperature_status) { + case Temperature_Status::NOT_SUPPORTED: + break; + case Temperature_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED; + break; + case Temperature_Status::BELOW_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL; + break; + case Temperature_Status::ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL; + break; + case Temperature_Status::OVERHEATING: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING; + break; + case Temperature_Status::EGT_ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL; + break; + } + + switch (state.fuel_pressure_status) { + case Fuel_Pressure_Status::NOT_SUPPORTED: + break; + case Fuel_Pressure_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED; + break; + case Fuel_Pressure_Status::BELOW_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL; + break; + case Fuel_Pressure_Status::ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL; + break; + } + + switch (state.oil_pressure_status) { + case Oil_Pressure_Status::NOT_SUPPORTED: + break; + case Oil_Pressure_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED; + break; + case Oil_Pressure_Status::BELOW_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL; + break; + case Oil_Pressure_Status::ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL; + break; + } + + switch (state.detonation_status) { + case Detonation_Status::NOT_SUPPORTED: + break; + case Detonation_Status::NOT_OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED; + break; + case Detonation_Status::OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED; + break; + } + + switch (state.misfire_status) { + case Misfire_Status::NOT_SUPPORTED: + break; + case Misfire_Status::NOT_OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED; + break; + case Misfire_Status::OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED; + break; + } + + switch (state.debris_status) { + case Debris_Status::NOT_SUPPORTED: + break; + case Debris_Status::NOT_DETECTED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED; + break; + case Debris_Status::DETECTED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED; + break; + } + + pkt.engine_load_percent = state.engine_load_percent; + pkt.engine_speed_rpm = state.engine_speed_rpm; + pkt.spark_dwell_time_ms = state.spark_dwell_time_ms; + pkt.atmospheric_pressure_kpa = state.atmospheric_pressure_kpa; + pkt.intake_manifold_pressure_kpa = state.intake_manifold_pressure_kpa; + pkt.intake_manifold_temperature = state.intake_manifold_temperature; + pkt.coolant_temperature = state.coolant_temperature; + pkt.oil_pressure = state.oil_pressure; + pkt.oil_temperature = state.oil_temperature; + pkt.fuel_pressure = state.fuel_pressure; + pkt.fuel_consumption_rate_cm3pm = state.fuel_consumption_rate_cm3pm; + pkt.estimated_consumed_fuel_volume_cm3 = state.estimated_consumed_fuel_volume_cm3; + pkt.throttle_position_percent = state.throttle_position_percent; + pkt.ecu_index = state.ecu_index; + pkt.spark_plug_usage = uint8_t(state.spark_plug_usage); + + // assume single set of cylinder status + pkt.cylinder_status.len = 1; + auto &c = pkt.cylinder_status.data[0]; + const auto &state_c = state.cylinder_status[0]; + c.ignition_timing_deg = state_c.ignition_timing_deg; + c.injection_time_ms = state_c.injection_time_ms; + c.cylinder_head_temperature = state_c.cylinder_head_temperature; + c.exhaust_gas_temperature = state_c.exhaust_gas_temperature; + c.lambda_coefficient = state_c.lambda_coefficient; + + uint8_t buffer[UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_MAX_SIZE] {}; + const uint16_t total_size = uavcan_equipment_ice_reciprocating_Status_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + +} +#endif // HAL_PERIPH_ENABLE_EFI + + // printf to CAN LogMessage for debugging void can_printf(const char *fmt, ...) { diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 28fc92e11c..937d91294a 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -62,7 +62,8 @@ def build(bld): 'AC_PID', 'AP_BLHeli', 'AP_ESC_Telem', - 'AP_Stats' + 'AP_Stats', + 'AP_EFI', ] bld.ap_stlib( name= 'AP_Periph_libs',