Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Arducopter.pde_RTL:

Browse Source 
added a new RTL function that goes into Loiter, first, checks if we have reached RTL_Altitude, then enters WP mode to come home.
Removes Approach mode. Uses Auto_Approach value to decide if we should land or descend to a certain altitude
This commit is contained in:
Jason Short 2012-07-01 13:30:13 -07:00
parent 8c8f44e6f7
commit e5fbcb629d
1 changed files with 53 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
ArduCopter/ArduCopter.pde
View File

@ -333,8 +333,7 @@ static const char* flight_mode_strings[] = {
"POSITION", // 8
"LAND", // 9
"OF_LOITER", // 10
"APP", // 11
"TOY"}; // 12 THOR Added for additional Fligt mode
"TOY"}; // 11 THOR Added for additional Fligt mode
/* Radio values
Channel assignments
@ -685,6 +684,8 @@ static int16_t landing_boost;
// for controlling the landing throttle curve
//verifies landings
static int16_t ground_detector;
// have we reached our desired altitude brefore heading home?
static bool rtl_reached_alt;
////////////////////////////////////////////////////////////////////////////////
// Toy Mode - THOR
@ -1819,25 +1820,20 @@ static void update_navigation()
break;
case RTL:
// We have reached Home
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if loiter_timer value > 0, we are set to trigger auto_land or approach after 20 seconds
set_mode(LOITER);
// force loitering above home
next_WP.lat = home.lat;
next_WP.lng = home.lng;
// If land is enabled OR failsafe OR auto approach altitude is set
if(g.rtl_land_enabled || failsafe || g.rtl_approach_alt >= 1)
loiter_timer = millis();
else
loiter_timer = 0;
break;
// have we reached the desired Altitude?
if(alt_change_flag <= REACHED_ALT){ // we are at or above the target alt
if(rtl_reached_alt == false){
rtl_reached_alt = true;
do_RTL();
}
wp_control = WP_MODE;
// checks if we have made it to home
update_nav_RTL();
} else{
// we need to loiter until we are ready to come home
wp_control = LOITER_MODE;
}
wp_control = WP_MODE;
slow_wp = true;
// calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME
update_auto_yaw();
@ -1867,7 +1863,7 @@ static void update_navigation()
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
if( g.rc_2.control_in == 0 && g.rc_1.control_in == 0 ){
if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0){
loiter_override = false;
wp_control = LOITER_MODE;
}
@ -1877,19 +1873,19 @@ static void update_navigation()
if(loiter_timer != 0){
// If we have a safe approach alt set and we have been loitering for 20 seconds(default), begin approach
if(g.rtl_approach_alt >= 1 && (millis() - loiter_timer) > (RTL_APPROACH_DELAY * 1000)){
if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()){
// just to make sure we clear the timer
loiter_timer = 0;
set_mode(APPROACH);
}
// Kick us out of loiter and begin landing if the loiter_timer is set
else if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()){
// just to make sure we clear the timer
loiter_timer = 0;
set_mode(LAND);
if(home_distance < 300){
next_WP.lat = home.lat;
next_WP.lng = home.lng;
if(g.rtl_approach_alt == 0){
set_mode(LAND);
if(home_distance < 300){
next_WP.lat = home.lat;
next_WP.lng = home.lng;
}
}else{
if(g.rtl_approach_alt < current_loc.alt){
set_new_altitude(g.rtl_approach_alt);
}
}
}
}
@ -1908,11 +1904,6 @@ static void update_navigation()
update_nav_wp();
break;
case APPROACH:
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case CIRCLE:
yaw_tracking = MAV_ROI_WPNEXT;
wp_control = CIRCLE_MODE;
@ -1953,6 +1944,32 @@ static void update_navigation()
}
}
static void update_nav_RTL()
{
// Have we have reached Home?
if(wp_distance <= g.waypoint_radius){
// if loiter_timer value > 0, we are set to trigger auto_land or approach
set_mode(LOITER);
// just un case we arrive and we aren't at the lower RTL alt yet.
if(current_loc.alt > get_RTL_alt()){
set_new_altitude(get_RTL_alt());
}
// force loitering above home
next_WP.lat = home.lat;
next_WP.lng = home.lng;
// If land is enabled OR failsafe OR auto approach altitude is set
// we will go into automatic land, (g.rtl_approach_alt) is the lowest point
if(failsafe || g.rtl_approach_alt >= 0)
loiter_timer = millis();
else
loiter_timer = 0;
}
slow_wp = true;
}
static void read_AHRS(void)
{
// Perform IMU calculations and get attitude info