From 1ebea1e7590d0d66b18cd39fcc45897bca8b8256 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 1/5] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 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 01d94a8881..8e516a708f 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 @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From c91bb76b42d8d85339902c0b29123243156a0f0b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 2/5] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 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 01d94a8881..8e516a708f 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 @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From 5c59d7a434791222a6fad49e64206432a86c22bf Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 23:05:25 +0200 Subject: [PATCH 3/5] do not run tecs if we are on ground to prevent integrator filling --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 8e516a708f..05988ef53a 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 @@ -1024,7 +1024,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { + if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); } @@ -1757,6 +1757,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, unsigned mode, bool pitch_max_special) { + /* do not run tecs if we are not in air */ + if (_vehicle_status.condition_landed) { + return; + } + if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; From 6ce106eea465b9bbaf59f4d44a2954bd107d1035 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:36:26 +0200 Subject: [PATCH 4/5] limit minimum pitch in altitude controller modes if in a takeoff situation --- .../fw_pos_control_l1_main.cpp | 63 +++++++++++++------ 1 file changed, 45 insertions(+), 18 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 05988ef53a..c262e80c5f 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 @@ -389,13 +389,16 @@ private: float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** - * Control position. + * Check if we are in a takeoff situation */ + bool in_takeoff_situation(); /** * Do takeoff help when in altitude controlled modes + * @param hold_altitude altitude setpoint for controller + * @param pitch_limit_min minimum pitch allowed */ - bool do_takeoff_help(); + void do_takeoff_help(float *hold_altitude, float *pitch_limit_min); /** * Update desired altitude base on user pitch stick input @@ -405,6 +408,9 @@ private: */ bool update_desired_altitude(float dt); + /** + * Control position. + */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); @@ -987,20 +993,26 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -bool FixedwingPositionControl::do_takeoff_help() -{ +bool FixedwingPositionControl::in_takeoff_situation() { const hrt_abstime delta_takeoff = 10000000; const float throttle_threshold = 0.1f; - const float delta_alt_takeoff = 50.0f; - float climbout = false; - - /* demand 30 m above ground if user switched into this mode during takeoff */ - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { - _hold_alt = _ground_alt + delta_alt_takeoff; - climbout = true; + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) { + return true; + } + + return false; +} + +void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min) +{ + /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ + if (in_takeoff_situation()) { + *hold_altitude = _ground_alt + _parameters.climbout_diff; + *pitch_limit_min = math::radians(10.0f); + } else { + *pitch_limit_min = _parameters.pitch_limit_min; } - return climbout; } bool @@ -1419,8 +1431,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); + /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1437,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); @@ -1452,6 +1468,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } + /* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration + to make sure the plane does not start rolling + */ + if (in_takeoff_situation()) { + _hdg_hold_enabled = false; + _yaw_lock_engaged = true; + } + if (_yaw_lock_engaged) { /* just switched back from non heading-hold to heading hold */ @@ -1512,8 +1536,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1530,7 +1557,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); From 0446efa9a4b8e9399f64d4c65303bab971342b3e Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:46:37 +0200 Subject: [PATCH 5/5] limit roll angle in loiter and position control mode if we are in a takeoff situation --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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 c262e80c5f..e4682689af 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 @@ -1134,6 +1134,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1505,6 +1511,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } } } else { _hdg_hold_enabled = false;