From daf16184202b02119aa1ac83cb82cf85979d43f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Sep 2014 12:42:42 +0200 Subject: additional upper pitch limit during launch The pitch limit can be used by the laucnhdetector to limit pitch during critical phases of a launch. For example this can be used to limit pitch while attached to a bungee differently from the standard pitch limit. --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) (limited to 'src/modules/fw_pos_control_l1') 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 84c14be01..402b171f5 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 @@ -1115,7 +1115,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_rad = math::radians( + launchDetector.getPitchMax(_parameters.pitch_limit_max)); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ @@ -1123,7 +1129,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), + takeoff_pitch_max_rad, _parameters.throttle_min, takeoff_throttle, _parameters.throttle_cruise, true, @@ -1199,7 +1205,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Copy thrust and pitch values from tecs * making sure again that the correct thrust is used, - * without depending on library calls */ + * without depending on library calls for safety reasons */ if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); -- cgit v1.2.3 From df181455ebdcfacda73615adba40fa224b4d074c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Sep 2014 13:00:26 +0200 Subject: launch pitch limit: add mtecs interface --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) (limited to 'src/modules/fw_pos_control_l1') 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 402b171f5..81e25da99 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 @@ -381,7 +381,8 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode = TECS_MODE_NORMAL); + tecs_mode mode = TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -1117,8 +1118,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ - float takeoff_pitch_max_rad = math::radians( - launchDetector.getPitchMax(_parameters.pitch_limit_max)); + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* apply minimum pitch and limit roll if target altitude is not within climbout_diff * meters */ @@ -1137,7 +1138,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - TECS_MODE_TAKEOFF); + TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), @@ -1391,7 +1393,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode) + tecs_mode mode, bool pitch_max_special) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ @@ -1406,6 +1408,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else { limitOverride.disablePitchMinOverride(); } + + if (pitch_max_special) { + /* Use the maximum pitch from the argument */ + limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { + /* use pitch max set by MT param */ + limitOverride.disablePitchMaxOverride(); + } _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { -- cgit v1.2.3