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

Copter: only start delay timer after reaching wp location and altitude

This commit is contained in:
Randy Mackay 2013-03-06 12:50:41 +09:00
parent a73f50494c
commit 5380973225
2 changed files with 24 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduCopter/commands_logic.pde
View File

@ -284,11 +284,15 @@ static void do_nav_wp()
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;
} }
// this will be used to remember the time in millis after we reach or pass the WP. // if not delay requirement, set the delay achieved bit of wp_verify_byte
loiter_time = 0; if( command_nav_queue.p1 == 0 ) {
wp_verify_byte |= NAV_DELAY;
// this is the delay, stored in seconds and expanded to millis }else{
loiter_time_max = command_nav_queue.p1 * 1000; // this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds and expanded to millis
loiter_time_max = command_nav_queue.p1 * 1000;
}
// reset control of yaw to default // reset control of yaw to default
if( g.yaw_override_behaviour == YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT ) { if( g.yaw_override_behaviour == YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT ) {
@ -436,39 +440,36 @@ static bool verify_land()
// verify_nav_wp - check if we have reached the next way point // verify_nav_wp - check if we have reached the next way point
static bool verify_nav_wp() static bool verify_nav_wp()
{ {
// Altitude checking // altitude checking
if(next_WP.options & WP_OPTION_ALT_REQUIRED) { if( !(wp_verify_byte & NAV_ALTITUDE) ) {
// we desire a certain minimum altitude
if(alt_change_flag == REACHED_ALT) { if(alt_change_flag == REACHED_ALT) {
// we have reached that altitude // we have reached that altitude
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;
} }
} }
// Did we pass the WP? // Distance checking // distance checking
if((wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) || check_missed_wp()) { if( !(wp_verify_byte & NAV_LOCATION) ) {
wp_verify_byte |= NAV_LOCATION; if(wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0) || check_missed_wp()) {
if(loiter_time == 0) { wp_verify_byte |= NAV_LOCATION;
loiter_time = millis();
} }
} }
// Hold at Waypoint checking, we cant move on until this is OK // delay checking once we've reached the location
if(wp_verify_byte & NAV_LOCATION) { if( !(wp_verify_byte & NAV_DELAY) && (wp_verify_byte & (NAV_LOCATION | NAV_ALTITUDE)) ) {
// we have reached our goal // start timer if necessary
if(loiter_time == 0) {
loiter_time = millis();
}
// loiter at the WP // check if timer has run out
set_nav_mode(NAV_LOITER); if ((millis() - loiter_time) >= loiter_time_max) {
if ((millis() - loiter_time) > loiter_time_max) {
wp_verify_byte |= NAV_DELAY; wp_verify_byte |= NAV_DELAY;
} }
} }
if( wp_verify_byte == (NAV_LOCATION | NAV_ALTITUDE | NAV_DELAY) ) { if( wp_verify_byte == (NAV_LOCATION | NAV_ALTITUDE | NAV_DELAY) ) {
gcs_send_text_fmt(PSTR("Reached Command #%i"),command_nav_index); gcs_send_text_fmt(PSTR("Reached Command #%i"),command_nav_index);
wp_verify_byte = 0;
copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached
return true; return true;
}else{ }else{

1
ArduCopter/navigation.pde
View File

@ -123,6 +123,7 @@ static void run_autopilot()
break; break;
case GUIDED: case GUIDED:
// switch to loiter once we've reached the target location and altitude // switch to loiter once we've reached the target location and altitude
// To-Do: this incorrectly checks verify_nav_wp even though the nav mode may be NAV_LOITER
if(verify_nav_wp()) { if(verify_nav_wp()) {
set_nav_mode(NAV_LOITER); set_nav_mode(NAV_LOITER);
} }