diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-02-17 19:48:45 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-02-17 20:45:05 +0100 |
commit | acf680389e95c82d2e0df1253f16842d320f3504 (patch) | |
tree | 3306566b97aae41ea080fd5e33b2ce6609b2e81b | |
parent | 838290fa63cf5592e5c9d7590d0f359f5f81aac3 (diff) | |
download | px4-firmware-acf680389e95c82d2e0df1253f16842d320f3504.tar.gz px4-firmware-acf680389e95c82d2e0df1253f16842d320f3504.tar.bz2 px4-firmware-acf680389e95c82d2e0df1253f16842d320f3504.zip |
launchdetector: add reset, #663
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 8 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 3 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 6 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.h | 1 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchMethod.h | 1 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 |
6 files changed, 17 insertions, 3 deletions
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index d5c759b17..1039b4a09 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -42,7 +42,7 @@ #include <systemlib/err.h> CatapultLaunchMethod::CatapultLaunchMethod() : - last_timestamp(0), + last_timestamp(hrt_absolute_time()), integrator(0.0f), launchDetected(false), threshold_accel(NULL, "LAUN_CAT_A", false), @@ -88,3 +88,9 @@ void CatapultLaunchMethod::updateParams() threshold_accel.update(); threshold_time.update(); } + +void CatapultLaunchMethod::reset() +{ + integrator = 0.0f; + launchDetected = false; +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index e943f11e9..b8476b4c8 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -55,11 +55,10 @@ public: 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; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index df9f2fe95..4109a90ba 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -59,6 +59,12 @@ LaunchDetector::~LaunchDetector() } +void LaunchDetector::reset() +{ + /* Reset all detectors */ + launchMethods[0]->reset(); +} + void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 7c2ff826c..05708c526 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -53,6 +53,7 @@ class __EXPORT LaunchDetector public: LaunchDetector(); ~LaunchDetector(); + void reset(); void update(float accel_x); bool getLaunchDetected(); diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index bfb5f8cb4..0cfbab3e0 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -47,6 +47,7 @@ public: virtual void update(float accel_x) = 0; virtual bool getLaunchDetected() = 0; virtual void updateParams() = 0; + virtual void reset() = 0; protected: private: }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 45fdaa355..a4142266b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1055,6 +1055,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; + launchDetector.reset(); } if (was_circle_mode && !_l1_control.circle_mode()) { |