Merge branch 'master' into smooth_pos_hold

src/lib/external_lgpl/tecs/tecs.cpp

@@ -554,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	// Calculate pitch demand
 	_update_pitch();
 
-//    // Write internal variables to the log_tuning structure. This
-//    // structure will be logged in dataflash at 10Hz
-	// log_tuning.hgt_dem  = _hgt_dem_adj;
-	// log_tuning.hgt      = _integ3_state;
-	// log_tuning.dhgt_dem = _hgt_rate_dem;
-	// log_tuning.dhgt     = _integ2_state;
-	// log_tuning.spd_dem  = _TAS_dem_adj;
-	// log_tuning.spd      = _integ5_state;
-	// log_tuning.dspd     = _vel_dot;
-	// log_tuning.ithr     = _integ6_state;
-	// log_tuning.iptch    = _integ7_state;
-	// log_tuning.thr      = _throttle_dem;
-	// log_tuning.ptch     = _pitch_dem;
-	// log_tuning.dspd_dem = _TAS_rate_dem;
+	_tecs_state.timestamp = now;
+
+	if (_underspeed) {
+		_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
+	} else if (_badDescent) {
+		_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
+	} else if (_climbOutDem) {
+		_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
+	} else {
+		// If no error flag applies, conclude normal
+		_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;
+
 }

src/lib/external_lgpl/tecs/tecs.h

@@ -28,6 +28,7 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
+		_tecs_state {},
 		_update_50hz_last_usec(0),
 		_update_speed_last_usec(0),
 		_update_pitch_throttle_last_usec(0),
@@ -120,24 +121,33 @@ public:
 		return _spdWeight;
 	}
 
-	// log data on internal state of the controller. Called at 10Hz
-	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
-
-	// struct PACKED log_TECS_Tuning {
-	// 	LOG_PACKET_HEADER;
-	// 	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;
-	// } log_tuning;
+	enum ECL_TECS_MODE {
+		ECL_TECS_MODE_NORMAL = 0,
+		ECL_TECS_MODE_UNDERSPEED,
+		ECL_TECS_MODE_BAD_DESCENT,
+		ECL_TECS_MODE_CLIMBOUT
+	};
+
+	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;
+		enum ECL_TECS_MODE mode;
+	};
+
+	void get_tecs_state(struct tecs_state& state) {
+		state = _tecs_state;
+	}
 
 	void set_time_const(float time_const) {
 		_timeConst = time_const;
@@ -212,6 +222,9 @@ public:
 	}
 
 private:
+
+	struct tecs_state _tecs_state;
+
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
 

src/modules/commander/commander.cpp

@@ -393,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
-	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
 		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -1085,7 +1085,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 
-				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
 					arming_state_changed = true;
 				}
@@ -1288,14 +1288,14 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
 				}
 
 			} else {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
@@ -1309,7 +1309,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
@@ -1368,7 +1368,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 					if (arming_ret == TRANSITION_CHANGED) {
 						arming_state_changed = true;
 					}
@@ -1394,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
 					if (status.main_state != MAIN_STATE_MANUAL) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 					} else {
-						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
 						}
@@ -2156,7 +2156,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -2219,7 +2219,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				break;
 			}

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         armed.ready_to_arm = test->current_state.ready_to_arm;
         
         // Attempt transition
-        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 			MTT_ALL_NOT_VALID,
 			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
-		{ "transition: MANUAL to AUTO_MISSION - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
-		{ "transition: AUTO_MISSION to MANUAL - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",

src/modules/commander/state_machine_helper.cpp

@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
 };
 
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
-			const struct safety_s   *safety,            /// current safety settings
-			arming_state_t          new_arming_state,   /// arming state requested
-			struct actuator_armed_s *armed,             /// current armed status
-			const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
+arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
+			const struct safety_s   *safety,		///< current safety settings
+			arming_state_t          new_arming_state,	///< arming state requested
+			struct actuator_armed_s *armed,			///< current armed status
+			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
 {
 	// Double check that our static arrays are still valid
 	ASSERT(ARMING_STATE_INIT == 0);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (new_arming_state == ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 

src/modules/commander/state_machine_helper.h

@@ -57,7 +57,7 @@ typedef enum {
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -146,6 +146,7 @@ private:
 	int		_range_finder_sub;		/**< range finder subscription */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
+	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
 	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
@@ -414,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* publications */
 	_attitude_sp_pub(-1),
+	_tecs_status_pub(-1),
 	_nav_capabilities_pub(-1),
 
 /* states */
@@ -1384,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 					    climbout_mode, climbout_pitch_min_rad,
 					    throttle_min, throttle_max, throttle_cruise,
 					    pitch_min_rad, pitch_max_rad);
+
+		struct TECS::tecs_state s;
+		_tecs.get_tecs_state(s);
+
+		struct tecs_status_s t;
+
+		t.timestamp = s.timestamp;
+
+		switch (s.mode) {
+			case TECS::ECL_TECS_MODE_NORMAL:
+				t.mode = TECS_MODE_NORMAL;
+				break;
+			case TECS::ECL_TECS_MODE_UNDERSPEED:
+				t.mode = TECS_MODE_UNDERSPEED;
+				break;
+			case TECS::ECL_TECS_MODE_BAD_DESCENT:
+				t.mode = TECS_MODE_BAD_DESCENT;
+				break;
+			case TECS::ECL_TECS_MODE_CLIMBOUT:
+				t.mode = TECS_MODE_CLIMBOUT;
+				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;
+
+		if (_tecs_status_pub > 0) {
+			orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
+		} else {
+			_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
+		}
 	}
 }
 

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
-	_status.altitude = altitude;
-	_status.altitudeFiltered = altitudeFiltered;
+	_status.altitude_filtered = altitudeFiltered;
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
-	_status.airspeed = airspeed;
-	_status.airspeedFiltered = airspeedFiltered;
+	_status.airspeed_filtered = airspeedFiltered;
 
 
 	/* use longitudinal acceleration setpoint for total energy control */

src/modules/sdlog2/sdlog2.c

@@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
 			log_msg.msg_type = LOG_TECS_MSG;
 			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
-			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
-			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
+			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.airspeed = buf.tecs_status.airspeed;
-			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
+			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;

src/modules/sdlog2/sdlog2_messages.h

@@ -333,13 +333,11 @@ struct log_GS1B_s {
 #define LOG_TECS_MSG 30
 struct log_TECS_s {
 	float altitudeSp;
-	float altitude;
 	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
 	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
@@ -455,7 +453,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, "fffffffffffffffB",	"ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */

src/modules/uORB/topics/tecs_status.h

@@ -51,11 +51,13 @@
  */
 
 typedef enum {
-	TECS_MODE_NORMAL,
+	TECS_MODE_NORMAL = 0,
 	TECS_MODE_UNDERSPEED,
 	TECS_MODE_TAKEOFF,
 	TECS_MODE_LAND,
-	TECS_MODE_LAND_THROTTLELIM
+	TECS_MODE_LAND_THROTTLELIM,
+	TECS_MODE_BAD_DESCENT,
+	TECS_MODE_CLIMBOUT
 } tecs_mode;
 
  /**
@@ -65,14 +67,12 @@ struct tecs_status_s {
 	uint64_t timestamp;		/**< timestamp, in microseconds since system start */
 
 	float altitudeSp;
-	float altitude;
-	float altitudeFiltered;
+	float altitude_filtered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
-	float airspeedFiltered;
+	float airspeed_filtered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
 

src/modules/uavcan/uavcan_main.cpp

@@ -253,7 +253,6 @@ int UavcanNode::run()
 	// XXX figure out the output count
 	_output_count = 2;
 
-
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	actuator_outputs_s outputs;
@@ -273,21 +272,23 @@ int UavcanNode::run()
 
 	_node.setStatusOk();
 
-	while (!_task_should_exit) {
+	/*
+	 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
+	 * Please note that with such multiplexing it is no longer possible to rely only on
+	 * the value returned from poll() to detect whether actuator control has timed out or not.
+	 * Instead, all ORB events need to be checked individually (see below).
+	 */
+	_poll_fds_num = 0;
+	_poll_fds[_poll_fds_num] = ::pollfd();
+	_poll_fds[_poll_fds_num].fd = busevent_fd;
+	_poll_fds[_poll_fds_num].events = POLLIN;
+	_poll_fds_num += 1;
 
+	while (!_task_should_exit) {
+		// update actuator controls subscriptions if needed
 		if (_groups_subscribed != _groups_required) {
 			subscribe();
 			_groups_subscribed = _groups_required;
-			/*
-			 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
-			 * Please note that with such multiplexing it is no longer possible to rely only on
-			 * the value returned from poll() to detect whether actuator control has timed out or not.
-			 * Instead, all ORB events need to be checked individually (see below).
-			 */
-			_poll_fds[_poll_fds_num] = ::pollfd();
-			_poll_fds[_poll_fds_num].fd = busevent_fd;
-			_poll_fds[_poll_fds_num].events = POLLIN;
-			_poll_fds_num += 1;
 		}
 
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@@ -301,7 +302,7 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 0;
+			unsigned poll_id = 1;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
@@ -312,12 +313,7 @@ int UavcanNode::run()
 				}
 			}
 
-			if (!controls_updated) {
-				// timeout: no control data, switch to failsafe values
-				// XXX trigger failsafe
-			}
-
-			//can we mix?
+			// can we mix?
 			if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
@@ -417,7 +413,8 @@ UavcanNode::subscribe()
 	// Subscribe/unsubscribe to required actuator control groups
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
-	_poll_fds_num = 0;
+	// the first fd used by CAN
+	_poll_fds_num = 1;
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
@@ -523,8 +520,8 @@ UavcanNode::print_info()
 		warnx("not running, start first");
 	}
 
-	warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
-	warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
+	warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
 }
 
 /*