void
GpsFailure::advance_gpsf()
{
updateParams();
switch (_gpsf_state) {
case GPSF_STATE_NONE:
_gpsf_state = GPSF_STATE_LOITER;
warnx("gpsf loiter");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
break;
case GPSF_STATE_LOITER:
_gpsf_state = GPSF_STATE_TERMINATE;
warnx("gpsf terminate");
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
warnx("mavlink sent");
break;
case GPSF_STATE_TERMINATE:
warnx("gpsf end");
_gpsf_state = GPSF_STATE_END;
default:
break;
}
}
void
EngineFailure::advance_ef()
{
switch (_ef_state) {
case EF_STATE_NONE:
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
break;
}
}
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
const low_battery_action_t low_battery_action)
{
switch (battery_warning) {
case battery_status_s::BATTERY_WARNING_NONE:
break;
case battery_status_s::BATTERY_WARNING_LOW:
mavlink_log_critical(mavlink_log_pub, "LOW BATTERY, RETURN ADVISED");
break;
case battery_status_s::BATTERY_WARNING_CRITICAL:
static constexpr char battery_critical[] = "CRITICAL BATTERY";
switch (low_battery_action) {
case LOW_BAT_ACTION::WARNING:
mavlink_log_critical(mavlink_log_pub, "%s, RETURN ADVISED!", battery_critical);
break;
case LOW_BAT_ACTION::RETURN:
// FALLTHROUGH
case LOW_BAT_ACTION::RETURN_OR_LAND:
// let us send the critical message even if already in RTL
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, RETURNING", battery_critical);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, RETURN FAILED", battery_critical);
}
break;
case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, LANDING AT CURRENT POSITION", battery_critical);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, LANDING FAILED", battery_critical);
}
break;
}
break;
case battery_status_s::BATTERY_WARNING_EMERGENCY:
static constexpr char battery_dangerous[] = "DANGEROUS BATTERY LEVEL";
switch (low_battery_action) {
case LOW_BAT_ACTION::WARNING:
mavlink_log_emergency(mavlink_log_pub, "%s, LANDING ADVISED!", battery_dangerous);
break;
case LOW_BAT_ACTION::RETURN:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, RETURNING", battery_dangerous);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, RETURN FAILED", battery_dangerous);
}
break;
case LOW_BAT_ACTION::RETURN_OR_LAND:
// FALLTHROUGH
case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, LANDING IMMEDIATELY", battery_dangerous);
} else {
mavlink_log_emergency(mavlink_log_pub, "%s, LANDING FAILED", battery_dangerous);
}
break;
}
break;
case battery_status_s::BATTERY_WARNING_FAILED:
mavlink_log_emergency(mavlink_log_pub, "BATTERY FAILED");
break;
}
}
