Merge branch 'beta' into stable

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -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

@@ -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
+	// Use 50% of maximum energy rate to allow margin for total energy controller
-//	float velRateMax;
+	float velRateMax;
-//	float velRateMin;
+	float velRateMin;
-//
+
-//	if ((_badDescent) || (_underspeed)) {
+	if ((_badDescent) || (_underspeed)) {
-//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-//
+
-//	} else {
+	} else {
-//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-//	}
+	}
-//
+
 //	// Apply rate limit
-//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * _DT)) {
-//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * _DT;
 //		_TAS_rate_dem = velRateMax;
 //
-//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * _DT)) {
-//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+//		_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_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax);;
-	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
+	_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
+	// Handle initialization
-//	// This is required because height demand is only updated at 5Hz
+	if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
-//	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+		_hgt_dem_in_old = demand;
-//	_hgt_dem_in_old = _hgt_dem;
+	}
-//
+	// Apply 2 point moving average to demanded height
-//	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
+	// This is required because height demand is updated in steps
-//	// 	_maxClimbRate);
+	if (isfinite(demand)) {
-//
+		_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
-//	// Limit height rate of change
+	} else {
-//	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
+		_hgt_dem = _hgt_dem_in_old;
-//		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+	}
-//
+	_hgt_dem_in_old = _hgt_dem;
-//	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
+
-//		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+	// Limit height demand
-//	}
+	// this is important to avoid a windup
-//
+	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * _DT)) {
-//	_hgt_dem_prev = _hgt_dem;
+		_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_dem_adj = 0.1f * _hgt_dem + 0.9f * _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_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

@@ -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

@@ -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_params.c

@@ -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

@@ -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 */