/*
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();
}
/*
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();
}
/*
* set_next_WP - sets the target location the vehicle should fly to
*/
void Plane::set_next_WP(const struct Location &loc)
{
if (auto_state.next_wp_no_crosstrack) {
// we should not try to cross-track for this waypoint
prev_WP_loc = current_loc;
// use cross-track for the next waypoint
auto_state.next_wp_no_crosstrack = false;
auto_state.no_crosstrack = true;
} else {
// copy the current WP into the OldWP slot
prev_WP_loc = next_WP_loc;
auto_state.no_crosstrack = false;
}
// Load the next_WP slot
// ---------------------
next_WP_loc = loc;
// if lat and lon is zero, then use current lat/lon
// this allows a mission to contain a "loiter on the spot"
// command
if (next_WP_loc.lat == 0 && next_WP_loc.lng == 0) {
next_WP_loc.lat = current_loc.lat;
next_WP_loc.lng = current_loc.lng;
// additionally treat zero altitude as current altitude
if (next_WP_loc.alt == 0) {
next_WP_loc.alt = current_loc.alt;
next_WP_loc.flags.relative_alt = false;
next_WP_loc.flags.terrain_alt = false;
}
}
// convert relative alt to absolute alt
if (next_WP_loc.flags.relative_alt) {
next_WP_loc.flags.relative_alt = false;
next_WP_loc.alt += home.alt;
}
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
// past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately
// considering the waypoint complete
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint");
prev_WP_loc = current_loc;
}
// used to control FBW and limit the rate of climb
// -----------------------------------------------
set_target_altitude_location(next_WP_loc);
// zero out our loiter vals to watch for missed waypoints
loiter_angle_reset();
setup_glide_slope();
setup_turn_angle();
loiter_angle_reset();
}
/*
change target altitude by a proportion of the target altitude offset
(difference in height to next WP from previous WP). proportion
should be between 0 and 1.
When proportion is zero we have reached the destination. When
proportion is 1 we are at the starting waypoint.
Note that target_altitude is setup initially based on the
destination waypoint
*/
void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
{
set_target_altitude_location(loc);
proportion = constrain_float(proportion, 0.0f, 1.0f);
change_target_altitude(-target_altitude.offset_cm*proportion);
//rebuild the glide slope if we are above it and supposed to be climbing
if(g.glide_slope_threshold > 0) {
if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) {
set_target_altitude_location(loc);
set_offset_altitude_location(loc);
change_target_altitude(-target_altitude.offset_cm*proportion);
//adjust the new target offset altitude to reflect that we are partially already done
if(proportion > 0.0f)
target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
}
}
}
void Plane::do_RTL(int32_t rtl_altitude)
{
auto_state.next_wp_no_crosstrack = true;
auto_state.no_crosstrack = true;
prev_WP_loc = current_loc;
next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude);
setup_terrain_target_alt(next_WP_loc);
set_target_altitude_location(next_WP_loc);
if (aparm.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
setup_glide_slope();
setup_turn_angle();
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
}