Skip to content

GitLab

Commit 8580ac01 authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Merge branch 'beta' into stable

parents de6d3a13 d7222923
Hide whitespace changes
Inline Side-by-side
ROMFS/px4fmu_common/init.d/10016_3dr_iris
View file @ 8580ac01
......@@ -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
......
ROMFS/px4fmu_common/init.d/4010_dji_f330
View file @ 8580ac01
......@@ -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
......
src/lib/external_lgpl/tecs/tecs.cpp
View file @ 8580ac01
......@@ -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;
}
......@@ -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()) {
......@@ -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
......@@ -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
......@@ -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) {
......@@ -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;
......
src/lib/external_lgpl/tecs/tecs.h
View file @ 8580ac01
......@@ -60,6 +60,7 @@ public:
_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),
......@@ -396,7 +397,8 @@ private:
// 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;
......
src/modules/mc_att_control/mc_att_control_params.c
View file @ 8580ac01
......@@ -36,8 +36,8 @@
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
......@@ -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
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment