Merge branch 'beta' into stable

ROMFS/px4fmu_common/init.d/rc.uavcan

@@ -3,16 +3,50 @@
 # UAVCAN initialization script.
 #
 
+#
+# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable.
+# TODO there should be a smarter way.
+#
+set UAVCAN_ENABLE 0
 if param compare UAVCAN_ENABLE 1
+then
+	set UAVCAN_ENABLE 1
+fi
+if param compare UAVCAN_ENABLE 2
+then
+	set UAVCAN_ENABLE 2
+fi
+
+echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE"
+
+#
+# Starting stuff according to UAVCAN_ENABLE value
+#
+if [ $UAVCAN_ENABLE -ge 1 ]
 then
 	if uavcan start
 	then
-		# First sensor publisher to initialize takes lowest instance ID
-		# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
-		sleep 1
 		echo "[i] UAVCAN started"
 	else
 		echo "[i] ERROR: Could not start UAVCAN"
 		tone_alarm $TUNE_ERR
 	fi
 fi
+
+if [ $UAVCAN_ENABLE -ge 2 ]
+then
+	if uavcan start fw
+	then
+		echo "[i] UAVCAN servers started"
+	else
+		echo "[i] ERROR: Could not start UAVCAN servers"
+		tone_alarm $TUNE_ERR
+	fi
+fi
+
+if [ $UAVCAN_ENABLE -ge 1 ]
+then
+	# First sensor publisher to initialize takes lowest instance ID
+	# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs
+	sleep 8
+fi

ROMFS/px4fmu_common/init.d/rc.usb

@@ -3,22 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 800000 -d /dev/ttyACM0 -x
-# Enable a number of interesting streams we want via USB
-mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300
-mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
-mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
-mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10
-mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
-mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100
-mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
-mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
-mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
-mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
-mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
-mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5
-mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100
-mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20
+mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
 
 # Exit shell to make it available to MAVLink
 exit

ROMFS/px4fmu_common/init.d/rcS

@@ -299,6 +299,11 @@ then
 	then
 	fi
 
+	#
+	# UAVCAN
+	#
+	sh /etc/init.d/rc.uavcan
+
 	#
 	# Sensors System (start before Commander so Preflight checks are properly run)
 	#
@@ -491,10 +496,6 @@ then
 		fi
 	fi
 
-	# UAVCAN
-	#
-	sh /etc/init.d/rc.uavcan
-
 	#
 	# Logging
 	#

msg/tecs_status.msg

@@ -18,9 +18,12 @@ float32 airspeed_filtered
 float32 airspeedDerivativeSp
 float32 airspeedDerivative
 
-float32 totalEnergyRateSp
-float32 totalEnergyRate
-float32 energyDistributionRateSp
-float32 energyDistributionRate
+float32 totalEnergyError
+float32 energyDistributionError
+float32 totalEnergyRateError
+float32 energyDistributionRateError
+
+float32 throttle_sp
+float32 pitch_sp
 
 uint8 mode

src/drivers/px4fmu/fmu.cpp

@@ -355,7 +355,7 @@ PX4FMU::init()
 	_task = task_spawn_cmd("fmuservo",
 			       SCHED_DEFAULT,
 			       SCHED_PRIORITY_DEFAULT,
-			       1600,
+			       1200,
 			       (main_t)&PX4FMU::task_main_trampoline,
 			       nullptr);
 

src/lib/external_lgpl/tecs/tecs.cpp

@@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 	// Calculate total energy values
 	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
 	float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
-	float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
+	_STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
 
 	// Apply 0.5 second first order filter to STEdot_error
 	// This is required to remove accelerometer noise from the  measurement
-	STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
-	_STEdotErrLast = STEdot_error;
+	_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast;
+	_STEdotErrLast = _STEdot_error;
 
 	// Calculate throttle demand
 	// If underspeed condition is set, then demand full throttle
@@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		}
 
 		// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
-		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+		_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
 		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
 
 		// Rate limit PD + FF throttle
@@ -438,11 +438,11 @@ void TECS::_update_pitch(void)
 	// 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;
-	float SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
-	float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * 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
-	float integ7_input = SEB_error * _integGain;
+	float integ7_input = _SEB_error * _integGain;
 
 	if (_pitch_dem_unc > _PITCHmaxf) {
 		integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
@@ -460,7 +460,7 @@ void TECS::_update_pitch(void)
 	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
 	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
-	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
 	if (_climbOutDem)
 	{
 		temp += _PITCHminf * gainInv;
@@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 		_tecs_state.mode = ECL_TECS_MODE_NORMAL;
 	}
 
-	_tecs_state.hgt_dem  = _hgt_dem_adj;
-	_tecs_state.hgt      = _integ3_state;
-	_tecs_state.dhgt_dem = _hgt_rate_dem;
-	_tecs_state.dhgt     = _integ2_state;
-	_tecs_state.spd_dem  = _TAS_dem_adj;
-	_tecs_state.spd      = _integ5_state;
-	_tecs_state.dspd     = _vel_dot;
-	_tecs_state.ithr     = _integ6_state;
-	_tecs_state.iptch    = _integ7_state;
-	_tecs_state.thr      = _throttle_dem;
-	_tecs_state.ptch     = _pitch_dem;
-	_tecs_state.dspd_dem = _TAS_rate_dem;
+	_tecs_state.altitude_sp = _hgt_dem_adj;
+	_tecs_state.altitude_filtered = _integ3_state;
+	_tecs_state.altitude_rate_sp = _hgt_rate_dem;
+	_tecs_state.altitude_rate = _integ2_state;
+
+	_tecs_state.airspeed_sp = _TAS_dem_adj;
+	_tecs_state.airspeed_rate_sp = _TAS_rate_dem;
+	_tecs_state.airspeed_filtered = _integ5_state;
+	_tecs_state.airspeed_rate = _vel_dot;
+
+	_tecs_state.total_energy_error = _STE_error;
+	_tecs_state.energy_distribution_error = _SEB_error;
+	_tecs_state.total_energy_rate_error = _STEdot_error;
+	_tecs_state.energy_distribution_error = _SEBdot_error;
+
+	_tecs_state.energy_error_integ = _integ6_state;
+	_tecs_state.energy_distribution_error_integ = _integ7_state;
+
+
+	_tecs_state.throttle_sp = _throttle_dem;
+	_tecs_state.pitch_sp = _pitch_dem;
 
 	_update_pitch_throttle_last_usec = now;
 

src/lib/external_lgpl/tecs/tecs.h

@@ -79,6 +79,10 @@ class __EXPORT TECS
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
+		_STE_error(0.0f),
+		_STEdot_error(0.0f),
+		_SEB_error(0.0f),
+		_SEBdot_error(0.0f),
 		_airspeed_enabled(false),
 		_states_initalized(false),
 		_in_air(false),
@@ -137,18 +141,22 @@ class __EXPORT TECS
 
 	struct tecs_state {
 		uint64_t timestamp;
-		float hgt;
-		float dhgt;
-		float hgt_dem;
-		float dhgt_dem;
-		float spd_dem;
-		float spd;
-		float dspd;
-		float ithr;
-		float iptch;
-		float thr;
-		float ptch;
-		float dspd_dem;
+		float altitude_filtered;
+		float altitude_sp;
+		float altitude_rate;
+		float altitude_rate_sp;
+		float airspeed_filtered;
+		float airspeed_sp;
+		float airspeed_rate;
+		float airspeed_rate_sp;
+		float energy_error_integ;
+		float energy_distribution_error_integ;
+		float total_energy_error;
+		float total_energy_rate_error;
+		float energy_distribution_error;
+		float energy_distribution_rate_error;
+		float throttle_sp;
+		float pitch_sp;
 		enum ECL_TECS_MODE mode;
 	};
 
@@ -376,6 +384,15 @@ class __EXPORT TECS
 	// Specific energy error quantities
 	float _STE_error;
 
+	// Energy error rate
+	float _STEdot_error;
+
+	// Specific energy balance error
+	float _SEB_error;
+
+	// Specific energy balance error rate
+	float _SEBdot_error;
+
 	// Time since last update of main TECS loop (seconds)
 	float _DT;
 

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -1115,7 +1115,7 @@ int AttitudePositionEstimatorEKF::start()
 	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
-					 7500,
+					 4800,
 					 (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 

src/modules/ekf_att_pos_estimator/module.mk

@@ -42,5 +42,5 @@ SRCS		= ekf_att_pos_estimator_main.cpp \
 		  estimator_22states.cpp \
 		  estimator_utilities.cpp
 
-EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=6100
+EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=3400
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1920,22 +1920,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 				break;
 		}
 
-		t.altitudeSp			= s.hgt_dem;
-		t.altitude_filtered		= s.hgt;
-		t.airspeedSp			= s.spd_dem;
-		t.airspeed_filtered		= s.spd;
-
-		t.flightPathAngleSp		= s.dhgt_dem;
-		t.flightPathAngle		= s.dhgt;
-		t.flightPathAngleFiltered	= s.dhgt;
-
-		t.airspeedDerivativeSp		= s.dspd_dem;
-		t.airspeedDerivative		= s.dspd;
-
-		t.totalEnergyRateSp		= s.thr;
-		t.totalEnergyRate		= s.ithr;
-		t.energyDistributionRateSp	= s.ptch;
-		t.energyDistributionRate	= s.iptch;
+		t.altitudeSp 		= s.altitude_sp;
+		t.altitude_filtered = s.altitude_filtered;
+		t.airspeedSp 		= s.airspeed_sp;
+		t.airspeed_filtered = s.airspeed_filtered;
+
+		t.flightPathAngleSp 		= s.altitude_rate_sp;
+		t.flightPathAngle 			= s.altitude_rate;
+		t.flightPathAngleFiltered 	= s.altitude_rate;
+
+		t.airspeedDerivativeSp 	= s.airspeed_rate_sp;
+		t.airspeedDerivative 	= s.airspeed_rate;
+
+		t.totalEnergyError 				= s.total_energy_error;
+		t.totalEnergyRateError 			= s.total_energy_rate_error;
+		t.energyDistributionError 		= s.energy_distribution_error;
+		t.energyDistributionRateError 	= s.energy_distribution_rate_error;
+
+		t.throttle_sp 	= s.throttle_sp;
+		t.pitch_sp 		= s.pitch_sp;
 
 		if (_tecs_status_pub > 0) {
 			orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	 * is running) */
 	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
-	/* Write part of the status message */
-	_status.flightPathAngleSp = flightPathAngleSp;
-	_status.flightPathAngle = flightPathAngle;
-	_status.flightPathAngleFiltered = flightPathAngleFiltered;
-	_status.airspeedDerivativeSp = airspeedDerivativeSp;
-	_status.airspeedDerivative = airspeedDerivative;
-	_status.totalEnergyRateSp = totalEnergyRateSp;
-	_status.totalEnergyRate = totalEnergyRate;
-	_status.energyDistributionRateSp = energyDistributionRateSp;
-	_status.energyDistributionRate = energyDistributionRate;
-	_status.mode = mode;
+	// /* Write part of the status message */
+	// _status.flightPathAngleSp = flightPathAngleSp;
+	// _status.flightPathAngle = flightPathAngle;
+	// _status.flightPathAngleFiltered = flightPathAngleFiltered;
+	// _status.airspeedDerivativeSp = airspeedDerivativeSp;
+	// _status.airspeedDerivative = airspeedDerivative;
+	// _status.totalEnergyRateSp = totalEnergyRateSp;
+	// _status.totalEnergyRate = totalEnergyRate;
+	// _status.energyDistributionRateSp = energyDistributionRateSp;
+	// _status.energyDistributionRate = energyDistributionRate;
+	// _status.mode = mode;
 
 	/** update control blocks **/
 	/* update total energy rate control block */