From 0611b26eeace29a366247026534872c714abd76d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 08:32:53 +0100 Subject: fw autoland: move constrain of roll to horizontal landing navigation Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') 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 84983785b..bfdca9cce 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 @@ -837,6 +837,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + land_noreturn_horizontal = true; } else { @@ -911,8 +914,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio 0.0f, throttle_max, throttle_land, flare_angle_rad, math::radians(15.0f)); - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); if (!land_noreturn_vertical) { mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); land_noreturn_vertical = true; -- cgit v1.2.3 From 881c89dd1b55f5e2dbb355562665a94dcc618217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 11:02:41 +0100 Subject: increase safety margin for takeoff speed --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') 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 bfdca9cce..2b1c01c83 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 @@ -972,7 +972,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(1.3f*_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, -- cgit v1.2.3 From b6f2c286e9774194f75d25d272ad66e9dc63bcd5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 11:09:14 +0100 Subject: disabling bad descent detection because of to many false positives --- src/lib/external_lgpl/tecs/tecs.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 510b8ed9c..1ae6e7653 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -368,14 +368,18 @@ void TECS::_detect_bad_descent(void) // 1) Underspeed protection not active // 2) Specific total energy error > 0 // This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set - float STEdot = _SPEdot + _SKEdot; - - if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { - _badDescent = true; +// float STEdot = _SPEdot + _SKEdot; +// +// if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { +// +// warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem); +// _badDescent = true; +// +// } else { +// _badDescent = false; +// } - } else { - _badDescent = false; - } + _badDescent = false; } void TECS::_update_pitch(void) -- cgit v1.2.3 From f267fcf4ff2da214560d3be7b9c6583cefac4475 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 11:11:06 +0100 Subject: disable a printf --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 1ae6e7653..fb7ffccda 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -240,7 +240,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_rate_dem = -_maxSinkRate; } - warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); + //warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); } void TECS::_detect_underspeed(void) -- cgit v1.2.3 From 85a76a32c5be43e8f1a4d82041e1b860dc21e217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 12:29:48 +0100 Subject: tecs: roll2thr: fix to be consistent with comment --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index fb7ffccda..a2ac6507e 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -300,7 +300,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); - STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); + STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f); if (STEdot_dem >= 0) { ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); -- cgit v1.2.3 From 3c6f01bea8a65e2c347d1b893b3fe0d152bff69c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 12:48:44 +0100 Subject: tecs: speedrate: use p loop instead of pre calculated speed rate for now --- src/lib/external_lgpl/tecs/tecs.cpp | 60 +++++++++++++--------- src/lib/external_lgpl/tecs/tecs.h | 5 ++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 1 + 4 files changed, 46 insertions(+), 25 deletions(-) (limited to 'src') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a2ac6507e..d089be080 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -169,35 +169,45 @@ void TECS::_update_speed_demand(void) // calculate velocity rate limits based on physical performance limits // provision to use a different rate limit if bad descent or underspeed condition exists // Use 50% of maximum energy rate to allow margin for total energy contgroller - float velRateMax; - float velRateMin; - - if ((_badDescent) || (_underspeed)) { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - - } else { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - } - - // Apply rate limit - if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; - _TAS_rate_dem = velRateMax; - - } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; - _TAS_rate_dem = velRateMin; +// float velRateMax; +// float velRateMin; +// +// if ((_badDescent) || (_underspeed)) { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// +// } else { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// } +// +// // Apply rate limit +// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; +// _TAS_rate_dem = velRateMax; +// +// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; +// _TAS_rate_dem = velRateMin; +// +// } else { +// _TAS_dem_adj = _TAS_dem; +// +// +// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; +// } - } else { - _TAS_dem_adj = _TAS_dem; - _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; - } + _TAS_dem_adj = _TAS_dem; + _TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now // Constrain speed demand again to protect against bad values on initialisation. _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); - _TAS_dem_last = _TAS_dem; +// _TAS_dem_last = _TAS_dem; + +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin); +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent , _underspeed); } void TECS::_update_height_demand(float demand, float state) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 06e1c8ad3..4fc009da9 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -184,6 +184,10 @@ public: _heightrate_p = heightrate_p; } + void set_speedrate_p(float speedrate_p) { + _speedrate_p = speedrate_p; + } + private: // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -208,6 +212,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 float _throttle_dem; 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 2b1c01c83..8b78bfbc4 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 @@ -217,6 +217,7 @@ private: float loiter_hold_radius; float heightrate_p; + float speedrate_p; float land_slope_angle; float land_slope_length; @@ -260,6 +261,7 @@ private: param_t loiter_hold_radius; param_t heightrate_p; + param_t speedrate_p; param_t land_slope_angle; param_t land_slope_length; @@ -431,6 +433,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -497,6 +500,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); @@ -523,6 +527,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index f206d808e..10a0c00fc 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); -- cgit v1.2.3