diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-23 09:10:36 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-23 09:10:36 +0200 |
commit | 52ffc3bced314d0d2ed36763e8761696f3c46b5d (patch) | |
tree | 4458d7d0f77c1334cc9c2f31257e27b6eb1e24ab /src/lib/launchdetection | |
parent | 8d3dd7363dbf9a29d88d2abb3364739749894684 (diff) | |
parent | 708ee8ae3aa4d4d5eac9e2fd86ed9673c126fe33 (diff) | |
download | px4-firmware-52ffc3bced314d0d2ed36763e8761696f3c46b5d.tar.gz px4-firmware-52ffc3bced314d0d2ed36763e8761696f3c46b5d.tar.bz2 px4-firmware-52ffc3bced314d0d2ed36763e8761696f3c46b5d.zip |
Merge remote-tracking branch 'julian/launchdelay' into launchdetectionstates
Diffstat (limited to 'src/lib/launchdetection')
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 19 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 4 | ||||
-rw-r--r-- | src/lib/launchdetection/launchdetection_params.c | 9 |
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. |