From 5a5617cb597b16b789a6fa175410d667f45cf907 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 11:42:53 +0200 Subject: [PATCH] launchdetector: return state of detection The launchdetector now has a intermediate state (controlsenabled) which is meant to be interpreted by the controller as: "perform attitude control but do not yet power up the motor". This can be used in the floating phase during a bungee launch for example. --- .../launchdetection/CatapultLaunchMethod.cpp | 70 +++++++++++-------- .../launchdetection/CatapultLaunchMethod.h | 13 ++-- src/lib/launchdetection/LaunchDetector.cpp | 27 +++++-- src/lib/launchdetection/LaunchDetector.h | 8 ++- src/lib/launchdetection/LaunchMethod.h | 13 +++- .../launchdetection/launchdetection_params.c | 8 ++- 6 files changed, 91 insertions(+), 48 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 2b876b6291..65ae461dbc 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -49,10 +49,10 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : SuperBlock(parent, "CAT"), last_timestamp(hrt_absolute_time()), integrator(0.0f), - launchDetected(false), - threshold_accel(this, "A"), - threshold_time(this, "T"), - delay(this, "DELAY") + state(LAUNCHDETECTION_RES_NONE), + thresholdAccel(this, "A"), + thresholdTime(this, "T"), + motorDelay(this, "MDEL") { } @@ -66,46 +66,56 @@ void CatapultLaunchMethod::update(float accel_x) float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; last_timestamp = hrt_absolute_time(); - if (accel_x > threshold_accel.get()) { - integrator += dt; -// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", -// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_time.get()) { - launchDetected = true; + switch (state) { + case LAUNCHDETECTION_RES_NONE: + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ + if (accel_x > thresholdAccel.get()) { + integrator += dt; + if (integrator > thresholdTime.get()) { + if (motorDelay.get() > 0.0f) { + state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full" + " throttle", (double)motorDelay.get()); + } else { + /* No motor delay set: go directly to enablemotors state */ + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + warnx("Launch detected: state: enablemotors (delay not activated)"); + } + } + } else { + /* reset */ + reset(); } + break; - } else { -// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", -// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - /* reset integrator */ - integrator = 0.0f; - } - - if (launchDetected) { - delayCounter += dt; - if (delayCounter > delay.get()) { - delayPassed = true; + case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: + /* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is + * over to allow full throttle */ + motorDelayCounter += dt; + if (motorDelayCounter > motorDelay.get()) { + warnx("Launch detected: state enablemotors"); + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + break; + + default: + break; + } } -bool CatapultLaunchMethod::getLaunchDetected() +LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const { - if (delay.get() > 0.0f) { - return delayPassed; - } else { - return launchDetected; - } + return state; } void CatapultLaunchMethod::reset() { integrator = 0.0f; - delayCounter = 0.0f; - launchDetected = false; - delayPassed = false; + motorDelayCounter = 0.0f; + state = LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index a64d482f6a..d918c3a763 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -57,20 +57,19 @@ public: ~CatapultLaunchMethod(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected() const; void reset(); private: hrt_abstime last_timestamp; float integrator; - float delayCounter; + float motorDelayCounter; - bool launchDetected; - bool delayPassed; + LaunchDetectionResult state; - control::BlockParamFloat threshold_accel; - control::BlockParamFloat threshold_time; - control::BlockParamFloat delay; + control::BlockParamFloat thresholdAccel; + control::BlockParamFloat thresholdTime; + control::BlockParamFloat motorDelay; }; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 3bf47bbb03..2958c0a315 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -46,6 +46,7 @@ namespace launchdetection LaunchDetector::LaunchDetector() : SuperBlock(NULL, "LAUN"), + activeLaunchDetectionMethodIndex(-1), launchdetection_on(this, "ALL_ON"), throttlePreTakeoff(this, "THR_PRE") { @@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector() void LaunchDetector::reset() { /* Reset all detectors */ - launchMethods[0]->reset(); + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + launchMethods[i]->reset(); + } + + /* Reset active launchdetector */ + activeLaunchDetectionMethodIndex = -1; + + } void LaunchDetector::update(float accel_x) @@ -77,17 +85,24 @@ void LaunchDetector::update(float accel_x) } } -bool LaunchDetector::getLaunchDetected() +LaunchDetectionResult LaunchDetector::getLaunchDetected() { if (launchdetection_on.get() == 1) { - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - if(launchMethods[i]->getLaunchDetected()) { - return true; + if (activeLaunchDetectionMethodIndex < 0) { + /* None of the active launchmethods has detected a launch, check all launchmethods */ + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + warnx("selecting launchmethod %d", i); + activeLaunchDetectionMethodIndex = i; // from now on only check this method + return launchMethods[i]->getLaunchDetected(); + } } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected(); } } - return false; + return LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 1a214b66ee..b48e724bad 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,7 +59,7 @@ public: void reset(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -67,6 +67,12 @@ public: // virtual bool getLaunchDetected(); protected: private: + int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods + which detected a Launch. If no launchMethod has detected a launch yet the + value is -1. Once one launchMetthod has detected a launch only this + method is checked for further adavancing in the state machine (e.g. when + to power up the motors) */ + LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index e04467f6a4..d2f091cea7 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -44,11 +44,22 @@ namespace launchdetection { +enum LaunchDetectionResult { + LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ + LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should + control the attitude. However any motors should not throttle + up and still be set to 'throttlePreTakeoff'. + For instance this is used to have a delay for the motor + when launching a fixed wing aircraft from a bungee */ + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control + attitude and also throttle up the motors. */ +}; + class LaunchMethod { public: virtual void update(float accel_x) = 0; - virtual bool getLaunchDetected() = 0; + virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; protected: diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 02f01bc667..d35eb11f60 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -78,14 +78,16 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); /** - * Catapult delay - * + * Motor delay * + * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller) + * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate * + * @unit seconds * @min 0 * @group Launch detection */ -PARAM_DEFINE_FLOAT(LAUN_CAT_DELAY, 0.0f); +PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); /** * Throttle setting while detecting launch. *