diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-11 12:51:34 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-11 12:51:34 +0100 |
commit | 4d8524f508fe9e74606fe80f063801a1c47baa14 (patch) | |
tree | 7092a00025bb0f229936a44999e0c10255a60219 /src/lib | |
parent | 16dd054fa6bfc305d3a66f17d295ceaf1f554c12 (diff) | |
parent | b6a087e92183ef9cca513357919b96ea2812d298 (diff) | |
download | px4-firmware-4d8524f508fe9e74606fe80f063801a1c47baa14.tar.gz px4-firmware-4d8524f508fe9e74606fe80f063801a1c47baa14.tar.bz2 px4-firmware-4d8524f508fe9e74606fe80f063801a1c47baa14.zip |
Merge branch 'master' of github.com:PX4/Firmware into paul_estimator
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 21 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 14 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 24 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.h | 10 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchMethod.h | 8 |
5 files changed, 50 insertions, 27 deletions
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index d5c759b17..12f80c4a3 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -41,12 +41,16 @@ #include "CatapultLaunchMethod.h" #include <systemlib/err.h> -CatapultLaunchMethod::CatapultLaunchMethod() : - last_timestamp(0), +namespace launchdetection +{ + +CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : + SuperBlock(parent, "CAT"), + last_timestamp(hrt_absolute_time()), integrator(0.0f), launchDetected(false), - threshold_accel(NULL, "LAUN_CAT_A", false), - threshold_time(NULL, "LAUN_CAT_T", false) + threshold_accel(this, "A"), + threshold_time(this, "T") { } @@ -83,8 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected() return launchDetected; } -void CatapultLaunchMethod::updateParams() + +void CatapultLaunchMethod::reset() { - threshold_accel.update(); - threshold_time.update(); + integrator = 0.0f; + launchDetected = false; +} + } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index e943f11e9..55c46ff3f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -44,22 +44,24 @@ #include "LaunchMethod.h" #include <drivers/drv_hrt.h> +#include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -class CatapultLaunchMethod : public LaunchMethod +namespace launchdetection +{ + +class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock { public: - CatapultLaunchMethod(); + CatapultLaunchMethod(SuperBlock *parent); ~CatapultLaunchMethod(); void update(float accel_x); bool getLaunchDetected(); - void updateParams(); + void reset(); private: hrt_abstime last_timestamp; -// float threshold_accel_raw; -// float threshold_time; float integrator; bool launchDetected; @@ -69,3 +71,5 @@ private: }; #endif /* CATAPULTLAUNCHMETHOD_H_ */ + +} diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index df9f2fe95..bf539701b 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -42,12 +42,16 @@ #include "CatapultLaunchMethod.h" #include <systemlib/err.h> +namespace launchdetection +{ + LaunchDetector::LaunchDetector() : - launchdetection_on(NULL, "LAUN_ALL_ON", false), - throttlePreTakeoff(NULL, "LAUN_THR_PRE", false) + SuperBlock(NULL, "LAUN"), + launchdetection_on(this, "ALL_ON"), + throttlePreTakeoff(this, "THR_PRE") { /* init all detectors */ - launchMethods[0] = new CatapultLaunchMethod(); + launchMethods[0] = new CatapultLaunchMethod(this); /* update all parameters of all detectors */ @@ -59,6 +63,12 @@ LaunchDetector::~LaunchDetector() } +void LaunchDetector::reset() +{ + /* Reset all detectors */ + launchMethods[0]->reset(); +} + void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { @@ -81,12 +91,4 @@ bool LaunchDetector::getLaunchDetected() return false; } -void LaunchDetector::updateParams() { - - launchdetection_on.update(); - throttlePreTakeoff.update(); - - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - launchMethods[i]->updateParams(); - } } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 7c2ff826c..8066ebab3 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -45,18 +45,21 @@ #include <stdint.h> #include "LaunchMethod.h" - +#include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -class __EXPORT LaunchDetector +namespace launchdetection +{ + +class __EXPORT LaunchDetector : public control::SuperBlock { public: LaunchDetector(); ~LaunchDetector(); + void reset(); void update(float accel_x); bool getLaunchDetected(); - void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -71,5 +74,6 @@ private: }; +} #endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index bfb5f8cb4..01fa7050e 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -41,14 +41,20 @@ #ifndef LAUNCHMETHOD_H_ #define LAUNCHMETHOD_H_ +namespace launchdetection +{ + class LaunchMethod { public: virtual void update(float accel_x) = 0; virtual bool getLaunchDetected() = 0; - virtual void updateParams() = 0; + virtual void reset() = 0; + protected: private: }; +} + #endif /* LAUNCHMETHOD_H_ */ |