aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Limits: experimental "bounce" mode.

Browse Source
This commit is contained in:
Andreas M. Antonopoulos 2012-07-31 18:39:58 -07:00
parent 4ffc5e211a
commit 7262b0429a
3 changed files with 64 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
ArduCopter/limits.pde
View File

@ -229,18 +229,18 @@ static void limits_loop() {
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;
}
// // 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
@ -250,29 +250,51 @@ static void limits_loop() {
// }
switch (limits.recmode()) {
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);
case 0: // RTL mode
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - RTL"));
// 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
// }
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;
}
// ESCALATE We have not recovered after 5 minutes of recovery action
if (((uint32_t)millis() - (uint32_t)limits.last_action) > 300000 ) {
// 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"));

13
libraries/AP_Limits/AP_Limits.cpp
View File

@ -44,8 +44,15 @@ const AP_Param::GroupInfo AP_Limits::var_info[] PROGMEM = {
// @Range: 1-8
// @User: Standard
AP_GROUPINFO("CHANNEL", 4, AP_Limits, _channel),
AP_GROUPEND
// @Param: RECMODE
// @DisplayName: Limits Recovery Mode
// @Description: Select how Limits should "recover". Set to 0 for RTL-like mode, where the vehicle navigates back to home until it is "safe". Set to 1, for bounce-mode, where the vehicle will hold position when it hits a limit. RTL mode is better for large fenced areas, Bounce mode for smaller spaces. Note: RTL mode will cause the vehicle to yaw 180 degrees (turn around) to navigate towards home when it hits a limit.
// @Values: 0:RTL mode, 1: Bounce mode
// @User: Standard
AP_GROUPINFO("RECMODE", 5, AP_Limits, _recmode),
AP_GROUPEND
};
AP_Limits::AP_Limits() {
@ -194,3 +201,7 @@ void AP_Limits::set_state(int s) {
AP_Int8 AP_Limits::channel() {
return _channel;
}
AP_Int8 AP_Limits::recmode() {
return _recmode;
}

2
libraries/AP_Limits/AP_Limits.h
View File

@ -49,6 +49,7 @@ public:
AP_Int8 state();
AP_Int8 safetime();
AP_Int8 channel();
AP_Int8 recmode();
// module linked list management methods
void modules(AP_Limit_Module *m); // pointer to the first module in linked list of modules
@ -91,6 +92,7 @@ protected:
AP_Int8 _safetime; // how long after recovered before switching to stabilize
AP_Int8 _state; // overall state - used in the main loop state machine inside the vehicle's slow loop.
AP_Int8 _channel; // channel used for switching limits on/off
AP_Int8 _recmode; // recovery mode: 0=RTL mode, 1=bounce mode
private:
AP_Limit_Module *_modules_head; // points to linked list of modules