Merge branch 'beta' into stable

NuttX @ 8891d035

-Subproject commit 678eea3d2c157c686439597abb0367b0f1e643f4
+Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee

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
 	#

v1.0 @ c390a63a

-Subproject commit d23a551e108d92c7a49a66d162cb9d542bbe6be1
+Subproject commit c390a63abfce15af0629bdf207c4b6a183fed118

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 @@ public:
 		_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 @@ public:
 
 	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 @@ private:
 	// 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;
 

uavcan @ 3ae5400a

-Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46
+Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd

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

src/modules/mavlink/mavlink_main.cpp

@@ -1353,6 +1353,8 @@ Mavlink::task_main(int argc, char *argv[])
 				_mode = MAVLINK_MODE_ONBOARD;
 			} else if (strcmp(optarg, "osd") == 0) {
 				_mode = MAVLINK_MODE_OSD;
+			} else if (strcmp(optarg, "config") == 0) {
+				_mode = MAVLINK_MODE_CONFIG;
 			}
 
 			break;
@@ -1449,8 +1451,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	_receive_thread = MavlinkReceiver::receive_start(this);
 
-	_task_running = true;
-
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
 	uint64_t param_time = 0;
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
@@ -1537,6 +1537,23 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("RC_CHANNELS_RAW", 5.0f);
 		break;
 
+	case MAVLINK_MODE_CONFIG:
+		// Enable a number of interesting streams we want via USB
+		configure_stream("PARAM_VALUE", 300.0f);
+		configure_stream("MISSION_ITEM", 50.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
+		configure_stream("VFR_HUD", 20.0f);
+		configure_stream("ATTITUDE", 100.0f);
+		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
+		configure_stream("RC_CHANNELS_RAW", 5.0f);
+		configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
+		configure_stream("MANUAL_CONTROL", 5.0f);
+		configure_stream("HIGHRES_IMU", 100.0f);
+		configure_stream("GPS_RAW_INT", 20.0f);
+
 	default:
 		break;
 	}
@@ -1703,6 +1720,9 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		perf_end(_loop_perf);
+
+		/* confirm task running only once fully initialized */
+		_task_running = true;
 	}
 
 	delete _subscribe_to_stream;

src/modules/mavlink/mavlink_main.h

@@ -129,7 +129,8 @@ public:
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
 		MAVLINK_MODE_ONBOARD,
-		MAVLINK_MODE_OSD
+		MAVLINK_MODE_OSD,
+		MAVLINK_MODE_CONFIG
 	};
 
 	void			set_mode(enum MAVLINK_MODE);

src/modules/sdlog2/sdlog2.c

@@ -105,6 +105,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/git_version.h>
+#include <systemlib/printload.h>
 #include <version/version.h>
 
 #include <mavlink/mavlink_log.h>
@@ -727,9 +728,16 @@ void sdlog2_start_log()
 	}
 
 	/* write all performance counters */
+	hrt_abstime curr_time = hrt_absolute_time();
+	struct print_load_s load;
 	int perf_fd = open_perf_file("preflight");
+	init_print_load_s(curr_time, &load);
+	print_load(curr_time, perf_fd, &load);
 	dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
 	perf_print_all(perf_fd);
+	dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n");
+	usleep(500 * 1000);
+	print_load(hrt_absolute_time(), perf_fd, &load);
 	close(perf_fd);
 
 	/* reset performance counters to get in-flight min and max values in post flight log */
@@ -765,8 +773,15 @@ void sdlog2_stop_log()
 
 	/* write all performance counters */
 	int perf_fd = open_perf_file("postflight");
+	hrt_abstime curr_time = hrt_absolute_time();
 	dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
 	perf_print_all(perf_fd);
+	struct print_load_s load;
+	dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n");
+	init_print_load_s(curr_time, &load);
+	print_load(curr_time, perf_fd, &load);
+	sleep(1);
+	print_load(hrt_absolute_time(), perf_fd, &load);
 	close(perf_fd);
 
 	/* free log writer performance counter */
@@ -1863,15 +1878,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
-			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
 			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
-			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
-			log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
-			log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
-			log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
+			log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
+			log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
+			log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
+			log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
+			log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp;
+			log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp;
 			log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
 			LOGBUFFER_WRITE_AND_COUNT(TECS);
 		}

src/modules/sdlog2/sdlog2_messages.h

@@ -360,16 +360,16 @@ struct log_TECS_s {
 	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
-	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
-
-	float totalEnergyRateSp;
-	float totalEnergyRate;
-	float energyDistributionRateSp;
-	float energyDistributionRate;
+	float totalEnergyError;
+	float totalEnergyRateError;
+	float energyDistributionError;
+	float energyDistributionRateError;
+	float throttle_sp;
+	float pitch_sp;
 
 	uint8_t mode;
 };
@@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 	LOG_FORMAT(ENCD, "qfqf",	"cnt0,vel0,cnt1,vel1"),
 	LOG_FORMAT(TSYN, "Q", 		"TimeOffset"),

src/modules/systemlib/module.mk

@@ -42,6 +42,7 @@ SRCS		 = err.c \
 		   bson/tinybson.c \
 		   conversions.c \
 		   cpuload.c \
+		   printload.c \
 		   getopt_long.c \
 		   up_cxxinitialize.c \
 		   pid/pid.c \

src/modules/systemlib/printload.c

+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file printload.c
+ *
+ * Print the current system load.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include <string.h>
+#include <stdio.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/printload.h>
+#include <drivers/drv_hrt.h>
+
+extern struct system_load_s system_load;
+
+#define CL "\033[K" // clear line
+
+void init_print_load_s(uint64_t t, struct print_load_s *s)
+{
+
+	s->total_user_time = 0;
+
+	s->running_count = 0;
+	s->blocked_count = 0;
+
+	s->new_time = t;
+	s->interval_start_time = t;