fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 13:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta'

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-04 10:56:53 +02:00
parent 5fd88f4d3e 5b3449e75b
commit 420acb9748
12 changed files with 124 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
-4
View File
@@ -163,7 +163,6 @@ private:
struct {
float tconst;
float p_p;
float p_d;
float p_i;
float p_ff;
float p_rmax_pos;
@@ -171,7 +170,6 @@ private:
float p_integrator_max;
float p_roll_feedforward;
float r_p;
float r_d;
float r_i;
float r_ff;
float r_integrator_max;
@@ -208,7 +206,6 @@ private:
param_t tconst;
param_t p_p;
param_t p_d;
param_t p_i;
param_t p_ff;
param_t p_rmax_pos;
@@ -216,7 +213,6 @@ private:
param_t p_integrator_max;
param_t p_roll_feedforward;
param_t r_p;
param_t r_d;
param_t r_i;
param_t r_ff;
param_t r_integrator_max;
src/modules/fw_att_control/fw_att_control_params.c
+5 -5
View File
@@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
* @max 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.01f);
PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f);
/**
* Maximum positive / up pitch rate.
@@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.01f);
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
/**
* Maximum negative / down pitch rate.
@@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
/**
* Pitch rate integrator limit
@@ -185,7 +185,7 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
/**
* Yaw rate proportional gain
@@ -258,7 +258,7 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
/**
* Yaw rate feed forward
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+35 -18
View File
@@ -179,6 +179,8 @@ private:
float _hdg_hold_yaw; /**< hold heading for velocity mode */
bool _hdg_hold_enabled; /**< heading hold enabled */
bool _yaw_lock_engaged; /**< yaw is locked for heading hold */
float _althold_epv; /**< the position estimate accuracy when engaging alt hold */
bool _was_in_deadband; /**< wether the last stick input was in althold deadband */
struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */
struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */
hrt_abstime _control_position_last_called; /**<last call of control_position */
@@ -508,6 +510,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_hdg_hold_yaw(0.0f),
_hdg_hold_enabled(false),
_yaw_lock_engaged(false),
_althold_epv(0.0f),
_was_in_deadband(false),
_hdg_hold_prev_wp{},
_hdg_hold_curr_wp{},
_control_position_last_called(0),
@@ -968,12 +972,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
bool FixedwingPositionControl::update_desired_altitude(float dt)
{
const float deadBand = (60.0f/1000.0f);
/*
* The complete range is -1..+1, so this is 6%
* of the up or down range or 3% of the total range.
*/
const float deadBand = 0.06f;
/*
* The correct scaling of the complete range needs
* to account for the missing part of the slope
* due to the deadband
*/
const float factor = 1.0f - deadBand;
// XXX this should go into a manual stick mapper
// class
static float _althold_epv = 0.0f;
static bool was_in_deadband = false;
/* Climbout mode sets maximum throttle and pitch up */
bool climbout_mode = false;
/*
@@ -988,24 +1000,29 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
_althold_epv = _global_pos.epv;
}
// XXX the sign magic in this function needs to be fixed
/*
* Manual control has as convention the rotation around
* an axis. Positive X means to rotate positively around
* the X axis in NED frame, which is pitching down
*/
if (_manual.x > deadBand) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
was_in_deadband = false;
climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH);
/* pitching down */
float pitch = -(_manual.x - deadBand) / factor;
_hold_alt += (_parameters.max_sink_rate * dt) * pitch;
_was_in_deadband = false;
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
was_in_deadband = false;
} else if (!was_in_deadband) {
/* pitching up */
float pitch = -(_manual.x + deadBand) / factor;
_hold_alt += (_parameters.max_climb_rate * dt) * pitch;
_was_in_deadband = false;
climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH);
} else if (!_was_in_deadband) {
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
_althold_epv = _global_pos.epv;
was_in_deadband = true;
_was_in_deadband = true;
}
return climbout_mode;
@@ -1485,7 +1502,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
pitch_limit_min,
((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@@ -1595,7 +1612,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
pitch_limit_min,
((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+2 -2
View File
@@ -327,7 +327,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
/**
* Speed <--> Altitude priority
@@ -378,7 +378,7 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);
/**
* Landing slope angle
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+3 -3
View File
@@ -204,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
mode = tecs_status_s::TECS_MODE_UNDERSPEED;
}
/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
/* Set special output limiters if we are not in TECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
@@ -221,7 +221,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
/* Apply override given by the limitOverride argument (this is used for limits which are not given by
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
* is running) */
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
@@ -253,7 +253,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
(double)accelerationLongitudinalSp, (double)airspeedDerivative);
}
/* publish status messge */
/* publish status message */
_status.update();
/* clean up */