diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-10-05 13:17:32 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-10-05 13:17:32 +0200 |
commit | 7f9a231b4014bce4ad44c4eb61bfa0e7efeb73fa (patch) | |
tree | 1fee14fa887b396a76581ed643f16abe4cf0d1aa /src/lib/launchdetection/LaunchDetector.cpp | |
parent | 9bda573151ae1b5fa87686ee58596ea14e052941 (diff) | |
parent | 80ed01a5b4324ab13b88dca0b0c8e5ea4f8841ce (diff) | |
download | px4-firmware-7f9a231b4014bce4ad44c4eb61bfa0e7efeb73fa.tar.gz px4-firmware-7f9a231b4014bce4ad44c4eb61bfa0e7efeb73fa.tar.bz2 px4-firmware-7f9a231b4014bce4ad44c4eb61bfa0e7efeb73fa.zip |
Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages
Conflicts:
src/modules/navigator/navigator.h
src/modules/navigator/navigator_main.cpp
Diffstat (limited to 'src/lib/launchdetection/LaunchDetector.cpp')
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 47 |
1 files changed, 41 insertions, 6 deletions
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 3bf47bbb0..52f5c3257 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,44 @@ 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; } +float LaunchDetector::getPitchMax(float pitchMaxDefault) { + if (!launchdetection_on.get()) { + return 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) { + return pitchMaxDefault; + } else { + return launchMethods[0]->getPitchMax(pitchMaxDefault); + } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); + } + +} + + } |