From b491356d3f229ad4e6b424fad4ab0c32d65612a1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 6 Feb 2022 21:33:02 -0500 Subject: [PATCH] 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 | 1 + msg/can_frame.msg | 15 ++++ msg/vehicle_command.msg | 1 + src/drivers/uavcan/uavcan_main.cpp | 38 +++++++++++ src/drivers/uavcan/uavcan_main.hpp | 23 +++++++ src/drivers/uavcan/uavcan_params.c | 12 ++++ src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 4 ++ src/modules/mavlink/mavlink_receiver.cpp | 55 ++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 7 +- src/modules/mavlink/streams/CAN_FRAME.hpp | 83 +++++++++++++++++++++++ 11 files changed, 236 insertions(+), 4 deletions(-) create mode 100644 msg/can_frame.msg create mode 100644 src/modules/mavlink/streams/CAN_FRAME.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 8ad5e81dbd..3551d39b5a 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/can_frame.msg b/msg/can_frame.msg new file mode 100644 index 0000000000..3ea4e799f0 --- /dev/null +++ b/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 diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 29fa8ef154..07817e43a8 100644 --- a/msg/vehicle_command.msg +++ b/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. diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 24b677da6f..240f959c9a 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/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) { diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 2bb8a434c0..f0c0b1999c 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -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_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}; }; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index ce6b34480c..c8e7a99f9a 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/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 * diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed54d899ad..e8c35d37f8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9c0edd4a42..0f1d95ad28 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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(), #endif // NAV_CONTROLLER_OUTPUT_HPP +#if defined(CAN_FRAME_HPP) + create_stream_list_item(), +#endif // CAN_FRAME_HPP #if defined(CAMERA_TRIGGER_HPP) create_stream_list_item(), #endif // CAMERA_TRIGGER_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0f396421a9..b45726b332 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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 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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ba43ff3fc0..5cc3be0e23 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/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 #include #include +#include #include #include #include @@ -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_pub{ORB_ID(airspeed)}; uORB::Publication _battery_pub{ORB_ID(battery_status)}; uORB::Publication _camera_status_pub{ORB_ID(camera_status)}; + uORB::Publication _can_frame_pub{ORB_ID(can_frame_in)}; uORB::Publication _cellular_status_pub{ORB_ID(cellular_status)}; uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; diff --git a/src/modules/mavlink/streams/CAN_FRAME.hpp b/src/modules/mavlink/streams/CAN_FRAME.hpp new file mode 100644 index 0000000000..ff98db3df0 --- /dev/null +++ b/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 + +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