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

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

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 */
 
 };
 

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

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:

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:
 };

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.
  *

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();