fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 17:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

follow me working

Browse Source
This commit is contained in:
Jimmy Johnson
2016-03-09 15:31:50 -08:00
committed by Lorenz Meier
parent 01e971b342
commit 26cb645ee0
7 changed files with 34 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+13 -12
View File
@@ -127,6 +127,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_manual_pub(nullptr),
_land_detector_pub(nullptr),
_time_offset_pub(nullptr),
_follow_me_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -1627,27 +1628,27 @@ 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 = {};
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.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.lat = follow_target_msg.lat*1e-7;
// follow_target.lon = follow_target_msg.lon*1e-7;
// follow_target.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_me_pub == nullptr) {
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target);
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
} else {
warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target.lat,
(double) follow_target.lon,
(double) follow_target.alt);
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target);
warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target_topic.lat,
(double) follow_target_topic.lon,
(double) follow_target_topic.alt);
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
}
}