/*
adjust altitude target depending on mode
*/
void Plane::adjust_altitude_target()
{
if (control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE) {
return;
}
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
// in land final TECS uses TECS_LAND_SINK as a target sink
// rate, and ignores the target altitude
set_target_altitude_location(next_WP_loc);
} else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) {
setup_landing_glide_slope();
adjust_landing_slope_for_rangefinder_bump();
} else if (reached_loiter_target()) {
// once we reach a loiter target then lock to the final
// altitude target
set_target_altitude_location(next_WP_loc);
} else if (target_altitude.offset_cm != 0 &&
!location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
// control climb/descent rate
set_target_altitude_proportion(next_WP_loc, 1.0f-auto_state.wp_proportion);
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location(next_WP_loc, prev_WP_loc);
} else {
set_target_altitude_location(next_WP_loc);
}
altitude_error_cm = calc_altitude_error_cm();
}
void Plane::adjust_landing_slope_for_rangefinder_bump(void)
{
#if RANGEFINDER_ENABLED == ENABLED
// check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by
// determining the slope from your current location to the land point then following that back up to the approach
// altitude and moving the prev_wp to that location. From there
float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction);
if (g.land_slope_recalc_shallow_threshold <= 0 ||
fabsf(correction_delta) < g.land_slope_recalc_shallow_threshold) {
return;
}
rangefinder_state.last_stable_correction = rangefinder_state.correction;
float corrected_alt_m = (adjusted_altitude_cm() - next_WP_loc.alt)*0.01f - rangefinder_state.correction;
float total_distance_m = get_distance(prev_WP_loc, next_WP_loc);
float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / auto_state.wp_distance;
prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt;
// re-calculate auto_state.land_slope with updated prev_WP_loc
setup_landing_glide_slope();
if (rangefinder_state.correction >= 0) { // we're too low or object is below us
// correction positive means we're too low so we should continue on with
// the newly computed shallower slope instead of pitching/throttling up
} else if (g.land_slope_recalc_steep_threshold_to_abort > 0) {
// correction negative means we're too high and need to point down (and speed up) to re-align
// to land on target. A large negative correction means we would have to dive down a lot and will
// generating way too much speed that we can not bleed off in time. It is better to remember
// the large baro altitude offset and abort the landing to come around again with the correct altitude
// offset and "perfect" slope.
// calculate projected slope with projected alt
float new_slope_deg = degrees(atan(auto_state.land_slope));
float initial_slope_deg = degrees(atan(auto_state.initial_land_slope));
// is projected slope too steep?
if (new_slope_deg - initial_slope_deg > g.land_slope_recalc_steep_threshold_to_abort) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
auto_state.land_alt_offset = rangefinder_state.correction;
auto_state.commanded_go_around = 1;
g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once
}
}
#endif
}
void AP_Landing::type_slope_adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float wp_distance, int32_t &target_altitude_offset_cm)
{
// check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by
// determining the slope from your current location to the land point then following that back up to the approach
// altitude and moving the prev_wp to that location. From there
float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction);
if (slope_recalc_shallow_threshold <= 0 ||
fabsf(correction_delta) < slope_recalc_shallow_threshold) {
return;
}
rangefinder_state.last_stable_correction = rangefinder_state.correction;
float corrected_alt_m = (adjusted_altitude_cm_fn() - next_WP_loc.alt)*0.01f - rangefinder_state.correction;
float total_distance_m = get_distance(prev_WP_loc, next_WP_loc);
float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / wp_distance;
prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt;
// re-calculate auto_state.land_slope with updated prev_WP_loc
setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
if (rangefinder_state.correction >= 0) { // we're too low or object is below us
// correction positive means we're too low so we should continue on with
// the newly computed shallower slope instead of pitching/throttling up
} else if (slope_recalc_steep_threshold_to_abort > 0 && !has_aborted_due_to_slope_recalc) {
// correction negative means we're too high and need to point down (and speed up) to re-align
// to land on target. A large negative correction means we would have to dive down a lot and will
// generating way too much speed that we can not bleed off in time. It is better to remember
// the large baro altitude offset and abort the landing to come around again with the correct altitude
// offset and "perfect" slope.
// calculate projected slope with projected alt
float new_slope_deg = degrees(atan(slope));
float initial_slope_deg = degrees(atan(initial_slope));
// is projected slope too steep?
if (new_slope_deg - initial_slope_deg > slope_recalc_steep_threshold_to_abort) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing slope too steep, aborting (%.0fm %.1fdeg)",
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
alt_offset = rangefinder_state.correction;
commanded_go_around = true;
has_aborted_due_to_slope_recalc = true; // only allow this once.
}
}
}
