bool Plane::verify_loiter_unlim()
{
if (mission.get_current_nav_cmd().p1 <= 1 && abs(g.rtl_radius) > 1) {
// if mission radius is 0,1, and rtl_radius is valid, use rtl_radius.
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
update_loiter(abs(g.rtl_radius));
} else {
// else use mission radius
update_loiter(mission.get_current_nav_cmd().p1);
}
return false;
}
/*
verify a LOITER_TO_ALT command. This involves checking we have
reached both the desired altitude and desired heading. The desired
altitude only needs to be reached once.
*/
bool Plane::verify_loiter_to_alt()
{
bool result = false;
update_loiter(mission.get_current_nav_cmd().p1);
// condition_value == 0 means alt has never been reached
if (condition_value == 0) {
// primary goal, loiter to alt
if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_acheive_target_alt)) {
// primary goal completed, initialize secondary heading goal
if (loiter.unable_to_acheive_target_alt) {
gcs_send_text_fmt(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", current_loc.alt/100);
}
condition_value = 1;
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 to alt complete");
}
return result;
}
bool Plane::verify_loiter_turns()
{
bool result = false;
uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
update_loiter(radius);
// LOITER_TURNS makes no sense as VTOL
auto_state.vtol_loiter = false;
if (condition_value != 0) {
// primary goal, loiter time
if (loiter.sum_cd > loiter.total_cd && loiter.sum_cd > 1) {
// 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 orbits complete");
}
return result;
}
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;
}
/*
verify a LOITER_TO_ALT command. This involves checking we have
reached both the desired altitude and desired heading. The desired
altitude only needs to be reached once.
*/
bool Plane::verify_loiter_to_alt()
{
update_loiter();
//has target altitude been reached?
if (condition_value != 0) {
if (labs(condition_value - current_loc.alt) < 500) {
//Only have to reach the altitude once -- that's why I need
//this global condition variable.
//
//This is in case of altitude oscillation when still trying
//to reach the target heading.
condition_value = 0;
} else {
return false;
}
}
//has target heading been reached?
if (condition_value2 != 0) {
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
next_nav_cmd)) {
//no next waypoint to shoot for -- go ahead and break out of loiter
return true;
}
// Bearing in radians
int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);
// get current heading. We should probably be using the ground
// course instead to improve the accuracy in wind
int32_t heading_cd = ahrs.yaw_sensor;
int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
/*
Check to see if the the plane is heading toward the land
waypoint
We use 10 degrees of slop so that we can handle 100
degrees/second of yaw
*/
if (abs(heading_err_cd) <= 1000) {
//Want to head in a straight line from _here_ to the next waypoint.
//DON'T want to head in a line from the center of the loiter to
//the next waypoint.
//Therefore: mark the "last" (next_wp_loc is about to be updated)
//wp lat/lon as the current location.
next_WP_loc = current_loc;
return true;
} else {
return false;
}
}
return true;
}
bool Plane::verify_RTL()
{
update_loiter();
if (auto_state.wp_distance <= (uint32_t)max(g.waypoint_radius,0) ||
nav_controller->reached_loiter_target()) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
return true;
} else {
return false;
}
}
bool Plane::verify_loiter_turns()
{
update_loiter();
if (loiter.sum_cd > loiter.total_cd) {
loiter.total_cd = 0;
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
// clear the command queue;
return true;
}
return false;
}
bool Plane::verify_RTL()
{
update_loiter();
if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) ||
nav_controller->reached_loiter_target()) {
gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME");
return true;
} else {
return false;
}
}
bool Plane::verify_loiter_turns()
{
update_loiter();
if (loiter.sum_cd > loiter.total_cd) {
loiter.total_cd = 0;
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
// clear the command queue;
return true;
}
return false;
}
bool Plane::verify_loiter_time()
{
update_loiter();
if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
}
} else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
return true;
}
return false;
}
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;
}
}
bool Plane::verify_loiter_unlim()
{
update_loiter();
return false;
}
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;
}
}
void Plane::update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
// distance and bearing calcs only
switch(control_mode) {
case AUTO:
update_commands();
break;
case RTL:
if (g.rtl_autoland == 1 &&
!auto_state.checked_for_autoland &&
nav_controller->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;
}
// fall through to LOITER
case LOITER:
case GUIDED:
// allow loiter direction to be changed in flight
if (g.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
update_loiter();
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:
// nothing to do
break;
}
}
