aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM: added some comments related to hold_course

Browse Source 
hold_course is either -1 (for disabled) or a course to hold for
takeoff/landing. This makes the code a bit clearer.

It also resets hold_course in all non-auto modes, to ensure it isn't
used
This commit is contained in:
Andrew Tridgell 2012-02-15 08:44:33 +11:00
parent 3e7cc2499b
commit 330ff5dc8b
2 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/ArduPlane.pde
View File

@ -979,7 +979,7 @@ static void update_current_flight_mode(void)
switch(nav_command_ID){ switch(nav_command_ID){
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
if (hold_course > -1) { if (hold_course != -1) {
calc_nav_roll(); calc_nav_roll();
} else { } else {
nav_roll = 0; nav_roll = 0;
@ -1012,12 +1012,16 @@ static void update_current_flight_mode(void)
nav_pitch = landing_pitch; // pitch held constant 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; g.channel_throttle.servo_out = 0;
} }
break; break;
default: default:
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
hold_course = -1; hold_course = -1;
calc_nav_roll(); calc_nav_roll();
calc_nav_pitch(); calc_nav_pitch();
@ -1025,11 +1029,13 @@ static void update_current_flight_mode(void)
break; break;
} }
}else{ }else{
// hold_course is only used in takeoff and landing
hold_course = -1;
switch(control_mode){ switch(control_mode){
case RTL: case RTL:
case LOITER: case LOITER:
case GUIDED: case GUIDED:
hold_course = -1;
crash_checker(); crash_checker();
calc_nav_roll(); calc_nav_roll();
calc_nav_pitch(); calc_nav_pitch();

14
ArduPlane/commands_logic.pde
View File

@ -276,7 +276,7 @@ static void do_loiter_time()
static bool verify_takeoff() static bool verify_takeoff()
{ {
if (g_gps->ground_speed > 300){ if (g_gps->ground_speed > 300){
if(hold_course == -1){ if (hold_course == -1) {
// save our current course to take off // save our current course to take off
if(g.compass_enabled) { if(g.compass_enabled) {
hold_course = dcm.yaw_sensor; 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; // recalc bearing error with hold_course;
nav_bearing = hold_course; nav_bearing = hold_course;
// recalc bearing error // recalc bearing error
@ -302,9 +302,15 @@ static bool verify_takeoff()
} }
} }
// we are executing a landing
static bool verify_land() 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))) if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
|| (current_loc.alt <= next_WP.alt + 300)){ || (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; // recalc bearing error with hold_course;
nav_bearing = hold_course; nav_bearing = hold_course;
// recalc bearing error // recalc bearing error