fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 05:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mtecs: landing mode which limits pitch and as well throttle at the end of the landing

Browse Source
This commit is contained in:
Thomas Gubler
2014-05-01 13:53:47 +02:00
parent 76e47c3135
commit dd8bfc2a0b
4 changed files with 60 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+8 -14
View File
@@ -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> &current_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> &current_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,
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+8
View File
@@ -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;
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+7 -3
View File
@@ -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 */
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+37
View File
@@ -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);