|
|
|
@ -1628,25 +1628,22 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
@@ -1628,25 +1628,22 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
|
|
|
|
|
|
|
|
|
void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) |
|
|
|
|
{ |
|
|
|
|
mavlink_follow_target_t follow_target_msg; |
|
|
|
|
follow_target_s follow_target_topic = { }; |
|
|
|
|
mavlink_follow_target_t follow_target_msg; |
|
|
|
|
follow_target_s follow_target_topic = { }; |
|
|
|
|
|
|
|
|
|
mavlink_msg_follow_target_decode(msg, &follow_target_msg); |
|
|
|
|
mavlink_msg_follow_target_decode(msg, &follow_target_msg); |
|
|
|
|
|
|
|
|
|
follow_target_topic.timestamp = hrt_absolute_time(); |
|
|
|
|
follow_target_topic.timestamp = hrt_absolute_time(); |
|
|
|
|
|
|
|
|
|
// memcpy(follow_target_topic.accel, follow_target_msg.acc, sizeof(follow_target_topic.accel));
|
|
|
|
|
//memcpy(follow_target_topic.velocity, follow_target_msg.vel, sizeof(follow_target_topic.velocity));
|
|
|
|
|
follow_target_topic.lat = follow_target_msg.lat*1e-7; |
|
|
|
|
follow_target_topic.lon = follow_target_msg.lon*1e-7; |
|
|
|
|
follow_target_topic.alt = follow_target_msg.alt; |
|
|
|
|
|
|
|
|
|
follow_target_topic.lat = follow_target_msg.lat*1e-7; |
|
|
|
|
follow_target_topic.lon = follow_target_msg.lon*1e-7; |
|
|
|
|
follow_target_topic.alt = follow_target_msg.alt; |
|
|
|
|
|
|
|
|
|
if (_follow_target_pub == nullptr) { |
|
|
|
|
_follow_target_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); |
|
|
|
|
} else { |
|
|
|
|
orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic); |
|
|
|
|
} |
|
|
|
|
if (_follow_target_pub == nullptr) { |
|
|
|
|
_follow_target_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); |
|
|
|
|
} else { |
|
|
|
|
orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|