Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate control_guided

This commit is contained in:
Randy Mackay 2014-01-28 22:45:07 +09:00 committed by Andrew Tridgell
parent 798add0b5c
commit 54be8402a2
1 changed files with 2 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/commands_logic.pde
View File

@ -594,12 +594,9 @@ static bool verify_yaw()
// this is not actually a mission command but rather a // this is not actually a mission command but rather a
static void do_guided(const struct Location *cmd) static void do_guided(const struct Location *cmd)
{ {
bool first_time = false;
// switch to guided mode if we're not already in guided mode // switch to guided mode if we're not already in guided mode
if (control_mode != GUIDED) { if (control_mode != GUIDED) {
if (set_mode(GUIDED)) { if (!set_mode(GUIDED)) {
first_time = true;
}else{
// if we failed to enter guided mode return immediately // if we failed to enter guided mode return immediately
return; return;
} }
@ -607,20 +604,7 @@ static void do_guided(const struct Location *cmd)
// set wp_nav's destination // set wp_nav's destination
Vector3f pos = pv_location_to_vector(*cmd); Vector3f pos = pv_location_to_vector(*cmd);
wp_nav.set_wp_destination(pos); guided_set_destination(pos);
// initialise wp_bearing for reporting purposes
wp_bearing = wp_nav.get_wp_bearing_to_destination();
// point nose at next waypoint if it is more than 10m away
if (auto_yaw_mode == AUTO_YAW_LOOK_AT_NEXT_WP) {
// get distance to new location
wp_distance = wp_nav.get_wp_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()