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_ */