// check we have required terrain data
bool Copter::pre_arm_terrain_check(bool display_failure)
{
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
// succeed if not using terrain data
if (!terrain_use()) {
return true;
}
// check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range
// To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
if ((rangefinder.num_sensors() > 0) && (g.rtl_altitude > rangefinder.max_distance_cm())) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range");
return false;
}
// show terrain statistics
uint16_t terr_pending, terr_loaded;
terrain.get_statistics(terr_pending, terr_loaded);
bool have_all_data = (terr_pending <= 0);
if (!have_all_data && display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
}
return have_all_data;
#else
return true;
#endif
}
// get_land_descent_speed - high level landing logic
// returns climb rate (in cm/s) which should be passed to the position controller
// should be called at 100hz or higher
float Copter::get_land_descent_speed()
{
#if RANGEFINDER_ENABLED == ENABLED
bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy;
#else
bool rangefinder_ok = false;
#endif
// get position controller's target altitude above terrain
Location_Class target_loc = pos_control.get_pos_target();
int32_t target_alt_cm;
// get altitude target above home by default
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm);
// try to use terrain if enabled
if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) {
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
// if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent
if ((target_alt_cm >= LAND_START_ALT) && !rangefinder_ok) {
if (g.land_speed_high > 0) {
// user has asked for a different landing speed than normal descent rate
return -abs(g.land_speed_high);
}
return pos_control.get_speed_down();
}else{
return -abs(g.land_speed);
}
}
// 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, bool terrain_following_allowed)
{
float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f;
// 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
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_path.terrain_used = 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
// ensure we do not descend
ret = MAX(ret, curr_alt);
// convert return-target to alt-above-home or alt-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_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME);
}
}
// add ret to altitude
rtl_return_target.alt += ret;
}
void Copter::update_precland()
{
int32_t height_above_terrain_cm;
bool rangefinder_height_above_terrain_cm_valid = get_rangefinder_height_above_terrain(height_above_terrain_cm);
// use range finder altitude if it is valid, else try to get terrain alt, else use height above home
if (!rangefinder_height_above_terrain_cm_valid) {
if (!terrain_use() || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_terrain_cm)) {
height_above_terrain_cm = current_loc.alt;
}
}
copter.precland.update(height_above_terrain_cm, rangefinder_height_above_terrain_cm_valid);
}
void Copter::update_precland()
{
int32_t height_above_ground_cm = current_loc.alt;
// use range finder altitude if it is valid, else try to get terrain alt
if (rangefinder_alt_ok()) {
height_above_ground_cm = rangefinder_state.alt_cm;
} else if (terrain_use()) {
if (!current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm)) {
height_above_ground_cm = current_loc.alt;
}
}
copter.precland.update(height_above_ground_cm, rangefinder_alt_ok());
}
// check we have required terrain data
bool Copter::pre_arm_terrain_check()
{
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
// succeed if not using terrain data
if (!terrain_use()) {
return true;
}
// show terrain statistics
uint16_t terr_pending, terr_loaded;
terrain.get_statistics(terr_pending, terr_loaded);
return (terr_pending <= 0);
#else
return true;
#endif
}
void Sub::update_precland()
{
int32_t height_above_ground_cm = current_loc.alt;
// use range finder altitude if it is valid, else try to get terrain alt
if (rangefinder_alt_ok()) {
height_above_ground_cm = rangefinder_state.alt_cm;
} else if (terrain_use()) {
current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm);
}
sub.precland.update(height_above_ground_cm);
// log output
Log_Write_Precland();
}
// compute the return target - home or rally point
// return altitude in cm above home at which vehicle should return home
// 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_target(bool terrain_following_allowed)
{
// set return target to nearest rally point or home position (Note: alt is absolute)
#if AC_RALLY == ENABLED
rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt);
#else
rtl_path.return_target = ahrs.get_home();
#endif
// 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
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_path.origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
!rtl_path.return_target.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_path.terrain_used = false;
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
// convert return-target alt (which is an absolute alt) to alt-above-home or alt-above-terrain
if (!rtl_path.terrain_used || !rtl_path.return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) {
if (!rtl_path.return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) {
// this should never happen but just in case
rtl_path.return_target.set_alt_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME);
}
rtl_path.terrain_used = false;
}
// set new target altitude to return target altitude
// Note: this is alt-above-home or terrain-alt depending upon use_terrain
// Note: ignore negative altitudes which could happen if user enters negative altitude for rally point or terrain is higher at rally point compared to home
int32_t target_alt = MAX(rtl_path.return_target.alt, 0);
// increase target to maximum of current altitude + climb_min and rtl altitude
target_alt = MAX(target_alt, curr_alt + MAX(0, g.rtl_climb_min));
target_alt = MAX(target_alt, MAX(g.rtl_altitude, RTL_ALT_MIN));
// reduce climb if close to return target
float rtl_return_dist_cm = rtl_path.return_target.get_distance(rtl_path.origin_point) * 100.0f;
// don't allow really shallow slopes
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) {
target_alt = MAX(curr_alt, MIN(target_alt, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB)));
}
// set returned target alt to new target_alt
rtl_path.return_target.set_alt_cm(target_alt, rtl_path.terrain_used ? Location_Class::ALT_FRAME_ABOVE_TERRAIN : Location_Class::ALT_FRAME_ABOVE_HOME);
#if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled
// Note: because the rtl_path.climb_target's altitude is simply copied from the return_target's altitude,
// if terrain altitudes are being used, the code below which reduces the return_target's altitude can lead to
// the vehicle not climbing at all as RTL begins. This can be overly conservative and it might be better
// to apply the fence alt limit independently on the origin_point and return_target
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
// get return target as alt-above-home so it can be compared to fence's alt
if (rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) {
float fence_alt = fence.get_safe_alt()*100.0f;
if (target_alt > fence_alt) {
// reduce target alt to the fence alt
rtl_path.return_target.alt -= (target_alt - fence_alt);
}
}
}
#endif
// ensure we do not descend
rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
}
