/*
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();
}
/*
update the total angle we have covered in a loiter. Used to support
commands to do N circles of loiter
*/
void Plane::loiter_angle_update(void)
{
static const int32_t lap_check_interval_cd = 3*36000;
const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
if (loiter.sum_cd == 0 && !reached_loiter_target()) {
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0;
} else if (loiter.sum_cd == 0) {
// use 1 cd for initial delta
loiter_delta_cd = 1;
loiter.start_lap_alt_cm = current_loc.alt;
loiter.next_sum_lap_cd = lap_check_interval_cd;
} else {
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
}
loiter.old_target_bearing_cd = target_bearing_cd;
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
loiter.sum_cd += loiter_delta_cd * loiter.direction;
if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
loiter.reached_target_alt = true;
loiter.unable_to_acheive_target_alt = false;
loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;
} else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) {
// check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long
loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500;
loiter.start_lap_alt_cm = current_loc.alt;
loiter.next_sum_lap_cd += lap_check_interval_cd;
}
}
/*
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();
}
bool Plane::verify_loiter_time()
{
bool result = false;
// mission radius is always aparm.loiter_radius
update_loiter(0);
if (loiter.start_time_ms == 0) {
if (reached_loiter_target() && loiter.sum_cd > 1) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
}
} else if (condition_value != 0) {
// primary goal, loiter time
if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
// primary goal completed, initialize secondary heading goal
condition_value = 0;
result = verify_loiter_heading(true);
}
} else {
// secondary goal, loiter to heading
result = verify_loiter_heading(false);
}
if (result) {
gcs_send_text(MAV_SEVERITY_INFO,"Loiter time complete");
auto_state.vtol_loiter = false;
}
return result;
}
/*
work out target altitude adjustment from terrain lookahead
*/
float Plane::lookahead_adjustment(void)
{
#if AP_TERRAIN_AVAILABLE
int32_t bearing_cd;
int16_t distance;
// work out distance and bearing to target
if (control_mode == FLY_BY_WIRE_B) {
// there is no target waypoint in FBWB, so use yaw as an approximation
bearing_cd = ahrs.yaw_sensor;
distance = g.terrain_lookahead;
} else if (!reached_loiter_target()) {
bearing_cd = nav_controller->target_bearing_cd();
distance = constrain_float(auto_state.wp_distance, 0, g.terrain_lookahead);
} else {
// no lookahead when loitering
bearing_cd = 0;
distance = 0;
}
if (distance <= 0) {
// no lookahead
return 0;
}
float groundspeed = ahrs.groundspeed();
if (groundspeed < 1) {
// we're not moving
return 0;
}
// we need to know the climb ratio. We use 50% of the maximum
// climb rate so we are not constantly at 100% throttle and to
// give a bit more margin on terrain
float climb_ratio = 0.5f * SpdHgt_Controller->get_max_climbrate() / groundspeed;
if (climb_ratio <= 0) {
// lookahead makes no sense for negative climb rates
return 0;
}
// ask the terrain code for the lookahead altitude change
float lookahead = terrain.lookahead(bearing_cd*0.01f, distance, climb_ratio);
if (target_altitude.offset_cm < 0) {
// we are heading down to the waypoint, so we don't need to
// climb as much
lookahead += target_altitude.offset_cm*0.01f;
}
// constrain lookahead to a reasonable limit
return constrain_float(lookahead, 0, 1000.0f);
#else
return 0;
#endif
}
void Plane::update_loiter(uint16_t radius)
{
if (radius <= 1) {
// if radius is <=1 then use the general loiter radius. if it's small, use default
radius = (abs(aparm.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(aparm.loiter_radius);
if (next_WP_loc.loiter_ccw == 1) {
loiter.direction = -1;
} else {
loiter.direction = (aparm.loiter_radius < 0) ? -1 : 1;
}
}
if (loiter.start_time_ms != 0 &&
quadplane.guided_mode_enabled()) {
if (!auto_state.vtol_loiter) {
auto_state.vtol_loiter = true;
// reset loiter start time, so we don't consider the point
// reached till we get much closer
loiter.start_time_ms = 0;
quadplane.guided_start();
}
} else if ((loiter.start_time_ms == 0 &&
(control_mode == &mode_auto || control_mode == &mode_guided) &&
auto_state.crosstrack &&
current_loc.get_distance(next_WP_loc) > radius*3) ||
(control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == 1)) {
/*
if never reached loiter point and using crosstrack and somewhat far away from loiter point
navigate to it like in auto-mode for normal crosstrack behavior
we also use direct waypoint navigation if we are a quadplane
that is going to be switching to QRTL when it gets within
RTL_RADIUS
*/
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
} else {
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
}
if (loiter.start_time_ms == 0) {
if (reached_loiter_target() ||
auto_state.wp_proportion > 1) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
if (control_mode == &mode_guided || control_mode == &mode_avoidADSB) {
// starting a loiter in GUIDED means we just reached the target point
gcs().send_mission_item_reached_message(0);
}
if (quadplane.guided_mode_enabled()) {
quadplane.guided_start();
}
}
}
}
void Plane::update_loiter(uint16_t radius)
{
if (radius <= 1) {
// if radius is <=1 then use the general loiter radius. if it's small, use default
radius = (abs(g.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(g.loiter_radius);
if (next_WP_loc.flags.loiter_ccw == 1) {
loiter.direction = -1;
} else {
loiter.direction = (g.loiter_radius < 0) ? -1 : 1;
}
}
if (loiter.start_time_ms != 0 &&
quadplane.available() &&
quadplane.guided_mode != 0) {
if (!auto_state.vtol_loiter) {
auto_state.vtol_loiter = true;
// reset loiter start time, so we don't consider the point
// reached till we get much closer
loiter.start_time_ms = 0;
quadplane.guided_start();
}
} else if (loiter.start_time_ms == 0 &&
control_mode == AUTO &&
!auto_state.no_crosstrack &&
get_distance(current_loc, next_WP_loc) > radius*2) {
// if never reached loiter point and using crosstrack and somewhat far away from loiter point
// navigate to it like in auto-mode for normal crosstrack behavior
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
} else {
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
}
if (loiter.start_time_ms == 0) {
if (reached_loiter_target() ||
auto_state.wp_proportion > 1) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
if (control_mode == GUIDED) {
// starting a loiter in GUIDED means we just reached the target point
gcs_send_mission_item_reached_message(0);
}
if (quadplane.available() &&
quadplane.guided_mode != 0) {
quadplane.guided_start();
}
}
}
}
bool Plane::verify_RTL()
{
if (g.rtl_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
update_loiter(abs(g.rtl_radius));
if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) ||
reached_loiter_target()) {
gcs_send_text(MAV_SEVERITY_INFO,"Reached RTL location");
return true;
} else {
return false;
}
}
/*
update the total angle we have covered in a loiter. Used to support
commands to do N circles of loiter
*/
void Plane::loiter_angle_update(void)
{
int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
if (loiter.sum_cd == 0 && !reached_loiter_target()) {
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0;
} else if (loiter.sum_cd == 0) {
// use 1 cd for initial delta
loiter_delta_cd = 1;
} else {
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
}
loiter.old_target_bearing_cd = target_bearing_cd;
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
loiter.sum_cd += loiter_delta_cd * loiter.direction;
}
void Plane::update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
uint16_t radius = 0;
switch(control_mode) {
case AUTO:
if (home_is_set != HOME_UNSET) {
mission.update();
}
break;
case RTL:
if (quadplane.available() && quadplane.rtl_mode == 1 &&
nav_controller->reached_loiter_target()) {
set_mode(QRTL);
break;
} else if (g.rtl_autoland == 1 &&
!auto_state.checked_for_autoland &&
reached_loiter_target() &&
labs(altitude_error_cm) < 1000) {
// we've reached the RTL point, see if we have a landing sequence
jump_to_landing_sequence();
// prevent running the expensive jump_to_landing_sequence
// on every loop
auto_state.checked_for_autoland = true;
}
else if (g.rtl_autoland == 2 &&
!auto_state.checked_for_autoland) {
// Go directly to the landing sequence
jump_to_landing_sequence();
// prevent running the expensive jump_to_landing_sequence
// on every loop
auto_state.checked_for_autoland = true;
}
radius = abs(g.rtl_radius);
if (radius > 0) {
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
}
// fall through to LOITER
case LOITER:
case GUIDED:
update_loiter(radius);
break;
case CRUISE:
update_cruise();
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case INITIALISING:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CIRCLE:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
// nothing to do
break;
}
}
void Plane::update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
uint16_t radius = 0;
uint16_t qrtl_radius = abs(g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(aparm.loiter_radius);
}
switch(control_mode) {
case AUTO:
if (ahrs.home_is_set()) {
mission.update();
}
break;
case RTL:
if (quadplane.available() && quadplane.rtl_mode == 1 &&
(nav_controller->reached_loiter_target() ||
location_passed_point(current_loc, prev_WP_loc, next_WP_loc) ||
auto_state.wp_distance < MAX(qrtl_radius, quadplane.stopping_distance())) &&
AP_HAL::millis() - last_mode_change_ms > 1000) {
/*
for a quadplane in RTL mode we switch to QRTL when we
are within the maximum of the stopping distance and the
RTL_RADIUS
*/
set_mode(QRTL, MODE_REASON_UNKNOWN);
break;
} else if (g.rtl_autoland == 1 &&
!auto_state.checked_for_autoland &&
reached_loiter_target() &&
labs(altitude_error_cm) < 1000) {
// we've reached the RTL point, see if we have a landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
set_mode(AUTO, MODE_REASON_UNKNOWN);
}
// prevent running the expensive jump_to_landing_sequence
// on every loop
auto_state.checked_for_autoland = true;
}
else if (g.rtl_autoland == 2 &&
!auto_state.checked_for_autoland) {
// Go directly to the landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
set_mode(AUTO, MODE_REASON_UNKNOWN);
}
// prevent running the expensive jump_to_landing_sequence
// on every loop
auto_state.checked_for_autoland = true;
}
radius = abs(g.rtl_radius);
if (radius > 0) {
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
}
// fall through to LOITER
FALLTHROUGH;
case LOITER:
case AVOID_ADSB:
case GUIDED:
update_loiter(radius);
break;
case CRUISE:
update_cruise();
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case INITIALISING:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CIRCLE:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
// nothing to do
break;
}
}
