fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-13 20:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adding new follow target navigation and main states. New follow target

Browse Source 
topic added. New follow fsm added to the navigator
This commit is contained in:
Jimmy Johnson
2016-03-27 12:10:57 +02:00
committed by Lorenz Meier
parent fc061ea94d
commit bbc8eaefd7
14 changed files with 266 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+27 -2
View File
@@ -227,7 +227,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor(msg);
break;
case MAVLINK_MSG_ID_FOLLOW_TARGET:
handle_message_follow_target(msg);
break;
default:
break;
}
@@ -1623,6 +1625,30 @@ 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);
follow_target_s follow_target_topic = { };
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;
if (_follow_me_pub == nullptr) {
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
} else {
warnx("publishing follow");
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
}
warnx("got message follow");
}
void
MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
{
@@ -1804,7 +1830,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
}
}
/**
* Receive data from UART.
*/