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