diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 22 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 8 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.h | 10 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 37 |
4 files changed, 60 insertions, 17 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 44afa43d8..5cdab10a1 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 @@ -362,12 +362,14 @@ private: /* * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) + * XXX need to clean up/remove this function once mtecs fully replaces TECS */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, bool climbout_mode, float climbout_pitch_min_rad, - const math::Vector<3> &ground_speed); + const math::Vector<3> &ground_speed, + fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL); }; @@ -958,7 +960,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas, flare_pitch_angle_rad, math::radians(15.0f), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad, ground_speed); + false, flare_pitch_angle_rad, ground_speed, land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -1031,7 +1033,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed); + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); /* 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)); @@ -1355,24 +1357,16 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, bool climbout_mode, float climbout_pitch_min_rad, - const math::Vector<3> &ground_speed) + const math::Vector<3> &ground_speed, + fwPosctrl::mTecs::tecs_mode mode) { if (_mTecs.getEnabled()) { - float flightPathAngle = 0.0f; float ground_speed_length = ground_speed.length(); if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - - if (!climbout_mode) { - /* Normal operation */ - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL); - } else { - /* Climbout/Takeoff mode requested */ - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); - } - + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode); } else { _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index a4cf0bdf8..b717429d3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -63,6 +63,8 @@ mTecs::mTecs() : _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), + _BlockOutputLimiterLandThrottle(this, "LND_THR"), + _BlockOutputLimiterLandPitch(this, "LND_PIT", true), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), _dtCalculated(false), @@ -148,6 +150,12 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh if (mode == TECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == TECS_MODE_LAND) { + // only limit pitch but do not limit throttle + outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == TECS_MODE_LAND_THROTTLELIM) { + outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; + outputLimiterPitch = &_BlockOutputLimiterLandPitch; } else if (mode == TECS_MODE_UNDERSPEED) { outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index fab4b161a..9ed233ba0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -61,7 +61,9 @@ public: typedef enum { TECS_MODE_NORMAL, TECS_MODE_UNDERSPEED, - TECS_MODE_TAKEOFF + TECS_MODE_TAKEOFF, + TECS_MODE_LAND, + TECS_MODE_LAND_THROTTLELIM } tecs_mode; /* @@ -111,8 +113,10 @@ protected: /* Output Limits in special modes */ BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index fd501c17a..3a05f2c88 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -323,3 +323,40 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); +/** + * Minimal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); + +/** + * Maximal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); + +/** + * Minimal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); + +/** + * Maximal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); |