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

Copter: RTL supports terrain altitudes

This commit is contained in:
Randy Mackay 2016-03-18 19:44:09 +09:00
parent 06ee6a7bd4
commit 352b6ae82a
2 changed files with 79 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/Copter.h
View File

@ -344,11 +344,11 @@ private:
bool rtl_state_complete; // set to true if the current state is completed
struct {
// NEU w/ origin-relative altitude
Vector3f origin_point;
Vector3f climb_target;
Vector3f return_target;
Vector3f descent_target;
// NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
Location_Class origin_point;
Location_Class climb_target;
Location_Class return_target;
Location_Class descent_target;
bool land;
} rtl_path;
@ -830,7 +830,7 @@ private:
void rtl_land_start();
void rtl_land_run();
void rtl_build_path();
float rtl_compute_return_alt_above_origin(float rtl_return_dist);
void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target);
bool sport_init(bool ignore_checks);
void sport_run();
bool stabilize_init(bool ignore_checks);

109
ArduCopter/control_rtl.cpp
View File

@ -90,7 +90,11 @@ void Copter::rtl_climb_start()
}
// set the destination
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)
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
}
wp_nav.set_fast_waypoint(true);
// hold current yaw during initial climb
@ -103,7 +107,11 @@ void Copter::rtl_return_start()
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
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)
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
}
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
@ -143,7 +151,11 @@ void Copter::rtl_climb_return_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
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?
}
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
@ -210,7 +222,11 @@ void Copter::rtl_loiterathome_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
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?
}
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
@ -310,14 +326,14 @@ void Copter::rtl_descent_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
pos_control.set_alt_target_with_slew(rtl_path.descent_target.z, G_Dt);
pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// check if we've reached within 20cm of final altitude
rtl_state_complete = fabsf(rtl_path.descent_target.z - inertial_nav.get_altitude()) < 20.0f;
rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;
}
// rtl_loiterathome_start - initialise controllers to loiter over home
@ -428,62 +444,83 @@ void Copter::rtl_land_run()
void Copter::rtl_build_path()
{
// origin point is our stopping point
pos_control.get_stopping_point_xy(rtl_path.origin_point);
pos_control.get_stopping_point_z(rtl_path.origin_point);
Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point);
rtl_path.origin_point = Location_Class(stopping_point);
rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME);
// set return target to nearest rally point or home position
#if AC_RALLY == ENABLED
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0);
rtl_path.return_target = pv_location_to_vector(rally_point);
rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt);
#else
rtl_path.return_target = pv_location_to_vector(ahrs.get_home());
rtl_path.return_target = ahrs.get_home();
#endif
Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point;
float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y);
// compute return altitude
rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist);
rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target);
// climb target is above our origin point at the return altitude
rtl_path.climb_target.x = rtl_path.origin_point.x;
rtl_path.climb_target.y = rtl_path.origin_point.y;
rtl_path.climb_target.z = rtl_path.return_target.z;
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());
// descent target is below return target at rtl_alt_final
rtl_path.descent_target.x = rtl_path.return_target.x;
rtl_path.descent_target.y = rtl_path.return_target.y;
rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final);
rtl_path.descent_target = Location_Class(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location_Class::ALT_FRAME_ABOVE_HOME);
// set land flag
rtl_path.land = g.rtl_alt_final <= 0;
}
// return altitude in cm above origin at which vehicle should return home
float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist)
// return altitude in cm above home at which vehicle should return home
// 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)
{
// maximum of current altitude + climb_min and rtl altitude
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f;
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { // don't allow really shallow slopes
ret = MAX(current_loc.alt, MIN(ret, MAX(rtl_return_dist*g.rtl_cone_slope, current_loc.alt+RTL_ABS_MIN_CLIMB)));
// curr_alt is current altitude above home or above terrain depending upon use_terrain
int32_t curr_alt = current_loc.alt;
// decide if we should use terrain altitudes
bool rtl_terrain_use = terrain_use();
if (rtl_terrain_use) {
// 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;
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
// maximum of current altitude + climb_min and rtl altitude
float ret = MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
// don't allow really shallow slopes
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) {
ret = MAX(curr_alt, MIN(ret, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB)));
}
#if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled
// Note: we are assuming the fence alt is the same frame as ret
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
ret = MIN(ret, fence.get_safe_alt()*100.0f);
}
#endif
#if AC_RALLY == ENABLED
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, ret+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
ret = rally_point.alt;
#endif
// ensure we do not descend
ret = MAX(ret, curr_alt);
return pv_alt_above_origin(ret);
// 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_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);
}
}
// add ret to altitude
rtl_return_target.alt += ret;
}