Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/ArduCopter/limits.pde

364 lines
13 KiB
Plaintext
Raw Blame History

// 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);
}
// 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_P(SEVERITY_LOW, PSTR("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_P(SEVERITY_LOW, PSTR("!GPSLock"));
if (limits.mods_triggered & LIMIT_GEOFENCE) gcs_send_text_P(SEVERITY_LOW, PSTR("!Geofence"));
if (limits.mods_triggered & LIMIT_ALTITUDE) gcs_send_text_P(SEVERITY_LOW, PSTR("!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);
set_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
Reference in New Issue View Git Blame Copy Permalink