diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 518d233099..3e91a16901 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -84,6 +84,7 @@ set(msg_files heater_status.msg home_position.msg hover_thrust_estimate.msg + internal_combustion_engine_status.msg input_rc.msg iridiumsbd_status.msg irlock_report.msg diff --git a/msg/internal_combustion_engine_status.msg b/msg/internal_combustion_engine_status.msg new file mode 100644 index 0000000000..301eb92a86 --- /dev/null +++ b/msg/internal_combustion_engine_status.msg @@ -0,0 +1,64 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 STATE_STOPPED = 0 # The engine is not running. This is the default state. +uint8 STATE_STARTING = 1 # The engine is starting. This is a transient state. +uint8 STATE_RUNNING = 2 # The engine is running normally. +uint8 STATE_FAULT = 3 # The engine can no longer function. +uint8 state + +uint32 FLAG_GENERAL_ERROR = 1 # General error. + +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED = 2 # Error of the crankshaft sensor. This flag is optional. +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR = 4 + +uint32 FLAG_TEMPERATURE_SUPPORTED = 8 # Temperature levels. These flags are optional +uint32 FLAG_TEMPERATURE_BELOW_NOMINAL = 16 # Under-temperature warning +uint32 FLAG_TEMPERATURE_ABOVE_NOMINAL = 32 # Over-temperature warning +uint32 FLAG_TEMPERATURE_OVERHEATING = 64 # Critical overheating +uint32 FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL = 128 # Exhaust gas over-temperature warning + +uint32 FLAG_FUEL_PRESSURE_SUPPORTED = 256 # Fuel pressure. These flags are optional +uint32 FLAG_FUEL_PRESSURE_BELOW_NOMINAL = 512 # Under-pressure warning +uint32 FLAG_FUEL_PRESSURE_ABOVE_NOMINAL = 1024 # Over-pressure warning + +uint32 FLAG_DETONATION_SUPPORTED = 2048 # Detonation warning. This flag is optional. +uint32 FLAG_DETONATION_OBSERVED = 4096 # Detonation condition observed warning + +uint32 FLAG_MISFIRE_SUPPORTED = 8192 # Misfire warning. This flag is optional. +uint32 FLAG_MISFIRE_OBSERVED = 16384 # Misfire condition observed warning + +uint32 FLAG_OIL_PRESSURE_SUPPORTED = 32768 # Oil pressure. These flags are optional +uint32 FLAG_OIL_PRESSURE_BELOW_NOMINAL = 65536 # Under-pressure warning +uint32 FLAG_OIL_PRESSURE_ABOVE_NOMINAL = 131072 # Over-pressure warning + +uint32 FLAG_DEBRIS_SUPPORTED = 262144 # Debris warning. This flag is optional +uint32 FLAG_DEBRIS_DETECTED = 524288 # Detection of debris warning +uint32 flags + +uint8 engine_load_percent # Engine load estimate, percent, [0, 127] +uint32 engine_speed_rpm # Engine speed, revolutions per minute +float32 spark_dwell_time_ms # Spark dwell time, millisecond +float32 atmospheric_pressure_kpa # Atmospheric (barometric) pressure, kilopascal +float32 intake_manifold_pressure_kpa # Engine intake manifold pressure, kilopascal +float32 intake_manifold_temperature # Engine intake manifold temperature, kelvin +float32 coolant_temperature # Engine coolant temperature, kelvin +float32 oil_pressure # Oil pressure, kilopascal +float32 oil_temperature # Oil temperature, kelvin +float32 fuel_pressure # Fuel pressure, kilopascal +float32 fuel_consumption_rate_cm3pm # Instant fuel consumption estimate, (centimeter^3)/minute +float32 estimated_consumed_fuel_volume_cm3 # Estimate of the consumed fuel since the start of the engine, centimeter^3 +uint8 throttle_position_percent # Throttle position, percent +uint8 ecu_index # The index of the publishing ECU + + +uint8 SPARK_PLUG_SINGLE = 0 +uint8 SPARK_PLUG_FIRST_ACTIVE = 1 +uint8 SPARK_PLUG_SECOND_ACTIVE = 2 +uint8 SPARK_PLUG_BOTH_ACTIVE = 3 +uint8 spark_plug_usage # Spark plug activity report. + +float32 ignition_timing_deg # Cylinder ignition timing, angular degrees of the crankshaft +float32 injection_time_ms # Fuel injection time, millisecond +float32 cylinder_head_temperature # Cylinder head temperature (CHT), kelvin +float32 exhaust_gas_temperature # Exhaust gas temperature (EGT), kelvin +float32 lambda_coefficient # Estimated lambda coefficient, dimensionless ratio diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 36c29752da..049c539dd8 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -345,6 +345,8 @@ rtps: id: 160 - msg: baro_bias_estimate id: 161 + - msg: internal_combustion_engine_status + id: 162 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 180 diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 003b624c49..5c8fcb96e4 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -158,6 +158,7 @@ px4_add_module( sensors/accel.cpp sensors/gyro.cpp sensors/cbat.cpp + sensors/ice_status.cpp DEPENDS px4_uavcan_dsdlc diff --git a/src/drivers/uavcan/sensors/ice_status.cpp b/src/drivers/uavcan/sensors/ice_status.cpp new file mode 100644 index 0000000000..1b6274403b --- /dev/null +++ b/src/drivers/uavcan/sensors/ice_status.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Dmitry Ponomarev + */ + +#include "ice_status.hpp" +#include + +const char *const UavcanIceStatusBridge::NAME = "ice_status"; + +UavcanIceStatusBridge::UavcanIceStatusBridge(uavcan::INode &node) : + UavcanSensorBridgeBase("uavcan_ice_status", ORB_ID(internal_combustion_engine_status)), + _sub_ice_status_data(node) +{ } + +int UavcanIceStatusBridge::init() +{ + int res = _sub_ice_status_data.start(IceStatusCbBinder(this, &UavcanIceStatusBridge::ice_status_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +void UavcanIceStatusBridge::ice_status_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + auto report = ::internal_combustion_engine_status_s(); + report.timestamp = hrt_absolute_time(); + report.state = msg.state; + report.flags = msg.flags; + report.engine_load_percent = msg.engine_load_percent; + report.engine_speed_rpm = msg.engine_speed_rpm; + report.spark_dwell_time_ms = msg.spark_dwell_time_ms; + report.atmospheric_pressure_kpa = msg.atmospheric_pressure_kpa; + report.intake_manifold_pressure_kpa = msg.intake_manifold_pressure_kpa; + report.intake_manifold_temperature = msg.intake_manifold_temperature; + report.coolant_temperature = msg.coolant_temperature; + report.oil_pressure = msg.oil_pressure; + report.oil_temperature = msg.oil_temperature; + report.fuel_pressure = msg.fuel_pressure; + report.fuel_consumption_rate_cm3pm = msg.fuel_consumption_rate_cm3pm; + report.estimated_consumed_fuel_volume_cm3 = msg.estimated_consumed_fuel_volume_cm3; + report.throttle_position_percent = msg.throttle_position_percent; + report.ecu_index = msg.ecu_index; + report.spark_plug_usage = msg.spark_plug_usage; + + if (msg.cylinder_status.size() > 0) { + report.ignition_timing_deg = msg.cylinder_status[0].ignition_timing_deg; + report.injection_time_ms = msg.cylinder_status[0].injection_time_ms; + report.cylinder_head_temperature = msg.cylinder_status[0].cylinder_head_temperature; + report.exhaust_gas_temperature = msg.cylinder_status[0].exhaust_gas_temperature; + report.lambda_coefficient = msg.cylinder_status[0].lambda_coefficient; + } + + publish(msg.getSrcNodeID().get(), &report); +} + +int UavcanIceStatusBridge::init_driver(uavcan_bridge::Channel *channel) +{ + return PX4_OK; +} diff --git a/src/drivers/uavcan/sensors/ice_status.hpp b/src/drivers/uavcan/sensors/ice_status.hpp new file mode 100644 index 0000000000..54d2edb71a --- /dev/null +++ b/src/drivers/uavcan/sensors/ice_status.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Dmitry Ponomarev + */ + +#pragma once + +#include "sensor_bridge.hpp" + +#include + +class UavcanIceStatusBridge : public UavcanSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanIceStatusBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + + void ice_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + int init_driver(uavcan_bridge::Channel *channel) override; + + typedef uavcan::MethodBinder < UavcanIceStatusBridge *, + void (UavcanIceStatusBridge::*) + (const uavcan::ReceivedDataStructure &) > + IceStatusCbBinder; + + uavcan::Subscriber _sub_ice_status_data; + +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 4429c8d723..3b2a7224af 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -49,6 +49,7 @@ #include "accel.hpp" #include "gyro.hpp" #include "cbat.hpp" +#include "ice_status.hpp" /* * IUavcanSensorBridge @@ -76,6 +77,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List(), #endif // COMPONENT_INFORMATION_HPP #if defined(RAW_RPM_HPP) - create_stream_list_item() + create_stream_list_item(), #endif // RAW_RPM_HPP +#if defined(EFI_STATUS_HPP) + create_stream_list_item() +#endif // EFI_STATUS_HPP }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/streams/EFI_STATUS.hpp b/src/modules/mavlink/streams/EFI_STATUS.hpp new file mode 100644 index 0000000000..48ea82459c --- /dev/null +++ b/src/modules/mavlink/streams/EFI_STATUS.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef EFI_STATUS_HPP +#define EFI_STATUS_HPP + +#include + +class MavlinkStreamEfiStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamEfiStatus(mavlink); } + + static constexpr const char *get_name_static() { return "EFI_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_EFI_STATUS; } + + const char *get_name() const override { return MavlinkStreamEfiStatus::get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _internal_combustion_engine_status_sub.advertised() ? MAVLINK_MSG_ID_EFI_STATUS + MAVLINK_NUM_NON_PAYLOAD_BYTES : + 0; + } + +private: + explicit MavlinkStreamEfiStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _internal_combustion_engine_status_sub{ORB_ID(internal_combustion_engine_status)}; + + bool send() override + { + internal_combustion_engine_status_s internal_combustion_engine_status; + + if (_internal_combustion_engine_status_sub.update(&internal_combustion_engine_status)) { + mavlink_efi_status_t msg{}; + + msg.health = internal_combustion_engine_status.state; + msg.ecu_index = internal_combustion_engine_status.ecu_index; + msg.rpm = internal_combustion_engine_status.engine_speed_rpm; + msg.fuel_consumed = internal_combustion_engine_status.estimated_consumed_fuel_volume_cm3; + msg.fuel_flow = internal_combustion_engine_status.fuel_consumption_rate_cm3pm; + msg.engine_load = internal_combustion_engine_status.engine_load_percent; + msg.throttle_position = internal_combustion_engine_status.throttle_position_percent; + msg.spark_dwell_time = internal_combustion_engine_status.spark_dwell_time_ms; + msg.barometric_pressure = internal_combustion_engine_status.atmospheric_pressure_kpa; + msg.intake_manifold_pressure = internal_combustion_engine_status.intake_manifold_pressure_kpa; + msg.intake_manifold_temperature = internal_combustion_engine_status.intake_manifold_temperature; + + msg.cylinder_head_temperature = internal_combustion_engine_status.cylinder_head_temperature; + msg.ignition_timing = internal_combustion_engine_status.ignition_timing_deg; + msg.injection_time = internal_combustion_engine_status.injection_time_ms; + msg.exhaust_gas_temperature = internal_combustion_engine_status.exhaust_gas_temperature; + msg.throttle_out = internal_combustion_engine_status.throttle_position_percent; + msg.pt_compensation = internal_combustion_engine_status.lambda_coefficient; + + mavlink_msg_efi_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // EFI_STATUS_HPP