From acf680389e95c82d2e0df1253f16842d320f3504 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 17 Feb 2014 19:48:45 +0100 Subject: [PATCH 1/3] launchdetector: add reset, #663 --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 8 +++++++- src/lib/launchdetection/CatapultLaunchMethod.h | 3 +-- src/lib/launchdetection/LaunchDetector.cpp | 6 ++++++ src/lib/launchdetection/LaunchDetector.h | 1 + src/lib/launchdetection/LaunchMethod.h | 1 + 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 d5c759b17b..1039b4a091 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -42,7 +42,7 @@ #include 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 e943f11e9d..b8476b4c87 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 df9f2fe95c..4109a90bad 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 7c2ff826cf..05708c5262 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 bfb5f8cb4c..0cfbab3e09 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 45fdaa355e..a4142266b1 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()) { From f2b46389ee8c8e9dd73ad9ac1fc8170c759e8b1c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Feb 2014 21:42:41 +0100 Subject: [PATCH 2/3] fw: reset landing and takeoff state if switched to manual --- .../fw_pos_control_l1_main.cpp | 55 +++++++++++++++---- 1 file changed, 45 insertions(+), 10 deletions(-) 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 a4142266b1..ab2a2439bb 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; @@ -346,6 +348,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 @@ -396,7 +408,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false) + usePreTakeoffThrust(false), + last_manual(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -1042,20 +1055,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; - launchDetector.reset(); + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1150,6 +1157,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) { @@ -1160,6 +1173,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; } @@ -1310,6 +1329,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() { From 647142764fa3ffd5d99f4d43950975cc6a19bded Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 9 Mar 2014 22:08:28 +0400 Subject: [PATCH 3/3] position_estimator_inav: hotfix, change lower dt limit from 5 ms to 2 ms --- src/modules/position_estimator_inav/inertial_filter.c | 5 +---- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 13328edb4b..7cd0769486 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3]) void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * dt; - if (ewdt > 1.0f) - ewdt = 1.0f; // prevent over-correcting - ewdt *= e; + float ewdt = e * w * dt; x[i] += ewdt; if (i == 0) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d6d03367b6..a14354138b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.005); + dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; /* use GPS if it's valid and reference position initialized */