Browse Source

Updated MavlinkReceiver::handle_message_landing_target to warn users when they provide a landing target with an unsupported coordinate frame.

Updated Simulator::handle_message_landing_target to warn users when they provide a landing target that is not relative to a captured image.
release/1.12
obicons 4 years ago committed by Daniel Agar
parent
commit
a7e920d276
  1. 5
      src/modules/mavlink/mavlink_receiver.cpp
  2. 23
      src/modules/simulator/simulator_mavlink.cpp

5
src/modules/mavlink/mavlink_receiver.cpp

@ -2305,6 +2305,11 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) @@ -2305,6 +2305,11 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)
_landing_target_pose_pub.publish(landing_target_pose);
} else if (landing_target.position_valid) {
// We only support MAV_FRAME_LOCAL_NED. In this case, the frame was unsupported.
mavlink_log_critical(&_mavlink_log_pub, "landing target: coordinate frame %d unsupported",
landing_target.frame);
} else {
irlock_report_s irlock_report{};

23
src/modules/simulator/simulator_mavlink.cpp

@ -575,15 +575,20 @@ void Simulator::handle_message_landing_target(const mavlink_message_t *msg) @@ -575,15 +575,20 @@ void Simulator::handle_message_landing_target(const mavlink_message_t *msg)
mavlink_landing_target_t landing_target_mavlink;
mavlink_msg_landing_target_decode(msg, &landing_target_mavlink);
irlock_report_s report{};
report.timestamp = hrt_absolute_time();
report.signature = landing_target_mavlink.target_num;
report.pos_x = landing_target_mavlink.angle_x;
report.pos_y = landing_target_mavlink.angle_y;
report.size_x = landing_target_mavlink.size_x;
report.size_y = landing_target_mavlink.size_y;
_irlock_report_pub.publish(report);
if (landing_target_mavlink.position_valid) {
PX4_WARN("Only landing targets relative to captured images are supported");
} else {
irlock_report_s report{};
report.timestamp = hrt_absolute_time();
report.signature = landing_target_mavlink.target_num;
report.pos_x = landing_target_mavlink.angle_x;
report.pos_y = landing_target_mavlink.angle_y;
report.size_x = landing_target_mavlink.size_x;
report.size_y = landing_target_mavlink.size_y;
_irlock_report_pub.publish(report);
}
}
void Simulator::handle_message_odometry(const mavlink_message_t *msg)

Loading…
Cancel
Save