UAVCAN: publish CAN frames to uORB (ORB_ID(can_frame_out))

- can frames published to ORB_ID(can_frame_in) are injected

msg/CMakeLists.txt

@@ -56,6 +56,7 @@ set(msg_files
 	camera_capture.msg
 	camera_status.msg
 	camera_trigger.msg
+	can_frame.msg
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg

msg/can_frame.msg

@@ -0,0 +1,15 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint32 id
+
+bool is_rtr
+bool is_extended
+bool is_error
+
+uint8 dlc
+
+uint8[8] data
+
+uint8 ORB_QUEUE_LENGTH = 8
+
+# TOPICS can_frame can_frame_out can_frame_in

msg/vehicle_command.msg

@@ -96,6 +96,7 @@ uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    	# Command VTOL transition
 uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001	# Request arm authorization
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
+uint16 VEHICLE_CMD_CAN_FORWARD = 32000
 uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
 uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 

src/drivers/uavcan/uavcan_main.cpp

@@ -130,6 +130,10 @@ UavcanNode::~UavcanNode()
 		} while (_instance);
 	}
 
+	_node.removeRxFrameListener();
+	delete _rx_frame_listener_uorb;
+	_rx_frame_listener_uorb = nullptr;
+
 	// Removing the sensor bridges
 	_sensor_bridges.clear();
 
@@ -498,6 +502,17 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 
 	fill_node_info();
 
+	// install frame listener
+	int32_t uavcan_frame_dbg = 0;
+	param_get(param_find("UAVCAN_FRAME_DBG"), &uavcan_frame_dbg);
+
+	if (uavcan_frame_dbg == 1) {
+		PX4_INFO("UAVCAN FRAME DBG enabled");
+		_rx_frame_listener_uorb = new RxFrameUorbPublisher();
+		_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
+		_mirror_to_uorb = true;
+	}
+
 	int ret = _beep_controller.init();
 
 	if (ret < 0) {
@@ -689,6 +704,22 @@ UavcanNode::Run()
 		update_params();
 	}
 
+	if (_mirror_to_uorb && _can_frame_in_sub.updated()) {
+		can_frame_s can_frame;
+
+		if (_can_frame_in_sub.copy(&can_frame)) {
+			uavcan::CanFrame frame{};
+			frame.id = can_frame.id;
+			memcpy(frame.data, can_frame.data, sizeof(can_frame.data));
+			frame.dlc = can_frame.dlc;
+
+			uavcan::MonotonicTime tx_deadline = _node.getMonotonicTime() + uavcan::MonotonicDuration::fromUSec(1000);
+			uint8_t iface_mask = 3;
+
+			_node.injectTxFrame(frame, tx_deadline, iface_mask, uavcan::CanTxQueue::Volatile);
+		}
+	}
+
 	// Check for parameter requests (get/set/list)
 	if (_param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
 		uavcan_parameter_request_s request{};
@@ -877,6 +908,13 @@ UavcanNode::Run()
 					break;
 				}
 			}
+
+		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_CAN_FORWARD) {
+			if (_rx_frame_listener_uorb == nullptr) {
+				_rx_frame_listener_uorb = new RxFrameUorbPublisher();
+				_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
+				_mirror_to_uorb = true;
+			}
 		}
 
 		if (acknowledge) {

src/drivers/uavcan/uavcan_main.hpp

@@ -74,6 +74,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/can_frame.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -315,4 +316,26 @@ private:
 	bool _check_fw{false};
 
 	UavcanServers   *_servers{nullptr};
+
+
+	struct RxFrameUorbPublisher : public uavcan::IRxFrameListener {
+		void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
+		{
+			can_frame_s can_frame{};
+			can_frame.id = frame.id;
+			//can_frame.iface_index = frame.iface_index;
+			can_frame.dlc = frame.dlc;
+			memcpy(&can_frame.data, &frame.data, sizeof(frame.data));
+			can_frame.timestamp = hrt_absolute_time();
+			_can_frame_pub.publish(can_frame);
+		}
+	private:
+		uORB::Publication<can_frame_s> _can_frame_pub{ORB_ID(can_frame_out)};
+	};
+
+	RxFrameUorbPublisher *_rx_frame_listener_uorb{nullptr};
+
+	uORB::Subscription _can_frame_in_sub{ORB_ID(can_frame_in)};
+
+	bool _mirror_to_uorb{false};
 };

src/drivers/uavcan/uavcan_params.c

@@ -77,6 +77,18 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
  */
 PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
 
+/**
+ * UAVCAN CAN frame debug
+ *
+ * Publish UAVCAN CAN frames to ORB_ID(can_frame_out)
+ * CAN frames published to ORB_ID(can_frame_in) are injected into UAVCAN
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_FRAME_DBG, 0);
+
 /**
  * UAVCAN rangefinder minimum range
  *

src/modules/mavlink/mavlink_main.cpp

@@ -1711,6 +1711,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE_TARGET", 8.0f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
+		configure_stream_local("CAN_FRAME", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
 		configure_stream_local("EFI_STATUS", 10.0f);
 		configure_stream_local("ESC_INFO", 10.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -66,6 +66,7 @@
 #include "streams/BATTERY_STATUS.hpp"
 #include "streams/CAMERA_IMAGE_CAPTURED.hpp"
 #include "streams/CAMERA_TRIGGER.hpp"
+#include "streams/CAN_FRAME.hpp"
 #include "streams/COLLISION.hpp"
 #include "streams/COMMAND_LONG.hpp"
 #include "streams/COMPONENT_INFORMATION.hpp"
@@ -474,6 +475,9 @@ static const StreamListItem streams_list[] = {
 #if defined(NAV_CONTROLLER_OUTPUT_HPP)
 	create_stream_list_item<MavlinkStreamNavControllerOutput>(),
 #endif // NAV_CONTROLLER_OUTPUT_HPP
+#if defined(CAN_FRAME_HPP)
+	create_stream_list_item<MavlinkStreamCanFrame>(),
+#endif // CAN_FRAME_HPP
 #if defined(CAMERA_TRIGGER_HPP)
 	create_stream_list_item<MavlinkStreamCameraTrigger>(),
 #endif // CAMERA_TRIGGER_HPP

src/modules/mavlink/mavlink_receiver.cpp

@@ -234,6 +234,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_battery_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_CAN_FRAME:
+		handle_message_can_frame(msg);
+		break;
+
 	case MAVLINK_MSG_ID_SERIAL_CONTROL:
 		handle_message_serial_control(msg);
 		break;
@@ -369,7 +373,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 }
 
 bool
-MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component)
+MavlinkReceiver::evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component)
 {
 	/* evaluate if this system should accept this command */
 	bool target_ok = false;
@@ -382,6 +386,11 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c
 		target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
 		break;
 
+	case MAV_CMD_CAN_FORWARD:
+		// MAV_COMP_ID_MAVCAN
+		target_ok = ((target_system == 0) || (target_system == mavlink_system.sysid)) && (msg->compid == MAV_COMP_ID_MAVCAN);
+		break;
+
 	default:
 		target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid)
 				|| (target_component == MAV_COMP_ID_ALL));
@@ -482,12 +491,18 @@ template <class T>
 void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
 		const vehicle_command_s &vehicle_command)
 {
-	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
+	bool target_ok = evaluate_target_ok(msg, cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
 	bool send_ack = true;
 	uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 	uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility
 
 	if (!target_ok) {
+
+		if (cmd_mavlink.command == MAV_CMD_CAN_FORWARD) {
+			PX4_WARN("cmd:%d from SYSID:%d, COMPID:%d, target_ok:%d, target_system:%d, target_component:%d", cmd_mavlink.command,
+				 msg->sysid, msg->compid, target_ok, cmd_mavlink.target_system, cmd_mavlink.target_component);
+		}
+
 		// Reject alien commands only if there is no forwarding or we've never seen target component before
 		if (!_mavlink->get_forwarding_on()
 		    || !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
@@ -561,6 +576,18 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			send_ack = true;
 		}
 
+	} else if (cmd_mavlink.command == MAV_CMD_CAN_FORWARD) {
+
+		//PX4_INFO("cmd MAV_CMD_CAN_FORWARD");
+
+		// 1: bus	Bus number (0 to disable forwarding, 1 for first bus, 2 for 2nd bus, 3 for 3rd bus).
+
+		if (!_can_frame_pub.advertised()) {
+			PX4_INFO("MAV_CMD_CAN_FORWARD: enabling CAN_FRAME stream");
+			const char stream_name[] {"CAN_FRAME"};
+			_mavlink->configure_stream_threadsafe(stream_name, -1.f);
+		}
+
 	} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {
 
 		bool has_module = true;
@@ -1834,6 +1861,30 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
 	_battery_pub.publish(battery_status);
 }
 
+void
+MavlinkReceiver::handle_message_can_frame(mavlink_message_t *msg)
+{
+	if (!_can_frame_pub.advertised()) {
+		PX4_INFO("enabling CAN_FRAME stream");
+		const char stream_name[] {"CAN_FRAME"};
+		_mavlink->configure_stream_threadsafe(stream_name, -1.f);
+	}
+
+	mavlink_can_frame_t mavlink_can_frame;
+	mavlink_msg_can_frame_decode(msg, &mavlink_can_frame);
+
+	can_frame_s can_frame{};
+	//msg.target_system = 0;
+	//msg.target_component = 0;
+	//can_frame.bus = mavlink_can_frame.bus;
+	can_frame.id = mavlink_can_frame.id;
+	can_frame.dlc = mavlink_can_frame.len;
+	memcpy(can_frame.data, mavlink_can_frame.data, sizeof(mavlink_can_frame.data));
+
+	can_frame.timestamp = hrt_absolute_time();
+	_can_frame_pub.publish(can_frame);
+}
+
 void
 MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 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
@@ -67,6 +67,7 @@
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/camera_status.h>
+#include <uORB/topics/can_frame.h>
 #include <uORB/topics/cellular_status.h>
 #include <uORB/topics/collision_report.h>
 #include <uORB/topics/differential_pressure.h>
@@ -160,6 +161,7 @@ private:
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_att_pos_mocap(mavlink_message_t *msg);
 	void handle_message_battery_status(mavlink_message_t *msg);
+	void handle_message_can_frame(mavlink_message_t *msg);
 	void handle_message_cellular_status(mavlink_message_t *msg);
 	void handle_message_collision(mavlink_message_t *msg);
 	void handle_message_command_ack(mavlink_message_t *msg);
@@ -228,7 +230,7 @@ private:
 	int set_message_interval(int msgId, float interval, int data_rate = -1);
 	void get_message_interval(int msgId);
 
-	bool evaluate_target_ok(int command, int target_system, int target_component);
+	bool evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component);
 
 	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
 
@@ -294,6 +296,7 @@ private:
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
+	uORB::Publication<can_frame_s>                          _can_frame_pub{ORB_ID(can_frame_in)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};

src/modules/mavlink/streams/CAN_FRAME.hpp

@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 CAN_FRAME_HPP
+#define CAN_FRAME_HPP
+
+#include <uORB/topics/can_frame.h>
+
+class MavlinkStreamCanFrame : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCanFrame(mavlink); }
+
+	static constexpr const char *get_name_static() { return "CAN_FRAME"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CAN_FRAME; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _can_frame_out_sub.advertised() ? MAVLINK_MSG_ID_CAN_FRAME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	explicit MavlinkStreamCanFrame(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _can_frame_out_sub{ORB_ID(can_frame_out)};
+
+	bool send() override
+	{
+		can_frame_s can_frame;
+
+		if (_can_frame_out_sub.update(&can_frame)) {
+			mavlink_can_frame_t msg{};
+
+			msg.target_system = 0;
+			msg.target_component = 0;
+			msg.bus = 0;
+			msg.len = can_frame.dlc;
+			msg.id = can_frame.id;
+			memcpy(msg.data, can_frame.data, sizeof(msg.data));
+
+			mavlink_msg_can_frame_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // CAN_FRAME_HPP