diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index ccbc22fef7eef4aad607cd931850ee4b1be67694..d514d895f4ed317334e8c9abf1a0a4d9d750d759 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -58,7 +58,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : } -CatapultLaunchMethod::~CatapultLaunchMethod() { +CatapultLaunchMethod::~CatapultLaunchMethod() +{ } @@ -69,34 +70,41 @@ void CatapultLaunchMethod::update(float accel_x) 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()); + " 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; case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: /* Vehicle is currently controlling attitude but not with full throttle. Waiting until 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: @@ -119,10 +127,12 @@ void CatapultLaunchMethod::reset() state = LAUNCHDETECTION_RES_NONE; } -float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) { +float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) +{ /* If motor is turned on do not impose the extra limit on maximum pitch */ if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { return pitchMaxDefault; + } else { return pitchMaxPreThrottle.get(); } diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 6bcae853441827f3907a800f0c06b2fcc0eac749..38e556e88c9abb2ba1a894e49896a9105bb9f3ef 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -89,14 +89,15 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() { if (launchdetection_on.get() == 1) { if (activeLaunchDetectionMethodIndex < 0) { - /* None of the active launchmethods has detected a launch, check all launchmethods */ + /* None of the active launchmethods has detected a launch, check all launchmethods */ for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { - if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + 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(); } @@ -105,7 +106,8 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() return LAUNCHDETECTION_RES_NONE; } -float LaunchDetector::getPitchMax(float pitchMaxDefault) { +float LaunchDetector::getPitchMax(float pitchMaxDefault) +{ if (!launchdetection_on.get()) { return pitchMaxDefault; } @@ -113,11 +115,13 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault) { /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method, * otherwise use the default limit */ if (activeLaunchDetectionMethodIndex < 0) { - if (sizeof(launchMethods)/sizeof(LaunchMethod *) > 1) { + if (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) { return pitchMaxDefault; + } else { return launchMethods[0]->getPitchMax(pitchMaxDefault); } + } else { return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); }