Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-07 08:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity
d9e0bbbbab
ardupilot/ArduCopter/limits.pde
rmackay9 a5bb54e36e ArduCopter: RTL clean-up and slightly improved landing sensor 
Consolidated RTL state to be captured by rtl_state variable.
Combined update_RTL_Nav and verify_RTL functions which performed the same function but one was for missions, the other for the RTL flight mode.
Renamed some RTL parameters and global variables to have RTL at the front.
Landing detector now checks accel-throttle's I term and/or a very low throttle value
2012-12-06 10:31:52 +09:00

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