launchdetector: add reset, #663

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -42,7 +42,7 @@
 #include <systemlib/err.h>
 
 CatapultLaunchMethod::CatapultLaunchMethod() :
-	last_timestamp(0),
+	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
 	launchDetected(false),
 	threshold_accel(NULL, "LAUN_CAT_A", false),
@@ -88,3 +88,9 @@ void CatapultLaunchMethod::updateParams()
 	threshold_accel.update();
 	threshold_time.update();
 }
+
+void CatapultLaunchMethod::reset()
+{
+	integrator = 0.0f;
+	launchDetected = false;
+}

src/lib/launchdetection/CatapultLaunchMethod.h

@@ -55,11 +55,10 @@ public:
 	void update(float accel_x);
 	bool getLaunchDetected();
 	void updateParams();
+	void reset();
 
 private:
 	hrt_abstime last_timestamp;
-//	float threshold_accel_raw;
-//	float threshold_time;
 	float integrator;
 	bool launchDetected;
 

src/lib/launchdetection/LaunchDetector.cpp

@@ -59,6 +59,12 @@ LaunchDetector::~LaunchDetector()
 
 }
 
+void LaunchDetector::reset()
+{
+	/* Reset all detectors */
+	launchMethods[0]->reset();
+}
+
 void LaunchDetector::update(float accel_x)
 {
 	if (launchdetection_on.get() == 1) {

src/lib/launchdetection/LaunchDetector.h

@@ -53,6 +53,7 @@ class __EXPORT LaunchDetector
 public:
 	LaunchDetector();
 	~LaunchDetector();
+	void reset();
 
 	void update(float accel_x);
 	bool getLaunchDetected();

src/lib/launchdetection/LaunchMethod.h

@@ -47,6 +47,7 @@ public:
 	virtual void update(float accel_x) = 0;
 	virtual bool getLaunchDetected() = 0;
 	virtual void updateParams() = 0;
+	virtual void reset() = 0;
 protected:
 private:
 };

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1055,6 +1055,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
 			launch_detected = false;
 			usePreTakeoffThrust = false;
+			launchDetector.reset();
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {