Skip to content

Commit

Permalink
Merge branch 'beta' into stable
Browse files Browse the repository at this point in the history
  • Loading branch information
@LorenzMeier
LorenzMeier committed Aug 30, 2015
2 parents de6d3a1 + d722292 commit 8580ac0
Show file tree
Hide file tree
Showing 5 changed files with 46 additions and 38 deletions.
4 changes: 2 additions & 2 deletions ROMFS/px4fmu_common/init.d/10016_3dr_iris
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,11 @@ if [ $AUTOCNF == yes ]
then
# TODO tune roll/pitch separately
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_P 0.15
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 2.5
Expand Down
4 changes: 2 additions & 2 deletions ROMFS/px4fmu_common/init.d/4010_dji_f330
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,11 @@ sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_P 0.15
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
Expand Down
64 changes: 35 additions & 29 deletions src/lib/external_lgpl/tecs/tecs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
bool reset_altitude = false;

if (_update_50hz_last_usec == 0 || DT > DT_MAX) {
DT = 0.02f; // when first starting TECS, use a
// small time constant
DT = DT_DEFAULT; // when first starting TECS, use small time constant
reset_altitude = true;
}

Expand Down Expand Up @@ -132,14 +131,6 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
_TASmax = indicated_airspeed_max * EAS2TAS;
_TASmin = indicated_airspeed_min * EAS2TAS;

// Reset states of time since last update is too large
if (_update_speed_last_usec == 0 || DT > 1.0f || !_in_air) {
_integ5_state = (_EAS * EAS2TAS);
_integ4_state = 0.0f;
DT = 0.1f; // when first starting TECS, use a
// small time constant
}

// Get airspeed or default to halfway between min and max if
// airspeed is not being used and set speed rate to zero
if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
Expand All @@ -150,6 +141,16 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
_EAS = indicated_airspeed;
}

// Reset states on initial execution or if not active
if (_update_speed_last_usec == 0 || !_in_air) {
_integ4_state = 0.0f;
_integ5_state = (_EAS * EAS2TAS);
}

if (DT < DT_MIN || DT > DT_MAX) {
DT = DT_DEFAULT; // when first starting TECS, use small time constant
}

// Implement a second order complementary filter to obtain a
// smoothed airspeed estimate
// airspeed estimate is held in _integ5_state
Expand Down Expand Up @@ -440,9 +441,9 @@ void TECS::_update_pitch(void)
float SPE_weighting = 2.0f - SKE_weighting;

// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);

// Calculate integrator state, constraining input if pitch limits are exceeded
Expand Down Expand Up @@ -495,22 +496,27 @@ 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 || !_states_initalized) {
_integ6_state = 0.0f;
_integ7_state = 0.0f;
if (_update_pitch_throttle_last_usec == 0 || _DT > DT_MAX || !_in_air || !_states_initalized) {
_integ1_state = 0.0f;
_integ2_state = 0.0f;
_integ3_state = baro_altitude;
_integ4_state = 0.0f;
_integ5_state = _EAS;
_integ6_state = 0.0f;
_integ7_state = 0.0f;
_last_throttle_dem = throttle_cruise;
_last_pitch_dem = pitch;
_hgt_dem_adj_last = baro_altitude;
_hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last;
_hgt_dem_in_old = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem;
_underspeed = false;
_badDescent = false;
_last_pitch_dem = pitch;
_hgt_dem_adj_last = baro_altitude;
_hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last;
_hgt_dem_in_old = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem;
_underspeed = false;
_badDescent = false;

if (_DT > 1.0f || _DT < 0.001f) {
_DT = DT_MIN;
if (_DT > DT_MAX || _DT < DT_MIN) {
_DT = DT_DEFAULT;
}

} else if (_climbOutDem) {
Expand Down Expand Up @@ -549,8 +555,8 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// _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);

// Convert inputs
_THRmaxf = throttle_max;
_THRminf = throttle_min;
_THRmaxf = throttle_max;
_THRminf = throttle_min;
_PITCHmaxf = pitch_limit_max;
_PITCHminf = pitch_limit_min;
_climbOutDem = climbOutDem;
Expand Down
4 changes: 3 additions & 1 deletion src/lib/external_lgpl/tecs/tecs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ class __EXPORT TECS
_integ7_state(0.0f),
_last_pitch_dem(0.0f),
_vel_dot(0.0f),
_EAS(0.0f),
_TAS_dem(0.0f),
_TAS_dem_last(0.0f),
_hgt_dem_in_old(0.0f),
Expand Down Expand Up @@ -396,7 +397,8 @@ class __EXPORT TECS
// Time since last update of main TECS loop (seconds)
float _DT;

static constexpr float DT_MIN = 0.1f;
static constexpr float DT_MIN = 0.001f;
static constexpr float DT_DEFAULT = 0.02f;
static constexpr float DT_MAX = 1.0f;

bool _airspeed_enabled;
Expand Down
8 changes: 4 additions & 4 deletions src/modules/mc_att_control/mc_att_control_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <[email protected]>
* @author Lorenz Meier <[email protected]>
* @author Anton Babushkin <anton[email protected]>
* @author Lorenz Meier <[email protected]>
* @author Anton Babushkin <anton@px4.io>
*/

#include <systemlib/param/param.h>
Expand All @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f);

/**
* Roll rate I gain
Expand Down Expand Up @@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f);

/**
* Pitch rate I gain
Expand Down

0 comments on commit 8580ac0

Please sign in to comment.