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

Merge branch 'beta' into stable

parents de6d3a13 d7222923
......@@ -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
......
......@@ -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
......
......@@ -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
......@@ -495,7 +496,12 @@ 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) {
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;
......@@ -509,8 +515,8 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
_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) {
......
......@@ -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;
......
......@@ -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