From af2247bd94698bdd524bdfa53bcd76e8e505ae45 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Sat, 31 Jul 2021 18:41:49 +0200 Subject: [PATCH] mavlink: use dynamic camera comp id instead of hardcoded value --- msg/CMakeLists.txt | 1 + msg/camera_status.msg | 4 ++++ msg/tools/uorb_rtps_message_ids.yaml | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 6 ++++++ src/modules/mavlink/mavlink_receiver.h | 2 ++ src/modules/mavlink/streams/CAMERA_TRIGGER.hpp | 14 +++++++++++--- 6 files changed, 26 insertions(+), 3 deletions(-) create mode 100644 msg/camera_status.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3e91a16901..fe981ac0b9 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -47,6 +47,7 @@ set(msg_files baro_bias_estimate.msg battery_status.msg camera_capture.msg + camera_status.msg camera_trigger.msg cellular_status.msg collision_constraints.msg diff --git a/msg/camera_status.msg b/msg/camera_status.msg new file mode 100644 index 0000000000..c83be897d9 --- /dev/null +++ b/msg/camera_status.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 active_sys_id # mavlink system id of the currently active camera +uint8 active_comp_id # mavlink component id of currently active camera diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 049c539dd8..8a6b0d9ae0 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -347,6 +347,8 @@ rtps: id: 161 - msg: internal_combustion_engine_status id: 162 + - msg: camera_status + id: 163 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 180 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 838d05c665..060dd00707 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2001,6 +2001,8 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) if (same_system || hb.type == MAV_TYPE_GCS) { + camera_status_s camera_status{}; + switch (hb.type) { case MAV_TYPE_ANTENNA_TRACKER: _heartbeat_type_antenna_tracker = now; @@ -2024,6 +2026,10 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) case MAV_TYPE_CAMERA: _heartbeat_type_camera = now; + camera_status.timestamp = now; + camera_status.active_comp_id = msg->compid; + camera_status.active_sys_id = msg->sysid; + _camera_status_pub.publish(camera_status); break; default: diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9f898e4711..7584420673 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -62,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -266,6 +267,7 @@ private: uORB::Publication _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)}; 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 _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/CAMERA_TRIGGER.hpp b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp index 64d199e304..127904e9aa 100644 --- a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp +++ b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp @@ -34,6 +34,7 @@ #ifndef CAMERA_TRIGGER_HPP #define CAMERA_TRIGGER_HPP +#include #include class MavlinkStreamCameraTrigger : public MavlinkStream @@ -63,6 +64,12 @@ private: explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _camera_trigger_sub{ORB_ID(camera_trigger)}; + uORB::Subscription _camera_status_sub{ORB_ID(camera_status)}; + camera_status_s _camera_status = { + 0, //timestamp + 0, //target_sys_id + MAV_COMP_ID_CAMERA // active_comp_id + }; bool send() override { @@ -76,6 +83,7 @@ private: msg.seq = camera_trigger.seq; mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg); + _camera_status_sub.update(&_camera_status); vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); @@ -88,7 +96,7 @@ private: vcmd.param7 = NAN; vcmd.command = MAV_CMD_IMAGE_START_CAPTURE; vcmd.target_system = mavlink_system.sysid; - vcmd.target_component = MAV_COMP_ID_CAMERA; + vcmd.target_component = _camera_status.active_comp_id; MavlinkCommandSender::instance().handle_vehicle_command(vcmd, _mavlink->get_channel()); @@ -97,8 +105,8 @@ private: /* send MAV_CMD_DO_DIGICAM_CONTROL*/ mavlink_command_long_t command_long_msg{}; - command_long_msg.target_system = 0; // 0 for broadcast - command_long_msg.target_component = MAV_COMP_ID_CAMERA; + command_long_msg.target_system = _camera_status.active_sys_id; + command_long_msg.target_component = _camera_status.active_comp_id; command_long_msg.command = MAV_CMD_DO_DIGICAM_CONTROL; command_long_msg.confirmation = 0; command_long_msg.param1 = NAN;