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

Copter: RTL handles terrain data failure

This commit is contained in:
Randy Mackay 2016-04-22 20:44:57 +09:00
parent 47658fe964
commit 43d14defd4
3 changed files with 33 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/Copter.h
View File

@ -353,6 +353,7 @@ private:
Location_Class return_target;
Location_Class descent_target;
bool land;
bool terrain_used;
} rtl_path;
// Circle
@ -823,6 +824,7 @@ private:
bool throw_height_good();
bool rtl_init(bool ignore_checks);
void rtl_restart_without_terrain();
void rtl_run();
void rtl_climb_start();
void rtl_return_start();
@ -833,8 +835,8 @@ private:
void rtl_descent_run();
void rtl_land_start();
void rtl_land_run();
void rtl_build_path();
void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target);
void rtl_build_path(bool terrain_following_allowed);
void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed);
bool sport_init(bool ignore_checks);
void sport_run();
bool stabilize_init(bool ignore_checks);

52
ArduCopter/control_rtl.cpp
View File

@ -13,7 +13,7 @@
bool Copter::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_build_path();
rtl_build_path(true);
rtl_climb_start();
return true;
}else{
@ -21,6 +21,18 @@ bool Copter::rtl_init(bool ignore_checks)
}
}
// re-start RTL with terrain following disabled
void Copter::rtl_restart_without_terrain()
{
// log an error
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
if (rtl_path.terrain_used) {
rtl_build_path(false);
rtl_climb_start();
gcs_send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
}
}
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::rtl_run()
@ -91,9 +103,10 @@ void Copter::rtl_climb_start()
// set the destination
if (!wp_nav.set_wp_destination(rtl_path.climb_target)) {
// failure to set destination (likely because of missing terrain data)
// this should not happen because rtl_build_path will have checked terrain data was available
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
return;
}
wp_nav.set_fast_waypoint(true);
@ -108,9 +121,8 @@ void Copter::rtl_return_start()
rtl_state_complete = false;
if (!wp_nav.set_wp_destination(rtl_path.return_target)) {
// failure to set destination (likely because of missing terrain data)
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
// failure must be caused by missing terrain data, restart RTL
rtl_restart_without_terrain();
}
// initialise yaw to point home (maybe)
@ -151,11 +163,7 @@ void Copter::rtl_climb_return_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
if (!wp_nav.update_wpnav()) {
// failures to update probably caused by missing terrain data
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_UPDATE_TARGET);
// To-Do: handle failure to update probably caused by lack of terrain data by removing need for terrain data?
}
failsafe_terrain_set_status(wp_nav.update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
@ -173,7 +181,7 @@ void Copter::rtl_climb_return_run()
rtl_state_complete = wp_nav.reached_wp_destination();
}
// rtl_return_start - initialise return to home
// rtl_loiterathome_start - initialise return to home
void Copter::rtl_loiterathome_start()
{
rtl_state = RTL_LoiterAtHome;
@ -222,11 +230,7 @@ void Copter::rtl_loiterathome_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
if (!wp_nav.update_wpnav()) {
// failures to update probably caused by missing terrain data
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_UPDATE_TARGET);
// To-Do: handle failure to update probably caused by lack of terrain data by removing need for terrain data?
}
failsafe_terrain_set_status(wp_nav.update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
@ -441,7 +445,7 @@ void Copter::rtl_land_run()
rtl_state_complete = ap.land_complete;
}
void Copter::rtl_build_path()
void Copter::rtl_build_path(bool terrain_following_allowed)
{
// origin point is our stopping point
Vector3f stopping_point;
@ -458,7 +462,7 @@ void Copter::rtl_build_path()
#endif
// compute return altitude
rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target);
rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target, terrain_following_allowed);
// climb target is above our origin point at the return altitude
rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
@ -474,7 +478,7 @@ void Copter::rtl_build_path()
// rtl_origin_point is the stopping point of the vehicle when rtl is initiated
// rtl_return_target is the home or rally point that the vehicle is returning to. It's lat, lng and alt values must already have been filled in before this function is called
// rtl_return_target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target)
void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed)
{
float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f;
@ -482,14 +486,14 @@ void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Loca
int32_t curr_alt = current_loc.alt;
// decide if we should use terrain altitudes
bool rtl_terrain_use = terrain_use();
if (rtl_terrain_use) {
rtl_path.terrain_used = terrain_use() && terrain_following_allowed;
if (rtl_path.terrain_used) {
// attempt to retrieve terrain alt for current location, stopping point and origin
int32_t origin_terr_alt, return_target_terr_alt;
if (!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) ||
!current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
rtl_terrain_use = false;
rtl_path.terrain_used = false;
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
@ -514,7 +518,7 @@ void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Loca
ret = MAX(ret, curr_alt);
// convert return-target to alt-above-home or alt-above-terrain
if (!rtl_terrain_use || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) {
if (!rtl_path.terrain_used || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) {
if (!rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) {
// this should never happen but just in case
rtl_return_target.set_alt(0, Location_Class::ALT_FRAME_ABOVE_HOME);

2
ArduCopter/defines.h
View File

@ -406,7 +406,7 @@ enum ThrowModeState {
#define ERROR_CODE_MISSING_TERRAIN_DATA 2
// subsystem specific error codes -- navigation
#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2
#define ERROR_CODE_FAILED_TO_UPDATE_TARGET 3
#define ERROR_CODE_RESTARTED_RTL 3
// parachute failed to deploy because of low altitude or landed
#define ERROR_CODE_PARACHUTE_TOO_LOW 2