Merge remote-tracking branch 'private_swissfang/stable' into obcfailsafe

Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

8 files changed, 124 insertions, 57 deletions

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/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 0bb65252f..53071630d 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -463,10 +463,10 @@ BottleDrop::task_main()
 			continue;
 		}
 
-		const unsigned sleeptime_us = 50000;
+		const unsigned sleeptime_us = 9500;
 
 		hrt_abstime last_run = hrt_absolute_time();
-		float dt_runs = 1e6f / sleeptime_us;
+		float dt_runs = sleeptime_us / 1e6f;
 
 		// switch to faster updates during the drop
 		while (_drop_state > DROP_STATE_INIT) {
@@ -517,53 +517,49 @@ BottleDrop::task_main()
 				case DROP_STATE_TARGET_VALID:
 				{
 
-					// Update drop point at 10 Hz
-					if (counter % 10 == 0) {
-
-						az = g;                 				// acceleration in z direction[m/s^2]
-						vz = 0; 						// velocity in z direction [m/s]
-						z = 0; 							// fallen distance [m]
-						h_0 = _global_pos.alt - _target_position.alt; 		// height over target at start[m]
-						h = h_0;						// height over target [m]
-						ax = 0;							// acceleration in x direction [m/s^2]
-						vx = groundspeed_body;// XXX project					// ground speed in x direction [m/s]
-						x = 0;							// traveled distance in x direction [m]
-						vw = 0;                 				// wind speed [m/s]
-						vrx = 0;						// relative velocity in x direction [m/s]
-						v = groundspeed_body;					// relative speed vector [m/s]
-						Fd = 0;							// Drag force [N]
-						Fdx = 0;						// Drag force in x direction [N]
-						Fdz = 0;						// Drag force in z direction [N]
-
-
-						// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
-						while (h > 0.05f) {
-							// z-direction
-							vz = vz + az * dt_freefall_prediction;
-							z = z + vz * dt_freefall_prediction;
-							h = h_0 - z;
-
-							// x-direction
-							vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
-							vx = vx + ax * dt_freefall_prediction;
-							x = x + vx * dt_freefall_prediction;
-							vrx = vx + vw;
-
-							// drag force
-							v = sqrtf(vz * vz + vrx * vrx);
-							Fd = 0.5f * rho * A * cd * (v * v);
-							Fdx = Fd * vrx / v;
-							Fdz = Fd * vz / v;
-
-							// acceleration
-							az = g - Fdz / m;
-							ax = -Fdx / m;
-						}
-
-						// compute drop vector
-						x = groundspeed_body * t_signal + x;
+					az = g;							// acceleration in z direction[m/s^2]
+					vz = 0; 						// velocity in z direction [m/s]
+					z = 0; 							// fallen distance [m]
+					h_0 = _global_pos.alt - _target_position.alt; 		// height over target at start[m]
+					h = h_0;						// height over target [m]
+					ax = 0;							// acceleration in x direction [m/s^2]
+					vx = groundspeed_body;// XXX project					// ground speed in x direction [m/s]
+					x = 0;							// traveled distance in x direction [m]
+					vw = 0;							// wind speed [m/s]
+					vrx = 0;						// relative velocity in x direction [m/s]
+					v = groundspeed_body;					// relative speed vector [m/s]
+					Fd = 0;							// Drag force [N]
+					Fdx = 0;						// Drag force in x direction [N]
+					Fdz = 0;						// Drag force in z direction [N]
+
+
+					// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
+					while (h > 0.05f) {
+						// z-direction
+						vz = vz + az * dt_freefall_prediction;
+						z = z + vz * dt_freefall_prediction;
+						h = h_0 - z;
+
+						// x-direction
+						vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
+						vx = vx + ax * dt_freefall_prediction;
+						x = x + vx * dt_freefall_prediction;
+						vrx = vx + vw;
+
+						// drag force
+						v = sqrtf(vz * vz + vrx * vrx);
+						Fd = 0.5f * rho * A * cd * (v * v);
+						Fdx = Fd * vrx / v;
+						Fdz = Fd * vz / v;
+
+						// acceleration
+						az = g - Fdz / m;
+						ax = -Fdx / m;
 					}
 
+					// compute drop vector
+					x = groundspeed_body * t_signal + x;
+
 					x_t = 0.0f;
 					y_t = 0.0f;
 
@@ -688,10 +684,10 @@ BottleDrop::task_main()
 
 			// update_actuators();
 
-			// run at roughly 20 Hz
+			// run at roughly 100 Hz
 			usleep(sleeptime_us);
 
-			dt_runs = 1e6f / hrt_elapsed_time(&last_run);
+			dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
 			last_run = hrt_absolute_time();
 		}
 	}
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 d177f7552..00b12fffa 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
@@ -390,7 +390,8 @@ private:
 			bool climbout_mode, float climbout_pitch_min_rad,
 			float altitude,
 			const math::Vector<3> &ground_speed,
-			tecs_mode mode = TECS_MODE_NORMAL);
+			tecs_mode mode = TECS_MODE_NORMAL,
+			bool pitch_max_special = false);
 
 };
 
@@ -1140,7 +1141,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_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+				float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
+
+				/* 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 */
@@ -1148,7 +1155,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,
@@ -1156,7 +1163,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							math::radians(10.0f)),
 							_global_pos.alt,
 							ground_speed,
-							TECS_MODE_TAKEOFF);
+							TECS_MODE_TAKEOFF,
+							takeoff_pitch_max_deg != _parameters.pitch_limit_max);
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
@@ -1228,7 +1236,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		 /* making sure again that the correct thrust is used,
-		 * without depending on library calls */
+		 * without depending on library calls for safety reasons */
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	} else {
 		/* Copy thrust and pitch values from tecs */
@@ -1420,7 +1428,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		bool climbout_mode, float climbout_pitch_min_rad,
 		float altitude,
 		const math::Vector<3> &ground_speed,
-		tecs_mode mode)
+		tecs_mode mode, bool pitch_max_special)
 {
 	if (_mTecs.getEnabled()) {
 		/* Using mtecs library: prepare arguments for mtecs call */
@@ -1440,6 +1448,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		} else {
 			limitOverride.disablePitchMinOverride();
 		}
+
+		if (pitch_max_special) {
+			/* Use the maximum pitch from the argument */
+			limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
+		} else {
+			/* use pitch max set by MT param */
+			limitOverride.disablePitchMaxOverride();
+		}
 		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
 				limitOverride);
 	} else {