/* 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(); }
/* adjust altitude target depending on mode */ void Plane::adjust_altitude_target() { Location target_location; if (control_mode == FLY_BY_WIRE_B || control_mode == CRUISE) { return; } if (landing.is_flaring()) { // during a landing flare, use TECS_LAND_SINK as a target sink // rate, and ignores the target altitude set_target_altitude_location(next_WP_loc); } else if (landing.is_on_approach()) { landing.setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude.offset_cm); landing.adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, auto_state.wp_distance, target_altitude.offset_cm); } else if (landing.get_target_altitude_location(target_location)) { set_target_altitude_location(target_location); } 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(); }
/* a special glide slope calculation for the landing approach During the land approach use a linear glide slope to a point projected through the landing point. We don't use the landing point itself as that leads to discontinuities close to the landing point, which can lead to erratic pitch control */ void Plane::setup_landing_glide_slope(void) { float total_distance = get_distance(prev_WP_loc, next_WP_loc); // If someone mistakenly puts all 0's in their LAND command then total_distance // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. if (total_distance < 1) { total_distance = 1; } // height we need to sink for this WP float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f; // current ground speed float groundspeed = ahrs.groundspeed(); if (groundspeed < 0.5f) { groundspeed = 0.5f; } // calculate time to lose the needed altitude float sink_time = total_distance / groundspeed; if (sink_time < 0.5f) { sink_time = 0.5f; } // find the sink rate needed for the target location float sink_rate = sink_height / sink_time; // the height we aim for is the one to give us the right flare point float aim_height = aparm.land_flare_sec * sink_rate; if (aim_height <= 0) { aim_height = g.land_flare_alt; } // don't allow the aim height to be too far above LAND_FLARE_ALT if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) { aim_height = g.land_flare_alt*2; } // calculate slope to landing point bool is_first_calc = is_zero(auto_state.land_slope); auto_state.land_slope = (sink_height - aim_height) / total_distance; if (is_first_calc) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(auto_state.land_slope))); } // time before landing that we will flare float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate(); // distance to flare is based on ground speed, adjusted as we // get closer. This takes into account the wind float flare_distance = groundspeed * flare_time; // don't allow the flare before half way along the final leg if (flare_distance > total_distance/2) { flare_distance = total_distance/2; } // project a point 500 meters past the landing point, passing // through the landing point const float land_projection = 500; int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); // now calculate our aim point, which is before the landing // point and above it Location loc = next_WP_loc; location_update(loc, land_bearing_cd*0.01f, -flare_distance); loc.alt += aim_height*100; // calculate point along that slope 500m ahead location_update(loc, land_bearing_cd*0.01f, land_projection); loc.alt -= auto_state.land_slope * land_projection * 100; // setup the offset_cm for set_target_altitude_proportion() target_altitude.offset_cm = loc.alt - prev_WP_loc.alt; // calculate the proportion we are to the target float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc); // now setup the glide slope for landing set_target_altitude_proportion(loc, 1.0f - land_proportion); // stay within the range of the start and end locations in altitude constrain_target_altitude_location(loc, prev_WP_loc); }