AP_Limits: experimental "bounce" mode.

ArduCopter/limits.pde

@@ -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"));

libraries/AP_Limits/AP_Limits.cpp

@@ -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;
+}

libraries/AP_Limits/AP_Limits.h

@@ -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