Merge branch 'fw_autoland_att_tecs' into fw_autoland_att_tecs_navigator

Conflicts:
	src/lib/external_lgpl/tecs/tecs.cpp
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

src/lib/external_lgpl/tecs/tecs.cpp

@@ -169,35 +169,45 @@ 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 velRateMax;
-	float velRateMin;
+//	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;
+//		_TAS_rate_dem = velRateMax;
+//
+//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+//		_TAS_rate_dem = velRateMin;
+//
+//	} else {
+//		_TAS_dem_adj = _TAS_dem;
+//
+//
+//	_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+//	}
 
-	if ((_badDescent) || (_underspeed)) {
+	_TAS_dem_adj = _TAS_dem;
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
-		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;
-		_TAS_rate_dem = velRateMax;
-
-	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
-		_TAS_rate_dem = velRateMin;
-
-	} else {
-		_TAS_dem_adj = _TAS_dem;
-		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
-	}
 
 	// 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;
+//	_TAS_dem_last = _TAS_dem;
+
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f,  _badDescent %u , _underspeed %u",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state,  _badDescent , _underspeed);
 }
 
 void TECS::_update_height_demand(float demand, float state)
@@ -240,7 +250,7 @@ void TECS::_update_height_demand(float demand, float state)
 		_hgt_rate_dem = -_maxSinkRate;
 	}
 
-	// warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
+	//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
 }
 
 void TECS::_detect_underspeed(void)
@@ -300,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
 		// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
 		// drag increase during turns.
 		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
-		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
 			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
@@ -368,14 +378,18 @@ void TECS::_detect_bad_descent(void)
 	// 1) Underspeed protection not active
 	// 2) Specific total energy error > 0
 	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
-	float STEdot = _SPEdot + _SKEdot;
+//	float STEdot = _SPEdot + _SKEdot;
+//
+//	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+//
+//		warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
+//		_badDescent = true;
+//
+//	} else {
+//		_badDescent = false;
+//	}
 
-	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+	_badDescent = false;
-		_badDescent = true;
-
-	} else {
-		_badDescent = false;
-	}
 }
 
 void TECS::_update_pitch(void)

src/lib/external_lgpl/tecs/tecs.h

@@ -184,6 +184,10 @@ public:
 		_heightrate_p = heightrate_p;
 	}
 
+	void set_speedrate_p(float speedrate_p) {
+		_speedrate_p = speedrate_p;
+	}
+
 private:
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
@@ -208,6 +212,7 @@ private:
 	float _rollComp;
 	float _spdWeight;
 	float _heightrate_p;
+	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
 	float _throttle_dem;

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -217,6 +217,7 @@ private:
 		float loiter_hold_radius;
 
 		float heightrate_p;
+		float speedrate_p;
 
 		float land_slope_angle;
 		float land_slope_length;
@@ -260,6 +261,7 @@ private:
 		param_t loiter_hold_radius;
 
 		param_t heightrate_p;
+		param_t speedrate_p;
 
 		param_t land_slope_angle;
 		param_t land_slope_length;
@@ -431,6 +433,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
 	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
 	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
+	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -497,6 +500,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
 
 	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
 
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
 	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
@@ -523,6 +527,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
 	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
 	_tecs.set_heightrate_p(_parameters.heightrate_p);
+	_tecs.set_speedrate_p(_parameters.speedrate_p);
 
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
@@ -957,7 +962,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			if (altitude_error > 15.0f) {
 
 				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
 							    _airspeed.indicated_airspeed_m_s, eas2tas,
 							    true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)),
 							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 
 PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
 
 PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
 PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);