fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 02:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

follow target mode working

Browse Source
This commit is contained in:
Jimmy Johnson
2016-03-27 12:10:57 +02:00
committed by Lorenz Meier
parent bbc8eaefd7
commit 69351675b6
8 changed files with 70 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+12 -12
View File
@@ -1625,28 +1625,28 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) {
mavlink_follow_target_t follow_me_msg;
mavlink_msg_follow_target_decode(msg, &follow_me_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_msg_follow_target_decode(msg, &follow_target_msg);
follow_target_topic.timestamp = hrt_absolute_time();
memcpy(follow_target_topic.accel, follow_me_msg.acc, sizeof(follow_target_topic.accel));
memcpy(follow_target_topic.velocity, follow_me_msg.acc, sizeof(follow_target_topic.velocity));
//memcpy(follow_target_topic.attitude_q, follow_me_msg.attitude quaternion, sizeof(follow_target_topic.attitude_q));
follow_target_topic.lat = follow_me_msg.lat;
follow_target_topic.lon = follow_me_msg.lon;
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;
if (_follow_me_pub == nullptr) {
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
} else {
warnx("publishing follow");
warnx("publishing follow %f %f %f", (double) follow_target_topic.velocity[0], (double) follow_target_topic.velocity[1], (double) follow_target_topic.velocity[2]);
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
}
warnx("got message follow");
}
void