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

Made RTL hold position until it reaches altitude

This commit is contained in:
Jason Short 2012-01-07 22:21:31 -08:00
parent b8bcd81b39
commit ca80dc549c
1 changed files with 30 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
ArduCopter/ArduCopter.pde
View File

@ -1390,15 +1390,17 @@ void update_roll_pitch_mode(void)
if(do_simple && new_radio_frame){ if(do_simple && new_radio_frame){
update_simple_mode(); update_simple_mode();
} }
#if WIND_COMP_STAB == 1 #if WIND_COMP_STAB == 1
// in this mode, nav_roll and nav_pitch = the iterm // in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll); g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch); g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
#else #else
// in this mode, nav_roll and nav_pitch = the iterm // in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
#endif #endif
break; break;
case ROLL_PITCH_AUTO: case ROLL_PITCH_AUTO:
@ -1612,6 +1614,10 @@ static void update_navigation()
set_mode(LOITER); set_mode(LOITER);
auto_land_timer = millis(); auto_land_timer = millis();
}else if(current_loc.alt < (next_WP.alt - 300)){
// don't navigate if we are below our target alt
wp_control = LOITER_MODE;
}else{ }else{
// calculates desired Yaw // calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
@ -1655,10 +1661,13 @@ static void update_navigation()
case LAND: case LAND:
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
if (current_loc.alt < 250){ if(verify_land()) { // JLN fix for auto land in RTL
wp_control = NO_NAV_MODE; set_mode(STABILIZE);
next_WP.alt = -200; // force us down } else {
// calculates the desired Roll and Pitch
update_nav_wp();
} }
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
break; break;
@ -2012,14 +2021,19 @@ static void update_nav_wp()
}else if(wp_control == NO_NAV_MODE){ }else if(wp_control == NO_NAV_MODE){
// calc the Iterms for Loiter based on velocity // calc the Iterms for Loiter based on velocity
//if ((x_actual_speed + y_actual_speed) == 0) #if WIND_COMP_STAB == 1
if (g_gps->ground_speed < 50) if (g_gps->ground_speed < 50)
calc_wind_compensation(); calc_wind_compensation();
else else
reduce_wind_compensation(); reduce_wind_compensation();
// rotate nav_lat, nav_lon to roll and pitch // rotate nav_lat, nav_lon to roll and pitch
calc_loiter_pitch_roll(); calc_loiter_pitch_roll();
#else
// clear out our nav so we can do things like land straight
nav_pitch = 0;
nav_roll = 0;
#endif
} }
} }