FW launchdetector only run if armed

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -40,33 +40,23 @@
  */
 
 #include "CatapultLaunchMethod.h"
-#include <systemlib/err.h>
 
 namespace launchdetection
 {
 
 CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
 	SuperBlock(parent, "CAT"),
-	last_timestamp(hrt_absolute_time()),
-	integrator(0.0f),
-	motorDelayCounter(0.0f),
-	state(LAUNCHDETECTION_RES_NONE),
 	thresholdAccel(this, "A"),
 	thresholdTime(this, "T"),
 	motorDelay(this, "MDEL"),
 	pitchMaxPreThrottle(this, "PMAX")
 {
-
-}
-
-CatapultLaunchMethod::~CatapultLaunchMethod()
-{
-
+	last_timestamp = hrt_absolute_time();
 }
 
 void CatapultLaunchMethod::update(float accel_x)
 {
-	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
+	float dt = hrt_elapsed_time(&last_timestamp) * 1e-6f;
 	last_timestamp = hrt_absolute_time();
 
 	switch (state) {
@@ -79,18 +69,17 @@ void CatapultLaunchMethod::update(float accel_x)
 			if (integrator > thresholdTime.get()) {
 				if (motorDelay.get() > 0.0f) {
 					state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
-					warnx("Launch detected: enablecontrol, waiting %8.4fs until full throttle",
-					      double(motorDelay.get()));
+					PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle",
+						 double(motorDelay.get()));
 
 				} else {
 					/* No motor delay set: go directly to enablemotors state */
 					state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
-					warnx("Launch detected: enablemotors (delay not activated)");
+					PX4_WARN("Launch detected: enablemotors (delay not activated)");
 				}
 			}
 
 		} else {
-			/* reset */
 			reset();
 		}
 
@@ -102,7 +91,7 @@ void CatapultLaunchMethod::update(float accel_x)
 		motorDelayCounter += dt;
 
 		if (motorDelayCounter > motorDelay.get()) {
-			warnx("Launch detected: state enablemotors");
+			PX4_WARN("Launch detected: state enablemotors");
 			state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 		}
 
@@ -112,7 +101,6 @@ void CatapultLaunchMethod::update(float accel_x)
 		break;
 
 	}
-
 }
 
 LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
@@ -120,7 +108,6 @@ LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
 	return state;
 }
 
-
 void CatapultLaunchMethod::reset()
 {
 	integrator = 0.0f;
@@ -137,7 +124,6 @@ float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault)
 	} else {
 		return pitchMaxPreThrottle.get();
 	}
-
 }
 
-}
+} // namespace launchdetection

src/lib/launchdetection/CatapultLaunchMethod.h

@@ -54,19 +54,19 @@ class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
 {
 public:
 	CatapultLaunchMethod(SuperBlock *parent);
-	~CatapultLaunchMethod();
+	~CatapultLaunchMethod() override = default;
 
-	void update(float accel_x);
-	LaunchDetectionResult getLaunchDetected() const;
-	void reset();
-	float getPitchMax(float pitchMaxDefault);
+	void update(float accel_x) override;
+	LaunchDetectionResult getLaunchDetected() const override;
+	void reset() override;
+	float getPitchMax(float pitchMaxDefault) override;
 
 private:
-	hrt_abstime last_timestamp;
-	float integrator;
-	float motorDelayCounter;
+	hrt_abstime last_timestamp{0};
+	float integrator{0.0f};
+	float motorDelayCounter{0.0f};
 
-	LaunchDetectionResult state;
+	LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE};
 
 	control::BlockParamFloat thresholdAccel;
 	control::BlockParamFloat thresholdTime;
@@ -79,4 +79,4 @@ private:
 
 #endif /* CATAPULTLAUNCHMETHOD_H_ */
 
-}
+} // namespace launchdetection

src/lib/launchdetection/LaunchDetector.cpp

@@ -37,62 +37,56 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include "LaunchDetector.h"
 #include "CatapultLaunchMethod.h"
-#include <systemlib/err.h>
+#include "LaunchDetector.h"
 
 namespace launchdetection
 {
 
 LaunchDetector::LaunchDetector() :
 	SuperBlock(nullptr, "LAUN"),
-	activeLaunchDetectionMethodIndex(-1),
-	launchdetection_on(this, "ALL_ON"),
-	throttlePreTakeoff(nullptr, "FW_THR_IDLE")
+	launchdetection_on(this, "ALL_ON")
 {
 	/* init all detectors */
 	launchMethods[0] = new CatapultLaunchMethod(this);
 
-
 	/* update all parameters of all detectors */
 	updateParams();
 }
 
 LaunchDetector::~LaunchDetector()
 {
-
+	delete launchMethods[0];
 }
 
 void LaunchDetector::reset()
 {
 	/* Reset all detectors */
-	for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
-		launchMethods[i]->reset();
+	for (const auto launchMethod : launchMethods) {
+		launchMethod->reset();
 	}
 
 	/* Reset active launchdetector */
-	activeLaunchDetectionMethodIndex  = -1;
-
-
+	activeLaunchDetectionMethodIndex = -1;
 }
 
 void LaunchDetector::update(float accel_x)
 {
-	if (launchdetection_on.get() == 1) {
-		for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
-			launchMethods[i]->update(accel_x);
+	if (launchDetectionEnabled()) {
+		for (const auto launchMethod : launchMethods) {
+			launchMethod->update(accel_x);
 		}
 	}
 }
 
 LaunchDetectionResult LaunchDetector::getLaunchDetected()
 {
-	if (launchdetection_on.get() == 1) {
+	if (launchDetectionEnabled()) {
 		if (activeLaunchDetectionMethodIndex < 0) {
 			/* None of the active launchmethods has detected a launch, check all launchmethods */
 			for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
 				if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
-					warnx("selecting launchmethod %d", i);
+					PX4_WARN("selecting launchmethod %d", i);
 					activeLaunchDetectionMethodIndex = i; // from now on only check this method
 					return launchMethods[i]->getLaunchDetected();
 				}
@@ -108,7 +102,7 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
 
 float LaunchDetector::getPitchMax(float pitchMaxDefault)
 {
-	if (!launchdetection_on.get()) {
+	if (!launchDetectionEnabled()) {
 		return pitchMaxDefault;
 	}
 
@@ -125,8 +119,6 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault)
 	} else {
 		return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
 	}
-
 }
 
-
-}
+} // namespace launchdetection

src/lib/launchdetection/LaunchDetector.h

@@ -41,9 +41,6 @@
 #ifndef LAUNCHDETECTOR_H
 #define LAUNCHDETECTOR_H
 
-#include <stdbool.h>
-#include <stdint.h>
-
 #include "LaunchMethod.h"
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
@@ -55,36 +52,34 @@ class __EXPORT LaunchDetector : public control::SuperBlock
 {
 public:
 	LaunchDetector();
+	~LaunchDetector() override;
+
 	LaunchDetector(const LaunchDetector &) = delete;
 	LaunchDetector operator=(const LaunchDetector &) = delete;
-	virtual ~LaunchDetector();
 
 	void reset();
 
 	void update(float accel_x);
 	LaunchDetectionResult getLaunchDetected();
-	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
-
-	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
+	bool launchDetectionEnabled() { return launchdetection_on.get() == 1; };
 
 	/* 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:
-	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) */
+	/* holds an 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 launchMethod has detected a launch only this
+	 * method is checked for further advancing in the state machine
+	 * (e.g. when to power up the motors)
+	 */
+	int activeLaunchDetectionMethodIndex{-1};
 
 	LaunchMethod *launchMethods[1];
-	control::BlockParamInt launchdetection_on;
-	control::BlockParamFloat throttlePreTakeoff;
 
+	control::BlockParamInt launchdetection_on;
 };
 
-}
+} // namespace launchdetection
 
 #endif // LAUNCHDETECTOR_H

src/lib/launchdetection/LaunchMethod.h

@@ -48,8 +48,7 @@ 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
+							  up. 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, the controller should control
 							attitude and also throttle up the motors. */
@@ -58,7 +57,7 @@ enum LaunchDetectionResult {
 class LaunchMethod
 {
 public:
-	virtual ~LaunchMethod() {};
+	virtual ~LaunchMethod() = default;
 
 	virtual void update(float accel_x) = 0;
 	virtual LaunchDetectionResult getLaunchDetected() const = 0;
@@ -67,10 +66,8 @@ public:
 	/* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */
 	virtual float getPitchMax(float pitchMaxDefault) = 0;
 
-protected:
-private:
 };
 
-}
+} // namespace launchdetection
 
 #endif /* LAUNCHMETHOD_H_ */

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -118,21 +118,13 @@ using matrix::Vector3f;
 extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
 using namespace launchdetection;
+using namespace runwaytakeoff;
 
 class FixedwingPositionControl
 {
 public:
-	/**
-	 * Constructor
-	 */
 	FixedwingPositionControl();
-
-	/**
-	 * Destructor, also kills the sensors task.
-	 */
 	~FixedwingPositionControl();
-
-	// prevent copying
 	FixedwingPositionControl(const FixedwingPositionControl &) = delete;
 	FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete;
 
@@ -222,10 +214,11 @@ private:
 	hrt_abstime _time_went_in_air{0};			///< time at which the plane went in the air */
 
 	/* Takeoff launch detection and runway */
-	launchdetection::LaunchDetector _launchDetector;
+	LaunchDetector _launchDetector;
 	LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
+	hrt_abstime _launch_detection_notify{0};
 
-	runwaytakeoff::RunwayTakeoff _runway_takeoff;
+	RunwayTakeoff _runway_takeoff;
 
 	bool _last_manual{false};				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
 
@@ -412,11 +405,6 @@ private:
 	void		get_waypoint_heading_distance(float heading, struct position_setpoint_s &waypoint_prev,
 			struct position_setpoint_s &waypoint_next, bool flag_init);
 
-	/**
-	 * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
-	 */
-	float		get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
-
 	/**
 	 * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
 	 */
@@ -1175,9 +1163,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 		nav_speed_2d = ground_speed;
 	}
 
-	/* define altitude error */
-	float altitude_error = pos_sp_curr.alt - _global_pos.alt;
-
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
@@ -1362,7 +1347,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 				}
 
 				_land_noreturn_horizontal = true;
-				mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold");
+				mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold");
 			}
 
 			if (_land_noreturn_horizontal) {
@@ -1470,7 +1455,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 					if (!_land_motor_lim) {
 						_land_motor_lim  = true;
-						mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle");
+						mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle");
 					}
 				}
 
@@ -1500,7 +1485,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					// just started with the flaring phase
 					_att_sp.pitch_body = 0.0f;
 					_flare_height = _global_pos.alt - terrain_alt;
-					mavlink_log_info(&_mavlink_log_pub, "#Landing, flaring");
+					mavlink_log_info(&_mavlink_log_pub, "Landing, flaring");
 					_land_noreturn_vertical = true;
 
 				} else {
@@ -1508,6 +1493,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 						_att_sp.pitch_body = radians(_parameters.land_flare_pitch_min_deg) *
 								     constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
 					}
+
+					// otherwise continue using previous _att_sp.pitch_body
 				}
 
 				_flare_curve_alt_rel_last = flare_curve_alt_rel;
@@ -1530,7 +1517,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 
 					if (!_land_onslope) {
-						mavlink_log_info(&_mavlink_log_pub, "#Landing, on slope");
+						mavlink_log_info(&_mavlink_log_pub, "Landing, on slope");
 						_land_onslope = true;
 					}
 
@@ -1558,6 +1545,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
+			// continuously reset launch detection and runway takeoff until armed
+			if (!_control_mode.flag_armed) {
+				_runway_takeoff.reset();
+
+				_launchDetector.reset();
+				_launch_detection_state = LAUNCHDETECTION_RES_NONE;
+				_launch_detection_notify = 0;
+			}
+
 			if (_runway_takeoff.runwayTakeoffEnabled()) {
 				if (!_runway_takeoff.isInitialized()) {
 					Eulerf euler(Quatf(_ctrl_state.q));
@@ -1567,7 +1563,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					 * doesn't matter if it gets reset when takeoff is detected eventually */
 					_takeoff_ground_alt = _global_pos.alt;
 
-					mavlink_log_info(&_mavlink_log_pub, "#Takeoff on runway");
+					mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway");
 				}
 
 				float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
@@ -1614,20 +1610,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 				if (_launchDetector.launchDetectionEnabled() &&
 				    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 
-					/* Inform user that launchdetection is running */
-					static hrt_abstime last_sent = 0;
+					if (_control_mode.flag_armed) {
+						/* Perform launch detection */
 
-					if (hrt_absolute_time() - last_sent > 4e6) {
-						mavlink_log_critical(&_mavlink_log_pub, "#Launch detection running");
-						last_sent = hrt_absolute_time();
+						/* Inform user that launchdetection is running every 4s */
+						if (hrt_absolute_time() - _launch_detection_notify > 4e6) {
+							mavlink_log_critical(&_mavlink_log_pub, "Launch detection running");
+							_launch_detection_notify = hrt_absolute_time();
+						}
+
+						/* Detect launch */
+						_launchDetector.update(accel_body(0));
+
+						/* update our copy of the launch detection state */
+						_launch_detection_state = _launchDetector.getLaunchDetected();
 					}
 
-					/* Detect launch */
-					_launchDetector.update(accel_body(0));
-
-					/* update our copy of the launch detection state */
-					_launch_detection_state = _launchDetector.getLaunchDetected();
-
 				} else	{
 					/* no takeoff detection --> fly */
 					_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
@@ -1642,11 +1640,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					_att_sp.yaw_body = _l1_control.nav_bearing();
 
 					/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
-					 * full throttle, otherwise we use the preTakeOff Throttle */
+					 * full throttle, otherwise we use idle throttle */
 					float takeoff_throttle = _parameters.throttle_max;
 
 					if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
-						takeoff_throttle = _launchDetector.getThrottlePreTakeoff();
+						takeoff_throttle = _parameters.throttle_idle;
 					}
 
 					/* select maximum pitch: the launchdetector may impose another limit for the pitch
@@ -1654,16 +1652,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_parameters.pitch_limit_max);
 					float takeoff_pitch_max_rad = 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) {
+					float altitude_error = pos_sp_curr.alt - _global_pos.alt;
+
+					/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
+					if (_parameters.climbout_diff > 0.0f && altitude_error > _parameters.climbout_diff) {
 						/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 						tecs_update_pitch_throttle(pos_sp_curr.alt,
-									   calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+									   _parameters.airspeed_trim,
 									   eas2tas,
 									   radians(_parameters.pitch_limit_min),
 									   takeoff_pitch_max_rad,
-									   _parameters.throttle_min, takeoff_throttle,
+									   _parameters.throttle_min,
+									   takeoff_throttle,
 									   _parameters.throttle_cruise,
 									   true,
 									   max(radians(pos_sp_curr.pitch_min), radians(10.0f)),
@@ -1910,19 +1910,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 		   pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 		   _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
 		   !_runway_takeoff.runwayTakeoffEnabled()) {
+
 		/* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons.
 		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
-		_att_sp.thrust = (_launchDetector.launchDetectionEnabled()) ? _launchDetector.getThrottlePreTakeoff() :
-				 _parameters.throttle_idle;
+		_att_sp.thrust = _parameters.throttle_idle;
 
 	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 		   _runway_takeoff.runwayTakeoffEnabled()) {
+
 		_att_sp.thrust = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
 
 	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+
 		_att_sp.thrust = 0.0f;
 
 	} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
@@ -2000,11 +2002,11 @@ FixedwingPositionControl::handle_command()
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
 			if (_land_noreturn_vertical) {
-				mavlink_log_info(&_mavlink_log_pub, "#Landing, can't abort after flare");
+				mavlink_log_info(&_mavlink_log_pub, "Landing, can't abort after flare");
 
 			} else {
 				_fw_pos_ctrl_status.abort_landing = true;
-				mavlink_log_info(&_mavlink_log_pub, "#Landing, aborted");
+				mavlink_log_info(&_mavlink_log_pub, "Landing, aborted");
 			}
 		}
 	}
@@ -2196,9 +2198,18 @@ FixedwingPositionControl::task_main()
 
 void FixedwingPositionControl::reset_takeoff_state()
 {
-	_launch_detection_state = LAUNCHDETECTION_RES_NONE;
-	_launchDetector.reset();
-	_runway_takeoff.reset();
+	// only reset takeoff if !armed or just landed
+	if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed)) {
+
+		_runway_takeoff.reset();
+
+		_launchDetector.reset();
+		_launch_detection_state = LAUNCHDETECTION_RES_NONE;
+		_launch_detection_notify = 0;
+
+	} else {
+		_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+	}
 }
 
 void FixedwingPositionControl::reset_landing_state()