launchdetector: add reset, #663

5 files changed, 16 insertions, 3 deletions

diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index d5c759b17..1039b4a09 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/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;
+}
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index e943f11e9..b8476b4c8 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/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;
 
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index df9f2fe95..4109a90ba 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/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) {
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 7c2ff826c..05708c526 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -53,6 +53,7 @@ class __EXPORT LaunchDetector
 public:
 	LaunchDetector();
 	~LaunchDetector();
+	void reset();
 
 	void update(float accel_x);
 	bool getLaunchDetected();
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index bfb5f8cb4..0cfbab3e0 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/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:
 };