Add internal combustion engine status uavcan bridge and mavlink EFI_STATUS stream

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

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

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

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

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 <ponomarevda96@gmail.com>
+ */
+
+#include "ice_status.hpp"
+#include <uORB/topics/internal_combustion_engine_status.h>
+
+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<uavcan::equipment::ice::reciprocating::Status> &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;
+}

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 <ponomarevda96@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+
+#include <uavcan/equipment/ice/reciprocating/Status.hpp>
+
+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<uavcan::equipment::ice::reciprocating::Status> &msg);
+	int init_driver(uavcan_bridge::Channel *channel) override;
+
+	typedef uavcan::MethodBinder < UavcanIceStatusBridge *,
+		void (UavcanIceStatusBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::ice::reciprocating::Status> &) >
+		IceStatusCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::ice::reciprocating::Status, IceStatusCbBinder> _sub_ice_status_data;
+
+};

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<IUavcanSensorBridge
 	list.add(new UavcanRangefinderBridge(node));
 	list.add(new UavcanAccelBridge(node));
 	list.add(new UavcanGyroBridge(node));
+	list.add(new UavcanIceStatusBridge(node));
 }
 
 /*

src/modules/logger/logged_topics.cpp

@@ -67,6 +67,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("home_position");
 	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 500);
+	add_topic("internal_combustion_engine_status", 10);
 	add_topic("mag_worker_data");
 	add_topic("manual_control_setpoint", 200);
 	add_topic("manual_control_switches");

src/modules/mavlink/mavlink_main.cpp

@@ -1537,6 +1537,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
 		configure_stream_local("DISTANCE_SENSOR", 0.5f);
+		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESC_INFO", 1.0f);
 		configure_stream_local("ESC_STATUS", 1.0f);
 		configure_stream_local("ESTIMATOR_STATUS", 0.5f);
@@ -1598,6 +1599,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
 		configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
@@ -1739,6 +1741,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("EFI_STATUS", 10.0f);
 		configure_stream_local("ESC_INFO", 10.0f);
 		configure_stream_local("ESC_STATUS", 10.0f);
 		configure_stream_local("ESTIMATOR_STATUS", 5.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -70,6 +70,7 @@
 #include "streams/COMMAND_LONG.hpp"
 #include "streams/COMPONENT_INFORMATION.hpp"
 #include "streams/DISTANCE_SENSOR.hpp"
+#include "streams/EFI_STATUS.hpp"
 #include "streams/ESC_INFO.hpp"
 #include "streams/ESC_STATUS.hpp"
 #include "streams/ESTIMATOR_STATUS.hpp"
@@ -535,8 +536,11 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamComponentInformation>(),
 #endif // COMPONENT_INFORMATION_HPP
 #if defined(RAW_RPM_HPP)
-	create_stream_list_item<MavlinkStreamRawRpm>()
+	create_stream_list_item<MavlinkStreamRawRpm>(),
 #endif // RAW_RPM_HPP
+#if defined(EFI_STATUS_HPP)
+	create_stream_list_item<MavlinkStreamEfiStatus>()
+#endif // EFI_STATUS_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

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 <uORB/topics/internal_combustion_engine_status.h>
+
+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