From 708ee8ae3aa4d4d5eac9e2fd86ed9673c126fe33 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 22 Aug 2014 20:30:06 +0200
Subject: autolaunch: added param for delay

---
 src/lib/launchdetection/CatapultLaunchMethod.h | 4 ++++
 1 file changed, 4 insertions(+)

(limited to 'src/lib/launchdetection/CatapultLaunchMethod.h')

diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index 23757f6f3..a64d482f6 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -63,10 +63,14 @@ public:
 private:
 	hrt_abstime last_timestamp;
 	float integrator;
+	float delayCounter;
+
 	bool launchDetected;
+	bool delayPassed;
 
 	control::BlockParamFloat threshold_accel;
 	control::BlockParamFloat threshold_time;
+	control::BlockParamFloat delay;
 
 };
 
-- 
cgit v1.2.3


From 5a5617cb597b16b789a6fa175410d667f45cf907 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 23 Aug 2014 11:42:53 +0200
Subject: launchdetector: return state of detection

The launchdetector now has a intermediate state (controlsenabled) which
is meant to be interpreted by the controller as: "perform attitude
control but do not yet power up the motor". This can be used in the
floating phase during a bungee launch for example.
---
 src/lib/launchdetection/CatapultLaunchMethod.cpp | 70 ++++++++++++++----------
 src/lib/launchdetection/CatapultLaunchMethod.h   | 13 ++---
 src/lib/launchdetection/LaunchDetector.cpp       | 27 +++++++--
 src/lib/launchdetection/LaunchDetector.h         |  8 ++-
 src/lib/launchdetection/LaunchMethod.h           | 13 ++++-
 src/lib/launchdetection/launchdetection_params.c |  8 ++-
 6 files changed, 91 insertions(+), 48 deletions(-)

(limited to 'src/lib/launchdetection/CatapultLaunchMethod.h')

diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index 2b876b629..65ae461db 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -49,10 +49,10 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
 	SuperBlock(parent, "CAT"),
 	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
-	launchDetected(false),
-	threshold_accel(this, "A"),
-	threshold_time(this, "T"),
-	delay(this, "DELAY")
+	state(LAUNCHDETECTION_RES_NONE),
+	thresholdAccel(this, "A"),
+	thresholdTime(this, "T"),
+	motorDelay(this, "MDEL")
 {
 
 }
@@ -66,46 +66,56 @@ void CatapultLaunchMethod::update(float accel_x)
 	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
 	last_timestamp = hrt_absolute_time();
 
-	if (accel_x > threshold_accel.get()) {
-		integrator += dt;
-//		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		if (integrator > threshold_time.get()) {
-			launchDetected = true;
+	switch (state) {
+	case LAUNCHDETECTION_RES_NONE:
+		/* Detect a acceleration that is longer and stronger as the minimum given by the params */
+		if (accel_x > thresholdAccel.get()) {
+			integrator += dt;
+			if (integrator > thresholdTime.get()) {
+				if (motorDelay.get() > 0.0f) {
+					state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
+					warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
+							" throttle", (double)motorDelay.get());
+				} else {
+					/* No motor delay set: go directly to enablemotors state */
+					state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+					warnx("Launch detected: state: enablemotors (delay not activated)");
+				}
+			}
+		} else {
+			/* reset */
+			reset();
 		}
+		break;
+
+	case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
+		/* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
+		 * over to allow full throttle */
+		motorDelayCounter += dt;
+		if (motorDelayCounter > motorDelay.get()) {
+			warnx("Launch detected: state enablemotors");
+			state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+		}
+		break;
 
-	} else {
-//		warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		/* reset integrator */
-		integrator = 0.0f;
-	}
+	default:
+		break;
 
-	if (launchDetected) {
-		delayCounter += dt;
-		if (delayCounter > delay.get()) {
-			delayPassed = true;
-		}
 	}
 
 }
 
-bool CatapultLaunchMethod::getLaunchDetected()
+LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
 {
-	if (delay.get() > 0.0f) {
-		return delayPassed;
-	} else {
-		return launchDetected;
-	}
+	return state;
 }
 
 
 void CatapultLaunchMethod::reset()
 {
 	integrator = 0.0f;
-	delayCounter = 0.0f;
-	launchDetected = false;
-	delayPassed = false;
+	motorDelayCounter = 0.0f;
+	state = LAUNCHDETECTION_RES_NONE;
 }
 
 }
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index a64d482f6..d918c3a76 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -57,20 +57,19 @@ public:
 	~CatapultLaunchMethod();
 
 	void update(float accel_x);
-	bool getLaunchDetected();
+	LaunchDetectionResult getLaunchDetected() const;
 	void reset();
 
 private:
 	hrt_abstime last_timestamp;
 	float integrator;
-	float delayCounter;
+	float motorDelayCounter;
 
-	bool launchDetected;
-	bool delayPassed;
+	LaunchDetectionResult state;
 
-	control::BlockParamFloat threshold_accel;
-	control::BlockParamFloat threshold_time;
-	control::BlockParamFloat delay;
+	control::BlockParamFloat thresholdAccel;
+	control::BlockParamFloat thresholdTime;
+	control::BlockParamFloat motorDelay;
 
 };
 
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index 3bf47bbb0..2958c0a31 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -46,6 +46,7 @@ namespace launchdetection
 
 LaunchDetector::LaunchDetector() :
 	SuperBlock(NULL, "LAUN"),
+	activeLaunchDetectionMethodIndex(-1),
 	launchdetection_on(this, "ALL_ON"),
 	throttlePreTakeoff(this, "THR_PRE")
 {
@@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector()
 void LaunchDetector::reset()
 {
 	/* Reset all detectors */
-	launchMethods[0]->reset();
+	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+		launchMethods[i]->reset();
+	}
+
+	/* Reset active launchdetector */
+	activeLaunchDetectionMethodIndex  = -1;
+
+
 }
 
 void LaunchDetector::update(float accel_x)
@@ -77,17 +85,24 @@ void LaunchDetector::update(float accel_x)
 	}
 }
 
-bool LaunchDetector::getLaunchDetected()
+LaunchDetectionResult LaunchDetector::getLaunchDetected()
 {
 	if (launchdetection_on.get() == 1) {
-		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-			if(launchMethods[i]->getLaunchDetected()) {
-				return true;
+		if (activeLaunchDetectionMethodIndex < 0) {
+		/* None of the active launchmethods has detected a launch, check all launchmethods */
+			for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+				if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
+					warnx("selecting launchmethod %d", i);
+					activeLaunchDetectionMethodIndex = i; // from now on only check this method
+					return launchMethods[i]->getLaunchDetected();
+				}
 			}
+		} else {
+			return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
 		}
 	}
 
-	return false;
+	return LAUNCHDETECTION_RES_NONE;
 }
 
 }
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 1a214b66e..b48e724ba 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -59,7 +59,7 @@ public:
 	void reset();
 
 	void update(float accel_x);
-	bool getLaunchDetected();
+	LaunchDetectionResult getLaunchDetected();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -67,6 +67,12 @@ public:
 //	virtual bool getLaunchDetected();
 protected:
 private:
+	int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods
+					       which detected a Launch. If no launchMethod has detected a launch yet the
+					       value is -1. Once one launchMetthod has detected a launch only this
+					       method is checked for further adavancing in the state machine (e.g. when
+					       to power up the motors) */
+
 	LaunchMethod* launchMethods[1];
 	control::BlockParamInt launchdetection_on;
 	control::BlockParamFloat throttlePreTakeoff;
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index e04467f6a..d2f091cea 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -44,11 +44,22 @@
 namespace launchdetection
 {
 
+enum LaunchDetectionResult {
+	LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
+	LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
+							  control the attitude. However any motors should not throttle
+							  up and still be set to 'throttlePreTakeoff'.
+							  For instance this is used to have a delay for the motor
+							  when launching a fixed wing aircraft from a bungee */
+	LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control
+							attitude and also throttle up the motors. */
+};
+
 class LaunchMethod
 {
 public:
 	virtual void update(float accel_x) = 0;
-	virtual bool getLaunchDetected() = 0;
+	virtual LaunchDetectionResult getLaunchDetected() const = 0;
 	virtual void reset() = 0;
 
 protected:
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 02f01bc66..d35eb11f6 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -78,14 +78,16 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
 PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 
 /**
- * Catapult delay
- *
+ * Motor delay
  *
+ * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller)
+ * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
  *
+ * @unit seconds
  * @min 0
  * @group Launch detection
  */
-PARAM_DEFINE_FLOAT(LAUN_CAT_DELAY, 0.0f);
+PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
 /**
  * Throttle setting while detecting launch.
  *
-- 
cgit v1.2.3


From daf16184202b02119aa1ac83cb82cf85979d43f9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Sep 2014 12:42:42 +0200
Subject: additional upper pitch limit during launch

The pitch limit can be used by the laucnhdetector to limit pitch during
critical phases of a launch. For example this can be used to limit pitch
while attached to a bungee differently from the standard pitch limit.
---
 src/lib/launchdetection/CatapultLaunchMethod.cpp     | 13 ++++++++++++-
 src/lib/launchdetection/CatapultLaunchMethod.h       |  4 ++++
 src/lib/launchdetection/LaunchDetector.cpp           | 20 ++++++++++++++++++++
 src/lib/launchdetection/LaunchDetector.h             |  3 +++
 src/lib/launchdetection/LaunchMethod.h               |  3 +++
 src/lib/launchdetection/launchdetection_params.c     | 16 +++++++++++++++-
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp     | 12 +++++++++---
 7 files changed, 66 insertions(+), 5 deletions(-)

(limited to 'src/lib/launchdetection/CatapultLaunchMethod.h')

diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index 65ae461db..2ea1c414b 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -52,7 +52,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
 	state(LAUNCHDETECTION_RES_NONE),
 	thresholdAccel(this, "A"),
 	thresholdTime(this, "T"),
-	motorDelay(this, "MDEL")
+	motorDelay(this, "MDEL"),
+	pitchMaxPreThrottle(this, "PMAX")
 {
 
 }
@@ -118,4 +119,14 @@ void CatapultLaunchMethod::reset()
 	state = LAUNCHDETECTION_RES_NONE;
 }
 
+float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) {
+	/* If motor is turned on do not impose the extra limit on maximum pitch */
+	if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+		return pitchMaxDefault;
+	} else {
+		return pitchMaxPreThrottle.get();
+	}
+
+}
+
 }
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index d918c3a76..321dfb1de 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -59,6 +59,7 @@ public:
 	void update(float accel_x);
 	LaunchDetectionResult getLaunchDetected() const;
 	void reset();
+	float getPitchMax(float pitchMaxDefault);
 
 private:
 	hrt_abstime last_timestamp;
@@ -70,6 +71,9 @@ private:
 	control::BlockParamFloat thresholdAccel;
 	control::BlockParamFloat thresholdTime;
 	control::BlockParamFloat motorDelay;
+	control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on.
+						       Can be used to make sure that the AC does not climb
+						       too much while attached to a bungee */
 
 };
 
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index 2958c0a31..52f5c3257 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -105,4 +105,24 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
 	return LAUNCHDETECTION_RES_NONE;
 }
 
+float LaunchDetector::getPitchMax(float pitchMaxDefault) {
+	if (!launchdetection_on.get()) {
+		return pitchMaxDefault;
+	}
+
+	/* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
+	 * otherwise use the default limit */
+	if (activeLaunchDetectionMethodIndex < 0) {
+		if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) {
+			return pitchMaxDefault;
+		} else {
+			return launchMethods[0]->getPitchMax(pitchMaxDefault);
+		}
+	} else {
+		return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
+	}
+
+}
+
+
 }
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index b48e724ba..4215b49d2 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -64,6 +64,9 @@ public:
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
 
+	/* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */
+	float getPitchMax(float pitchMaxDefault);
+
 //	virtual bool getLaunchDetected();
 protected:
 private:
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index d2f091cea..8b5220cb3 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -62,6 +62,9 @@ public:
 	virtual LaunchDetectionResult getLaunchDetected() const = 0;
 	virtual void reset() = 0;
 
+	/* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */
+	virtual float getPitchMax(float pitchMaxDefault) = 0;
+
 protected:
 private:
 };
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index d35eb11f6..e3aa7ab2d 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 /**
  * Motor delay
  *
- * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller)
+ * Delay between starting attitude control and powering up the throttle (giving throttle control to the controller)
  * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
  *
  * @unit seconds
@@ -88,6 +88,20 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
  * @group Launch detection
  */
 PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
+
+/**
+ * Maximum pitch before the throttle is powered up (during motor delay phase)
+ *
+ * This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on.
+ * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep).
+ *
+ * @unit deg
+ * @min 0
+ * @max 45
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f);
+
 /**
  * Throttle setting while detecting launch.
  *
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 84c14be01..402b171f5 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1115,7 +1115,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
 					launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
 
-				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
+				/* select maximum pitch: the launchdetector may impose another limit for the pitch
+				 * depending on the state of the launch */
+				float takeoff_pitch_max_rad = math::radians(
+						launchDetector.getPitchMax(_parameters.pitch_limit_max));
+
+				/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
+				 * meters */
 				if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
@@ -1123,7 +1129,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							calculate_target_airspeed(1.3f * _parameters.airspeed_min),
 							eas2tas,
 							math::radians(_parameters.pitch_limit_min),
-							math::radians(_parameters.pitch_limit_max),
+							takeoff_pitch_max_rad,
 							_parameters.throttle_min, takeoff_throttle,
 							_parameters.throttle_cruise,
 							true,
@@ -1199,7 +1205,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* Copy thrust and pitch values from tecs
 	 * making sure again that the correct thrust is used,
-	 * without depending on library calls */
+	 * without depending on library calls for safety reasons */
 	if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
-- 
cgit v1.2.3