// Main state machine loop for AP_Limits. Called from slow or superslow loop. #if AP_LIMITS == ENABLED uint8_t lim_state = 0, lim_old_state = 0; void set_recovery_home_alt() { uint32_t return_altitude_cm_ahl = 0; // in centimeters above home level. uint32_t amin_meters_ahl, amax_meters_ahl; // for flying vehicles only if (altitude_limit.enabled()) { amin_meters_ahl = (uint32_t) (altitude_limit.min_alt()); amax_meters_ahl = (uint32_t) (altitude_limit.max_alt()); // See if we have a meaningful setting if (amax_meters_ahl && ((amax_meters_ahl - amin_meters_ahl) > 1)) { // there is a max_alt set // set a return altitude that is halfway between the minimum and maximum altitude setting. // return_altitude is in centimeters, not meters, so we multiply return_altitude_cm_ahl = (uint32_t) (home.alt + (100 * (uint16_t) ((amax_meters_ahl - amin_meters_ahl) / 2))); } } else { return_altitude_cm_ahl = (uint32_t) (home.alt + g.RTL_altitude*100); } // final sanity check // if our return is less than 4 meters from ground, set it to 4m, to clear "people" height. if ((return_altitude_cm_ahl - (uint32_t) home.alt) < 400) { return_altitude_cm_ahl = home.alt + 400; } guided_WP.id = 0; guided_WP.p1 = 0; guided_WP.options = 0; guided_WP.lat = home.lat; guided_WP.lng = home.lng; guided_WP.alt = return_altitude_cm_ahl; } static void limits_loop() { lim_state = limits.state(); // Use limits channel to determine LIMITS_ENABLED or LIMITS_DISABLED state if (lim_state != LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) < LIMITS_ENABLE_PWM) { limits.set_state(LIMITS_DISABLED); } else if (lim_state == LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) >= LIMITS_ENABLE_PWM) { limits.set_state(LIMITS_ENABLED); } if ((uint32_t) millis() - (uint32_t) limits.last_status_update > 1000) { // more than a second has passed - time for an update gcs_send_message(MSG_LIMITS_STATUS); } if (lim_state != lim_old_state) { // state changed lim_old_state = lim_state; // we only use lim_oldstate here, for reporting purposes. So, reset it. gcs_send_message(MSG_LIMITS_STATUS); if (limits.debug()) switch (lim_state) { case LIMITS_INIT: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: INIT")); break; case LIMITS_DISABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: DISABLED")); break; case LIMITS_ENABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: ENABLED")); break; case LIMITS_TRIGGERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: TRIGGERED")); break; case LIMITS_RECOVERING: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERING")); break; case LIMITS_RECOVERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERED")); break; default: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: UNKNOWN")); break; } } switch (limits.state()) { // have not initialized yet case LIMITS_INIT: if (limits.init()) { // initialize system // See what the "master" on/off swith is and go to the appropriate start state if (!limits.enabled()) { limits.set_state(LIMITS_DISABLED); } else { limits.set_state(LIMITS_ENABLED); } } break; // We have been switched off case LIMITS_DISABLED: // check if we have been switched on if (limits.enabled()) { limits.set_state(LIMITS_ENABLED); break; } break; // Limits module is enabled case LIMITS_ENABLED: // check if we've been switched off if (!limits.enabled()) { limits.set_state(LIMITS_DISABLED); break; } // Until motors are armed, do nothing, just wait in ENABLED state if (!motors.armed()) { // we are waiting for motors to arm // do nothing break; } bool required_only; required_only = (limits.last_clear == 0); // if we haven't yet 'cleared' all limits, check required limits only // check if any limits have been breached and trigger if they have if (limits.check_triggered(required_only)) { // // TRIGGER - BREACH OF LIMITS // // make a note of which limits triggered, so if we know if we recovered them limits.mods_recovering = limits.mods_triggered; limits.last_action = 0; limits.last_trigger = millis(); limits.breach_count++; limits.set_state(LIMITS_TRIGGERED); break; } if (motors.armed() && limits.enabled() && !limits.mods_triggered) { // All clear. if (limits.debug()) gcs_send_text(SEVERITY_LOW, "Limits - All Clear"); limits.last_clear = millis(); } break; // Limits have been triggered case LIMITS_TRIGGERED: // check if we've been switched off if (!limits.enabled()) { limits.set_state(LIMITS_DISABLED); break; } #if LIMITS_TRIGGERED_PIN > 0 digitalWrite(LIMITS_TRIGGERED_PIN, HIGH); #endif if (limits.debug()) { if (limits.mods_triggered & LIMIT_GPSLOCK) gcs_send_text(SEVERITY_LOW, "!GPSLock"); if (limits.mods_triggered & LIMIT_GEOFENCE) gcs_send_text(SEVERITY_LOW, "!Geofence"); if (limits.mods_triggered & LIMIT_ALTITUDE) gcs_send_text(SEVERITY_LOW, "!Altitude"); } // If the motors are not armed, we have triggered pre-arm checks. Do nothing if (motors.armed() == false) { limits.set_state(LIMITS_ENABLED); // go back to checking limits break; } // If we are triggered but no longer in breach, that means we recovered // somehow, via auto recovery or pilot action if (!limits.check_all()) { limits.last_recovery = millis(); limits.set_state(LIMITS_RECOVERED); break; } else { limits.set_state(LIMITS_RECOVERING); limits.last_action = 0; // reset timer // We are about to take action on a real breach. Make sure we notify immediately gcs_send_message(MSG_LIMITS_STATUS); break; } break; // Take action to recover case LIMITS_RECOVERING: // If the motors are not armed, we have triggered pre-arm checks. Do nothing if (motors.armed() == false) { limits.set_state(LIMITS_ENABLED); // go back to checking limits break; } // check if we've been switched off if (!limits.enabled() && limits.old_mode_switch == oldSwitchPosition) { limits.old_mode_switch = 0; reset_control_switch(); limits.set_state(LIMITS_DISABLED); break; } // Still need action? if (limits.check_all() == 0) { // all triggers clear limits.set_state(LIMITS_RECOVERED); break; } if (limits.mods_triggered != limits.mods_recovering) { // if any *new* triggers, hit the trigger again // // TRIGGER - BREACH OF LIMITS // // make a note of which limits triggered, so if we know if we recovered them limits.mods_recovering = limits.mods_triggered; limits.last_action = 0; limits.last_trigger = millis(); limits.breach_count++; limits.set_state(LIMITS_TRIGGERED); limits.set_state(LIMITS_TRIGGERED); break; } // Recovery Action // if there was no previous action, take action, take note of time send GCS. if (limits.last_action == 0) { // save mode switch limits.old_mode_switch = oldSwitchPosition; // Take action // This ensures no "radical" RTL, like a full throttle take-off,happens if something triggers at ground level if ((uint32_t) current_loc.alt < ((uint32_t)home.alt * 200) ) { // we're under 2m (200cm), already at "people" height or on the ground if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action: near ground - LAND")); // TODO: Will this work for a plane? Does it make sense in general? set_mode(LAND); limits.last_action = millis(); // start counter gcs_send_message(MSG_LIMITS_STATUS); break; } // TODO: This applies only to planes - hold for porting // if (control_mode == MANUAL && g.auto_trim) { // // make sure we don't auto trim the surfaces on this change // control_mode = STABILIZE; // } if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - GUIDED to home")); set_recovery_home_alt(); set_mode(GUIDED); limits.last_action = millis(); gcs_send_message(MSG_LIMITS_STATUS); // if (!nav_ok) { // we don't have navigational data, now what? // // if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits No NAV for recovery!")); // // // flying vehicles - land? // //set_mode(ALT_HOLD); // // // other vehicles - TODO // } break; } // ESCALATE We have not recovered after 5 minutes of recovery action if (((uint32_t)millis() - (uint32_t)limits.last_action) > 300000 ) { // TODO: Secondary recovery if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Recovery Escalation: RTL")); set_mode(RTL); limits.last_action = millis(); break; } break; // Have recovered, relinquish control and re-enable case LIMITS_RECOVERED: // check if we've been switched off if (!limits.enabled()) { limits.set_state(LIMITS_DISABLED); break; } #if LIMITS_TRIGGERED_PIN > 0 digitalWrite(LIMITS_TRIGGERED_PIN, LOW); #endif // Reset action counter limits.last_action = 0; if (((uint32_t)millis() - (uint32_t)limits.last_recovery) > (uint32_t)(limits.safetime() * 1000)) { // Wait "safetime" seconds of recovery before we give back control // Our recovery action worked. limits.set_state(LIMITS_ENABLED); // Switch to stabilize if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - Returning controls")); set_mode(STABILIZE); limits.last_recovery = millis(); break; } break; default: if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits: unknown state")); break; } } // This function below, should really be in the AP_Limits class, but it is impossible to untangle the mavlink includes. void limits_send_mavlink_status(mavlink_channel_t chan) { limits.last_status_update = millis(); if (limits.enabled()) { mavlink_msg_limits_status_send(chan, (uint8_t) limits.state(), (uint32_t) limits.last_trigger, (uint32_t) limits.last_action, (uint32_t) limits.last_recovery, (uint32_t) limits.last_clear, (uint16_t) limits.breach_count, (LimitModuleBits) limits.mods_enabled, (LimitModuleBits) limits.mods_required, (LimitModuleBits) limits.mods_triggered); } } #endif