Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: RTL handles terrain data failure

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

52
ArduCopter/control_rtl.cpp
View File

@ -13,7 +13,7 @@
bool Copter::rtl_init(bool ignore_checks) bool Copter::rtl_init(bool ignore_checks)
{ {
if (position_ok() || ignore_checks) { if (position_ok() || ignore_checks) {
rtl_build_path(); rtl_build_path(true);
rtl_climb_start(); rtl_climb_start();
return true; return true;
}else{ }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 // rtl_run - runs the return-to-launch controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::rtl_run() void Copter::rtl_run()
@ -91,9 +103,10 @@ void Copter::rtl_climb_start()
// set the destination // set the destination
if (!wp_nav.set_wp_destination(rtl_path.climb_target)) { 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); 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); wp_nav.set_fast_waypoint(true);
@ -108,9 +121,8 @@ void Copter::rtl_return_start()
rtl_state_complete = false; rtl_state_complete = false;
if (!wp_nav.set_wp_destination(rtl_path.return_target)) { if (!wp_nav.set_wp_destination(rtl_path.return_target)) {
// failure to set destination (likely because of missing terrain data) // failure must be caused by missing terrain data, restart RTL
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); rtl_restart_without_terrain();
// To-Do: handle failure
} }
// initialise yaw to point home (maybe) // 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); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller // run waypoint controller
if (!wp_nav.update_wpnav()) { failsafe_terrain_set_status(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?
}
// call z-axis position controller (wpnav should have already updated it's alt target) // call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller(); pos_control.update_z_controller();
@ -173,7 +181,7 @@ void Copter::rtl_climb_return_run()
rtl_state_complete = wp_nav.reached_wp_destination(); 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() void Copter::rtl_loiterathome_start()
{ {
rtl_state = RTL_LoiterAtHome; rtl_state = RTL_LoiterAtHome;
@ -222,11 +230,7 @@ void Copter::rtl_loiterathome_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller // run waypoint controller
if (!wp_nav.update_wpnav()) { failsafe_terrain_set_status(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?
}
// call z-axis position controller (wpnav should have already updated it's alt target) // call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller(); pos_control.update_z_controller();
@ -441,7 +445,7 @@ void Copter::rtl_land_run()
rtl_state_complete = ap.land_complete; 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 // origin point is our stopping point
Vector3f stopping_point; Vector3f stopping_point;
@ -458,7 +462,7 @@ void Copter::rtl_build_path()
#endif #endif
// compute return altitude // 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 // 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()); 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_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 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) // 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; 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; int32_t curr_alt = current_loc.alt;
// decide if we should use terrain altitudes // decide if we should use terrain altitudes
bool rtl_terrain_use = terrain_use(); rtl_path.terrain_used = terrain_use() && terrain_following_allowed;
if (rtl_terrain_use) { if (rtl_path.terrain_used) {
// attempt to retrieve terrain alt for current location, stopping point and origin // attempt to retrieve terrain alt for current location, stopping point and origin
int32_t origin_terr_alt, return_target_terr_alt; 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) || 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) || !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)) { !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); 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); ret = MAX(ret, curr_alt);
// convert return-target to alt-above-home or alt-above-terrain // 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)) { if (!rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) {
// this should never happen but just in case // this should never happen but just in case
rtl_return_target.set_alt(0, Location_Class::ALT_FRAME_ABOVE_HOME); 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 #define ERROR_CODE_MISSING_TERRAIN_DATA 2
// subsystem specific error codes -- navigation // subsystem specific error codes -- navigation
#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2 #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 // parachute failed to deploy because of low altitude or landed
#define ERROR_CODE_PARACHUTE_TOO_LOW 2 #define ERROR_CODE_PARACHUTE_TOO_LOW 2