diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 8859af1903..41b092d130 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -979,7 +979,7 @@ static void update_current_flight_mode(void) switch(nav_command_ID){ case MAV_CMD_NAV_TAKEOFF: - if (hold_course > -1) { + if (hold_course != -1) { calc_nav_roll(); } else { nav_roll = 0; @@ -1012,12 +1012,16 @@ static void update_current_flight_mode(void) nav_pitch = landing_pitch; // pitch held constant } - if (land_complete){ + if (land_complete) { + // we are in the final stage of a landing - force + // zero throttle g.channel_throttle.servo_out = 0; } break; default: + // we are doing normal AUTO flight, the special cases + // are for takeoff and landing hold_course = -1; calc_nav_roll(); calc_nav_pitch(); @@ -1025,11 +1029,13 @@ static void update_current_flight_mode(void) break; } }else{ + // hold_course is only used in takeoff and landing + hold_course = -1; + switch(control_mode){ case RTL: case LOITER: case GUIDED: - hold_course = -1; crash_checker(); calc_nav_roll(); calc_nav_pitch(); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index c3a53ed7b9..6396d28a4d 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -276,7 +276,7 @@ static void do_loiter_time() static bool verify_takeoff() { if (g_gps->ground_speed > 300){ - if(hold_course == -1){ + if (hold_course == -1) { // save our current course to take off if(g.compass_enabled) { hold_course = dcm.yaw_sensor; @@ -286,7 +286,7 @@ static bool verify_takeoff() } } - if(hold_course > -1){ + if (hold_course != -1) { // recalc bearing error with hold_course; nav_bearing = hold_course; // recalc bearing error @@ -302,9 +302,15 @@ static bool verify_takeoff() } } +// we are executing a landing static bool verify_land() { - // we don't verify landing - we never go to a new Nav command after Land + // we don't 'verify' landing in the sense that it never completes, + // so we don't verify command completion. Instead we use this to + // adjust final landing parameters + + // Set land_complete if we are within 2 seconds distance or within + // 3 meters altitude of the landing point if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300)){ @@ -318,7 +324,7 @@ static bool verify_land() } } - if(hold_course > -1){ + if (hold_course != -1){ // recalc bearing error with hold_course; nav_bearing = hold_course; // recalc bearing error