Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 01:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: guided mode yaw fix

Browse Source 
Nose of copter now points at next guided point if it is more than 10m
away
This commit is contained in:
Randy Mackay 2013-06-02 11:25:35 +09:00
parent a0bf49ab42
commit 0460147a15
2 changed files with 31 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/GCS_Mavlink.pde
View File

@ -1729,11 +1729,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
// switch to guided mode // initiate guided mode
set_mode(GUIDED); do_guided(&tell_command);
// set wp_nav's destination
wp_nav.set_destination(pv_location_to_vector(tell_command));
// verify we recevied the command // verify we recevied the command
mavlink_msg_mission_ack_send( mavlink_msg_mission_ack_send(

29
ArduCopter/commands_logic.pde
View File

@ -843,6 +843,35 @@ static bool verify_nav_roi()
// Do (Now) commands // Do (Now) commands
/********************************************************************************/ /********************************************************************************/
// do_guided - start guided mode
// this is not actually a mission command but rather a
static void do_guided(const struct Location *cmd)
{
bool first_time = false;
// switch to guided mode if we're not already in guided mode
if (control_mode != GUIDED) {
set_mode(GUIDED);
first_time = true;
}
// set wp_nav's destination
Vector3f pos = pv_location_to_vector(*cmd);
wp_nav.set_destination(pos);
// initialise wp_bearing for reporting purposes
wp_bearing = wp_nav.get_bearing_to_destination();
// point nose at next waypoint if it is more than 10m away
if (yaw_mode == YAW_LOOK_AT_NEXT_WP) {
// get distance to new location
wp_distance = wp_nav.get_distance_to_destination();
// set original_wp_bearing to point at next waypoint
if (wp_distance >= 1000 || first_time) {
original_wp_bearing = wp_bearing;
}
}
}
static void do_change_speed() static void do_change_speed()
{ {
wp_nav.set_horizontal_velocity(command_cond_queue.p1 * 100); wp_nav.set_horizontal_velocity(command_cond_queue.p1 * 100);