Browse Source

Copter: only acceept Guided waypoints in Guided mode

Fix #1068. When receiving guided waypoints do not change to GUIDED mode. This serves as a safety precaution for GCS, since they must switch to guided mode before sending the waypoints.
mission-4.1.18
Arthur Benemann 11 years ago committed by Randy Mackay
parent
commit
e1b7e53c04
  1. 7
      ArduCopter/commands_logic.pde

7
ArduCopter/commands_logic.pde

@ -777,12 +777,9 @@ static bool verify_yaw()
// do_guided - start guided mode // do_guided - start guided mode
static bool do_guided(const AP_Mission::Mission_Command& cmd) static bool do_guided(const AP_Mission::Mission_Command& cmd)
{ {
// switch to guided mode if we're not already in guided mode // only process guided waypoint if we are in guided mode
if (control_mode != GUIDED) { if (control_mode != GUIDED) {
if (!set_mode(GUIDED)) { return false;
// if we failed to enter guided mode return immediately
return false;
}
} }
// set wp_nav's destination // set wp_nav's destination

Loading…
Cancel
Save