diff options
-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 | 52 |
6 files changed, 60 insertions, 11 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 ed6d8792c..774758ef4 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 @@ -176,6 +176,8 @@ private: bool launch_detected; bool usePreTakeoffThrust; + bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) + /* Landingslope object */ Landingslope landingslope; @@ -344,6 +346,16 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + /* + * Reset takeoff state + */ + int reset_takeoff_state(); + + /* + * Reset landing state + */ + int reset_landing_state(); }; namespace l1_control @@ -389,6 +401,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detected(false), + last_manual(false), usePreTakeoffThrust(false), flare_curve_alt_last(0.0f), launchDetector(), @@ -1022,19 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // mission is active _loiter_hold = false; - /* reset land state */ + /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { - land_noreturn_horizontal = false; - land_noreturn_vertical = false; - land_stayonground = false; - land_motor_lim = false; - land_onslope = false; + reset_landing_state(); } /* reset takeoff/launch state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { - launch_detected = false; - usePreTakeoffThrust = false; + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1131,6 +1139,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; + + /* reset landing and takeoff state */ + if (!last_manual) { + reset_landing_state(); + reset_takeoff_state(); + } } if (usePreTakeoffThrust) { @@ -1141,6 +1155,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _att_sp.pitch_body = _tecs.get_pitch_demand(); + if (_control_mode.flag_control_position_enabled) { + last_manual = false; + } else { + last_manual = true; + } + return setpoint; } @@ -1291,6 +1311,22 @@ FixedwingPositionControl::task_main() _exit(0); } +int FixedwingPositionControl::reset_takeoff_state() +{ + launch_detected = false; + usePreTakeoffThrust = false; + launchDetector.reset(); +} + +int FixedwingPositionControl::reset_landing_state() +{ + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; +} + int FixedwingPositionControl::start() { |