autolaunch: added param for delay

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;
 }
 
 }

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;
 
 };
 

src/lib/launchdetection/launchdetection_params.c

@@ -77,6 +77,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.
  *