mirror of https://github.com/ArduPilot/ardupilot
364 lines
11 KiB
Plaintext
364 lines
11 KiB
Plaintext
// 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 - do nothing"));
|
|
// // 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;
|
|
// }
|
|
|
|
|
|
switch (limits.recmode()) {
|
|
|
|
case 0: // RTL mode
|
|
|
|
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - RTL"));
|
|
|
|
set_mode(RTL);
|
|
limits.last_action = millis();
|
|
gcs_send_message(MSG_LIMITS_STATUS);
|
|
break;
|
|
|
|
case 1: // Bounce mode
|
|
|
|
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - bounce mode, POSITION"));
|
|
// ALT_HOLD gives us yaw hold, roll& pitch hold and throttle hold.
|
|
// It is like position hold, but without manual throttle control.
|
|
|
|
//set_recovery_home_alt();
|
|
set_mode(POSITION);
|
|
throttle_mode = THROTTLE_AUTO;
|
|
limits.last_action = millis();
|
|
gcs_send_message(MSG_LIMITS_STATUS);
|
|
break;
|
|
|
|
}
|
|
break;
|
|
}
|
|
|
|
|
|
// In bounce mode, take control for 3 seconds, and then wait for the pilot to make us "safe".
|
|
// If the vehicle does not recover, the escalation action will trigger.
|
|
if (limits.recmode() == 1) {
|
|
|
|
if (control_mode == POSITION && ((uint32_t)millis() - (uint32_t)limits.last_action) > 3000) {
|
|
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Recovery Bounce: Returning control to pilot"));
|
|
set_mode(STABILIZE);
|
|
} else if (control_mode == STABILIZE && ((uint32_t)millis() - (uint32_t)limits.last_action) > 6000) {
|
|
// after 3 more seconds, reset action counter to take action again
|
|
limits.last_action = 0;
|
|
}
|
|
}
|
|
|
|
// ESCALATE We have not recovered after 2 minutes of recovery action
|
|
|
|
if (((uint32_t)millis() - (uint32_t)limits.last_action) > 120000 ) {
|
|
|
|
// 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
|