Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 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
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+3
View File
@@ -39,5 +39,8 @@ then
param set FW_RR_P 0.3
fi
# Enable gamepad / joystick support
param set COM_RC_IN_MODE 2
set HIL yes
set MIXER AERT
ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
+3
View File
@@ -11,4 +11,7 @@ sh /etc/init.d/rc.mc_defaults
set MIXER quad_x
# Enable gamepad / joystick support
param set COM_RC_IN_MODE 2
set HIL yes
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+3
View File
@@ -11,4 +11,7 @@ sh /etc/init.d/rc.mc_defaults
set MIXER quad_+
# Enable gamepad / joystick support
param set COM_RC_IN_MODE 2
set HIL yes
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+3
View File
@@ -11,4 +11,7 @@ sh /etc/init.d/rc.fw_defaults
set HIL yes
# Enable gamepad / joystick support
param set COM_RC_IN_MODE 2
set MIXER AERT
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
+3
View File
@@ -36,5 +36,8 @@ fi
set HIL yes
# Enable gamepad / joystick support
param set COM_RC_IN_MODE 2
# Set the AERT mixer for HIL (even if the malolo is a flying wing)
set MIXER AERT
ROMFS/px4fmu_common/init.d/rc.fw_defaults
-4
View File
@@ -11,10 +11,6 @@ then
param set NAV_RTL_ALT 100
param set NAV_RTL_LAND_T -1
param set NAV_ACC_RAD 50
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
param set PE_VELNE_NOISE 0.3
param set PE_VELD_NOISE 0.35
src/lib/external_lgpl/tecs/tecs.cpp
+64 -53
View File
@@ -48,8 +48,6 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
bool reset_altitude = false;
_in_air = in_air;
if (_update_50hz_last_usec == 0 || DT > DT_MAX) {
DT = 0.02f; // when first starting TECS, use a
// small time constant
@@ -69,6 +67,8 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
_update_50hz_last_usec = now;
_EAS = airspeed;
_in_air = in_air;
// Get height acceleration
float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G);
// Perform filter calculation using backwards Euler integration
@@ -108,7 +108,9 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
_vel_dot = 0.0f;
}
_states_initalized = true;
if (!_in_air) {
_states_initalized = false;
}
}
@@ -179,40 +181,38 @@ 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;
// }
//
// Use 50% of maximum energy rate to allow margin for total energy controller
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;
// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * _DT)) {
// _TAS_dem_adj = _TAS_dem_adj + velRateMax * _DT;
// _TAS_rate_dem = velRateMax;
//
// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * _DT)) {
// _TAS_dem_adj = _TAS_dem_adj + velRateMin * _DT;
// _TAS_rate_dem = velRateMin;
//
// } else {
// _TAS_dem_adj = _TAS_dem;
//
//
// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / _DT;
// }
_TAS_dem_adj = _TAS_dem;
_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
_TAS_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax);;
_TAS_rate_dem = constrain((_TAS_dem_adj - _integ5_state) * _speedrate_p, velRateMin, velRateMax); //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;
// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
@@ -223,23 +223,29 @@ void TECS::_update_speed_demand(void)
void TECS::_update_height_demand(float demand, float state)
{
// // Apply 2 point moving average to demanded height
// // This is required because height demand is only updated at 5Hz
// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
// _hgt_dem_in_old = _hgt_dem;
//
// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
// // _maxClimbRate);
//
// // Limit height rate of change
// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
//
// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
// }
//
// _hgt_dem_prev = _hgt_dem;
// Handle initialization
if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
_hgt_dem_in_old = demand;
}
// Apply 2 point moving average to demanded height
// This is required because height demand is updated in steps
if (isfinite(demand)) {
_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
} else {
_hgt_dem = _hgt_dem_in_old;
}
_hgt_dem_in_old = _hgt_dem;
// Limit height demand
// this is important to avoid a windup
if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * _DT)) {
_hgt_dem = _hgt_dem_prev + _maxClimbRate * _DT;
} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * _DT)) {
_hgt_dem = _hgt_dem_prev - _maxSinkRate * _DT;
}
_hgt_dem_prev = _hgt_dem;
//
// // Apply first order lag to height demand
// _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
@@ -249,9 +255,9 @@ void TECS::_update_height_demand(float demand, float state)
// // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
// // _hgt_rate_dem);
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
_hgt_dem_adj = 0.1f * _hgt_dem + 0.9f * _hgt_dem_adj_last;
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_rate_dem = (_hgt_dem_adj - state) * _heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last) / _DT;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
@@ -485,7 +491,7 @@ void TECS::_update_pitch(void)
void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
{
// Initialise states and variables if DT > 1 second or in climbout
if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air) {
if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air || !_states_initalized) {
_integ6_state = 0.0f;
_integ7_state = 0.0f;
_last_throttle_dem = throttle_cruise;
@@ -498,8 +504,10 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
_TAS_dem_adj = _TAS_dem;
_underspeed = false;
_badDescent = false;
_DT = DT_MIN; // when first starting TECS, use a
// small time constant
if (_DT > 1.0f || _DT < 0.001f) {
_DT = DT_MIN;
}
} else if (_climbOutDem) {
_PITCHminf = ptchMinCO_rad;
@@ -512,6 +520,8 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
_underspeed = false;
_badDescent = false;
}
_states_initalized = true;
}
void TECS::_update_STE_rate_lim(void)
@@ -526,9 +536,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max)
{
if (!_states_initalized) {
return;
}
// Calculate time in seconds since last update
uint64_t now = ecl_absolute_time();
@@ -537,9 +544,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
// _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
// Update the speed estimate using a 2nd order complementary filter
_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
// Convert inputs
_THRmaxf = throttle_max;
_THRminf = throttle_min;
@@ -550,6 +554,13 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
if (!_in_air) {
return;
}
// Update the speed estimate using a 2nd order complementary filter
_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
// Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim();
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 */