Merge remote-tracking branch 'julian/launchdelay' into launchdetectionstates

3 files changed, 29 insertions, 3 deletions

diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index 9d479832f..2b876b629 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -51,7 +51,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
 	integrator(0.0f),
 	launchDetected(false),
 	threshold_accel(this, "A"),
-	threshold_time(this, "T")
+	threshold_time(this, "T"),
+	delay(this, "DELAY")
 {
 
 }
@@ -78,21 +79,33 @@ void CatapultLaunchMethod::update(float accel_x)
 //				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
 		/* reset integrator */
 		integrator = 0.0f;
-		launchDetected = false;
+	}
+
+	if (launchDetected) {
+		delayCounter += dt;
+		if (delayCounter > delay.get()) {
+			delayPassed = true;
+		}
 	}
 
 }
 
 bool CatapultLaunchMethod::getLaunchDetected()
 {
-	return launchDetected;
+	if (delay.get() > 0.0f) {
+		return delayPassed;
+	} else {
+		return launchDetected;
+	}
 }
 
 
 void CatapultLaunchMethod::reset()
 {
 	integrator = 0.0f;
+	delayCounter = 0.0f;
 	launchDetected = false;
+	delayPassed = false;
 }
 
 }
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index 23757f6f3..a64d482f6 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -63,10 +63,14 @@ public:
 private:
 	hrt_abstime last_timestamp;
 	float integrator;
+	float delayCounter;
+
 	bool launchDetected;
+	bool delayPassed;
 
 	control::BlockParamFloat threshold_accel;
 	control::BlockParamFloat threshold_time;
+	control::BlockParamFloat delay;
 
 };
 
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 8df8c696c..02f01bc66 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -78,6 +78,15 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
 PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 
 /**
+ * Catapult delay
+ *
+ *
+ *
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_DELAY, 0.0f);
+/**
  * Throttle setting while detecting launch.
  *
  * The throttle is set to this value while the system is waiting for the take-off.