fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 18:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: use dynamic camera comp id instead of hardcoded value

Browse Source
This commit is contained in:
Igor Mišić
2021-07-31 18:41:49 +02:00
committed by GitHub
parent 6379f2b032
commit af2247bd94
6 changed files with 26 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+6
View File
@@ -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:
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -62,6 +62,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_status.h>
#include <uORB/topics/cellular_status.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/differential_pressure.h>
@@ -266,6 +267,7 @@ private:
uORB::Publication<actuator_controls_s> _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_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<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/CAMERA_TRIGGER.hpp
+11 -3
View File
@@ -34,6 +34,7 @@
#ifndef CAMERA_TRIGGER_HPP
#define CAMERA_TRIGGER_HPP
#include <uORB/topics/camera_status.h>
#include <uORB/topics/camera_trigger.h>
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;