Merge branch 'master' of github.com:PX4/Firmware into paul_estimator

5 files changed, 50 insertions, 27 deletions

diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index d5c759b17..12f80c4a3 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -41,12 +41,16 @@
 #include "CatapultLaunchMethod.h"
 #include <systemlib/err.h>
 
-CatapultLaunchMethod::CatapultLaunchMethod() :
-	last_timestamp(0),
+namespace launchdetection
+{
+
+CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
+	SuperBlock(parent, "CAT"),
+	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
 	launchDetected(false),
-	threshold_accel(NULL, "LAUN_CAT_A", false),
-	threshold_time(NULL, "LAUN_CAT_T", false)
+	threshold_accel(this, "A"),
+	threshold_time(this, "T")
 {
 
 }
@@ -83,8 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
 	return launchDetected;
 }
 
-void CatapultLaunchMethod::updateParams()
+
+void CatapultLaunchMethod::reset()
 {
-	threshold_accel.update();
-	threshold_time.update();
+	integrator = 0.0f;
+	launchDetected = false;
+}
+
 }
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index e943f11e9..55c46ff3f 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -44,22 +44,24 @@
 #include "LaunchMethod.h"
 
 #include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-class CatapultLaunchMethod : public LaunchMethod
+namespace launchdetection
+{
+
+class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
 {
 public:
-	CatapultLaunchMethod();
+	CatapultLaunchMethod(SuperBlock *parent);
 	~CatapultLaunchMethod();
 
 	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;
 
@@ -69,3 +71,5 @@ private:
 };
 
 #endif /* CATAPULTLAUNCHMETHOD_H_ */
+
+}
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index df9f2fe95..bf539701b 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -42,12 +42,16 @@
 #include "CatapultLaunchMethod.h"
 #include <systemlib/err.h>
 
+namespace launchdetection
+{
+
 LaunchDetector::LaunchDetector() :
-	launchdetection_on(NULL, "LAUN_ALL_ON", false),
-	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
+	SuperBlock(NULL, "LAUN"),
+	launchdetection_on(this, "ALL_ON"),
+	throttlePreTakeoff(this, "THR_PRE")
 {
 	/* init all detectors */
-	launchMethods[0] = new CatapultLaunchMethod();
+	launchMethods[0] = new CatapultLaunchMethod(this);
 
 
 	/* update all parameters of all detectors */
@@ -59,6 +63,12 @@ LaunchDetector::~LaunchDetector()
 
 }
 
+void LaunchDetector::reset()
+{
+	/* Reset all detectors */
+	launchMethods[0]->reset();
+}
+
 void LaunchDetector::update(float accel_x)
 {
 	if (launchdetection_on.get() == 1) {
@@ -81,12 +91,4 @@ bool LaunchDetector::getLaunchDetected()
 	return false;
 }
 
-void LaunchDetector::updateParams() {
-
-	launchdetection_on.update();
-	throttlePreTakeoff.update();
-
-	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-		launchMethods[i]->updateParams();
-	}
 }
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 7c2ff826c..8066ebab3 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -45,18 +45,21 @@
 #include <stdint.h>
 
 #include "LaunchMethod.h"
-
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-class __EXPORT LaunchDetector
+namespace launchdetection
+{
+
+class __EXPORT LaunchDetector : public control::SuperBlock
 {
 public:
 	LaunchDetector();
 	~LaunchDetector();
+	void reset();
 
 	void update(float accel_x);
 	bool getLaunchDetected();
-	void updateParams();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -71,5 +74,6 @@ private:
 
 };
 
+}
 
 #endif // LAUNCHDETECTOR_H
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index bfb5f8cb4..01fa7050e 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -41,14 +41,20 @@
 #ifndef LAUNCHMETHOD_H_
 #define LAUNCHMETHOD_H_
 
+namespace launchdetection
+{
+
 class LaunchMethod
 {
 public:
 	virtual void update(float accel_x) = 0;
 	virtual bool getLaunchDetected() = 0;
-	virtual void updateParams() = 0;
+	virtual void reset() = 0;
+
 protected:
 private:
 };
 
+}
+
 #endif /* LAUNCHMETHOD_H_ */