Merged release_v1.0.0 branch into master

src/modules/commander/calibration_messages.h

@@ -49,7 +49,7 @@
 // instead of visual calibration until such a time as QGC is update to the new version.
 
 // The number in the cal started message is used to indicate the version stamp for the current calibration code.
-#define CAL_QGC_STARTED_MSG			"[cal] calibration started: 1 %s"
+#define CAL_QGC_STARTED_MSG			"[cal] calibration started: 2 %s"
 #define CAL_QGC_DONE_MSG			"[cal] calibration done: %s"
 #define CAL_QGC_FAILED_MSG			"[cal] calibration failed: %s"
 #define CAL_QGC_WARNING_MSG			"[cal] calibration warning: %s"

src/modules/commander/calibration_routines.cpp

@@ -244,7 +244,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 	const float	normal_still_thr = 0.25;		// normal still threshold
 	float		still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
 	float		accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
-	hrt_abstime	still_time = lenient_still_position ? 1000000 : 1500000;	// still time required in us
+	hrt_abstime	still_time = lenient_still_position ? 500000 : 1300000;	// still time required in us
     
 	px4_pollfd_struct_t fds[1];
 	fds[0].fd = accel_sub;
@@ -325,7 +325,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 				/* not still, reset still start time */
 				if (t_still != 0) {
 					mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
-					usleep(500000);
+					usleep(200000);
 					t_still = 0;
 				}
 			}
@@ -489,7 +489,7 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 		// Note that this side is complete
 		side_data_collected[orient] = true;
 		tune_neutral(true);
-		usleep(500000);
+		usleep(200000);
 	}
 	
 	if (sub_accel >= 0) {

src/modules/commander/commander.cpp

@@ -185,8 +185,6 @@ static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
 
-static float takeoff_alt = 5.0f;
-static int parachute_enabled = 0;
 static float eph_threshold = 5.0f;
 static float epv_threshold = 10.0f;
 
@@ -197,6 +195,8 @@ static struct vehicle_control_mode_s control_mode;
 static struct offboard_control_mode_s offboard_control_mode;
 static struct home_position_s _home;
 
+static unsigned _last_mission_instance = 0;
+
 /**
  * The daemon app only briefly exists to start
  * the background job. The stack size assigned in the
@@ -457,7 +457,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 	// For HIL platforms, require that simulated sensors are connected
-	if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
+	if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL &&
+		is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
 		mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
 		return TRANSITION_DENIED;
 	}
@@ -847,7 +848,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
 
 	//Play tune first time we initialize HOME
 	if (!status.condition_home_position_valid) {
-		tune_positive(true);
+		tune_home_set(true);
 	}
 
 	/* mark home position as set */
@@ -866,8 +867,6 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_sys_type = param_find("MAV_TYPE");
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
-	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
 	param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
 	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
 	param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
@@ -1285,11 +1284,7 @@ int commander_thread_main(int argc, char *argv[])
 				rc_calibration_check(mavlink_fd);
 			}
 
-			/* navigation parameters */
-			param_get(_param_takeoff_alt, &takeoff_alt);
-
 			/* Safety parameters */
-			param_get(_param_enable_parachute, &parachute_enabled);
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
 			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
@@ -1778,6 +1773,32 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 
+		/* Only evaluate mission state if home is set,
+		 * this prevents false positives for the mission
+		 * rejection. Back off 2 seconds to not overlay
+		 * home tune.
+		 */
+		if (status.condition_home_position_valid &&
+			(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
+			_last_mission_instance != mission_result.instance_count) {
+			if (!mission_result.valid) {
+				/* the mission is invalid */
+				tune_mission_fail(true);
+				warnx("mission fail");
+			} else if (mission_result.warning) {
+				/* the mission has a warning */
+				tune_mission_fail(true);
+				warnx("mission warning");
+			} else {
+				/* the mission is valid */
+				tune_mission_ok(true);
+				warnx("mission ok");
+			}
+
+			/* prevent further feedback until the mission changes */
+			_last_mission_instance = mission_result.instance_count;
+		}
+
 		/* RC input check */
 		if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
@@ -2510,12 +2531,15 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
+		/* override is not ok for the RTL and recovery mode */
+		control_mode.flag_external_manual_override_ok = false;
+		/* fallthrough */
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;

src/modules/commander/commander_helper.cpp

@@ -174,6 +174,39 @@ void set_tune(int tune)
 	}
 }
 
+void tune_home_set(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_GREEN);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(TONE_HOME_SET);
+	}
+}
+
+void tune_mission_ok(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_GREEN);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
+	}
+}
+
+void tune_mission_fail(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_GREEN);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
+	}
+}
+
 /**
  * Blink green LED and play positive tune (if use_buzzer == true).
  */

src/modules/commander/commander_helper.h

@@ -58,6 +58,9 @@ void buzzer_deinit(void);
 
 void set_tune_override(int tune);
 void set_tune(int tune);
+void tune_home_set(bool use_buzzer);
+void tune_mission_ok(bool use_buzzer);
+void tune_mission_fail(bool use_buzzer);
 void tune_positive(bool use_buzzer);
 void tune_neutral(bool use_buzzer);
 void tune_negative(bool use_buzzer);

src/modules/commander/mag_calibration.cpp

@@ -68,6 +68,8 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "mag";
 static const unsigned max_mags = 3;
+static constexpr float mag_sphere_radius = 0.2f;
+static const unsigned int calibration_sides = 6;
 
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
@@ -79,7 +81,7 @@ typedef struct  {
 	unsigned int	calibration_points_perside;
 	unsigned int	calibration_interval_perside_seconds;
 	uint64_t	calibration_interval_perside_useconds;
-	unsigned int	calibration_counter_total;
+	unsigned int	calibration_counter_total[max_mags];
 	bool		side_data_collected[detect_orientation_side_count];
 	float*		x[max_mags];
 	float*		y[max_mags];
@@ -187,6 +189,24 @@ int do_mag_calibration(int mavlink_fd)
 	return result;
 }
 
+static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
+{
+	float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
+
+	for (size_t i = 0; i < count; i++) {
+		float dx = sx - x[i];
+		float dy = sy - y[i];
+		float dz = sz - z[i];
+		float dist = sqrtf(dx * dx + dy * dy + dz * dz);
+
+		if (dist < min_sample_dist) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
 	calibrate_return result = calibrate_return_ok;
@@ -289,27 +309,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		int poll_ret = px4_poll(fds, fd_count, 1000);
 		
 		if (poll_ret > 0) {
+
+			int prev_count[max_mags];
+			bool rejected = false;
+
 			for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+
+				prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
+
 				if (worker_data->sub_mag[cur_mag] >= 0) {
 					struct mag_report mag;
 
 					orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
+
+					// Check if this measurement is good to go in
+					rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
+						worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
+						worker_data->calibration_counter_total[cur_mag],
+						calibration_sides * worker_data->calibration_points_perside);
 					
-					worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
-					worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
-					worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
-					
+					worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
+					worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
+					worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
+					worker_data->calibration_counter_total[cur_mag]++;
 				}
 			}
-			
-			worker_data->calibration_counter_total++;
-			calibration_counter_side++;
-			
-			// Progress indicator for side
-			mavlink_and_console_log_info(worker_data->mavlink_fd,
-						     "[cal] %s side calibration: progress <%u>",
-						     detect_orientation_str(orientation),
-						     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+
+			// Keep calibration of all mags in lockstep
+			if (rejected) {
+				// Reset counts, since one of the mags rejected the measurement
+				for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+					worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
+				}
+			} else {
+				calibration_counter_side++;
+
+				// Progress indicator for side
+				mavlink_and_console_log_info(worker_data->mavlink_fd,
+							     "[cal] %s side calibration: progress <%u>",
+							     detect_orientation_str(orientation),
+							     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+			}
 		} else {
 			poll_errcount++;
 		}
@@ -339,8 +379,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	
 	worker_data.mavlink_fd = mavlink_fd;
 	worker_data.done_count = 0;
-	worker_data.calibration_counter_total = 0;
-	worker_data.calibration_points_perside = 80;
+	worker_data.calibration_points_perside = 40;
 	worker_data.calibration_interval_perside_seconds = 20;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
 
@@ -348,9 +387,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
 	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
 	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
-	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
-	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
-	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
+	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
+	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
+	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
 	
 	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
 		// Initialize to no subscription
@@ -360,9 +399,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 		worker_data.x[cur_mag] = NULL;
 		worker_data.y[cur_mag] = NULL;
 		worker_data.z[cur_mag] = NULL;
+		worker_data.calibration_counter_total[cur_mag] = 0;
 	}
 
-	const unsigned int calibration_sides = 3;
 	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
 	
 	char str[30];
@@ -441,7 +480,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 				// Mag in this slot is available and we should have values for it to calibrate
 				
 				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
-							 worker_data.calibration_counter_total,
+							 worker_data.calibration_counter_total[cur_mag],
 							 100, 0.0f,
 							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
 							 &sphere_radius[cur_mag]);
@@ -453,6 +492,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 			}
 		}
 	}
+
+	// Print uncalibrated data points
+	if (result == calibrate_return_ok) {
+
+		printf("RAW DATA:\n--------------------\n");
+		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+
+			printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
+
+			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
+				float x = worker_data.x[cur_mag][i];
+				float y = worker_data.y[cur_mag][i];
+				float z = worker_data.z[cur_mag][i];
+				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
+			}
+
+			printf(">>>>>>>\n");
+		}
+
+		printf("CALIBRATED DATA:\n--------------------\n");
+		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+
+			printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
+
+			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
+				float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
+				float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
+				float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
+				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
+			}
+
+			printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
+			printf(">>>>>>>\n");
+		}
+	}
 	
 	// Data points are no longer needed
 	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {

src/modules/commander/state_machine_helper.cpp

@@ -39,7 +39,6 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
-#include <px4_posix.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <stdint.h>
@@ -131,12 +130,12 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
-#ifdef __PX4_NUTTX
 		/*
 		 * Perform an atomic state update
 		 */
+		#ifdef __PX4_NUTTX
 		irqstate_t flags = irqsave();
-#endif
+		#endif
 
 		/* enforce lockdown in HIL */
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
@@ -262,10 +261,10 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			status->arming_state = new_arming_state;
 		}
 
-#ifdef __PX4_NUTTX
 		/* end of atomic state update */
+		#ifdef __PX4_NUTTX
 		irqrestore(flags);
-#endif
+		#endif
 	}
 
 	if (ret == TRANSITION_DENIED) {
@@ -366,125 +365,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	return ret;
 }
 
-#ifdef __PX4_NUTTX
-static transition_result_t disable_publication(const int mavlink_fd)
-{
-	transition_result_t ret;
-
-	/* Disable publication of all attached sensors */
-	/* list directory */
-	DIR		*d;
-	d = opendir("/dev");
-
-	if (d) {
-		struct dirent	*direntry;
-		char devname[24];
-
-		while ((direntry = readdir(d)) != NULL) {
-
-			/* skip serial ports */
-			if (!strncmp("tty", direntry->d_name, 3)) {
-				continue;
-			}
-
-			/* skip mtd devices */
-			if (!strncmp("mtd", direntry->d_name, 3)) {
-				continue;
-			}
-
-			/* skip ram devices */
-			if (!strncmp("ram", direntry->d_name, 3)) {
-				continue;
-			}
-
-			/* skip MMC devices */
-			if (!strncmp("mmc", direntry->d_name, 3)) {
-				continue;
-			}
-
-			/* skip mavlink */
-			if (!strcmp("mavlink", direntry->d_name)) {
-				continue;
-			}
-
-			/* skip console */
-			if (!strcmp("console", direntry->d_name)) {
-				continue;
-			}
-
-			/* skip null */
-			if (!strcmp("null", direntry->d_name)) {
-				continue;
-			}
-
-			snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
-
-			int sensfd = ::open(devname, 0);
-
-			if (sensfd < 0) {
-				warn("failed opening device %s", devname);
-				continue;
-			}
-
-			int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
-			close(sensfd);
-
-			printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
-		}
-		closedir(d);
-		ret = TRANSITION_CHANGED;
-		mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-
-
-	} else {
-		/* failed opening dir */
-		mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
-		ret = TRANSITION_DENIED;
-	}
-	return ret;
-}
-
-#else
-
-static transition_result_t disable_publication(const int mavlink_fd)
-{
-	transition_result_t ret;
-	const char *devname;
-	unsigned int handle = 0;
-	for(;;) {
-		devname = px4_get_device_names(&handle);
-		if (devname == NULL)
-			break;
-
-		/* skip mavlink */
-		if (!strcmp("/dev/mavlink", devname)) {
-			continue;
-		}
-
-
-		int sensfd = px4_open(devname, 0);
-
-		if (sensfd < 0) {
-			warn("failed opening device %s", devname);
-			continue;
-		}
-
-		int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
-		px4_close(sensfd);
-
-			printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
-	}
-	ret = TRANSITION_CHANGED;
-	mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-
-	return ret;
-}
-#endif
-
 /**
  * Transition from one hil state to another
  */
-transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
@@ -495,7 +379,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 		switch (new_state) {
 		case vehicle_status_s::HIL_STATE_OFF:
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
-			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
+			mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
 			ret = TRANSITION_DENIED;
 			break;
 
@@ -503,7 +387,107 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 			if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
 			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
 			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-				ret = disable_publication(mavlink_fd);
+
+#ifdef __PX4_NUTTX
+				/* Disable publication of all attached sensors */
+				/* list directory */
+				DIR		*d;
+				d = opendir("/dev");
+
+				if (d) {
+					struct dirent	*direntry;
+					char devname[24];
+
+					while ((direntry = readdir(d)) != NULL) {
+
+						/* skip serial ports */
+						if (!strncmp("tty", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip mtd devices */
+						if (!strncmp("mtd", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip ram devices */
+						if (!strncmp("ram", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip MMC devices */
+						if (!strncmp("mmc", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip mavlink */
+						if (!strcmp("mavlink", direntry->d_name)) {
+							continue;
+						}
+
+						/* skip console */
+						if (!strcmp("console", direntry->d_name)) {
+							continue;
+						}
+
+						/* skip null */
+						if (!strcmp("null", direntry->d_name)) {
+							continue;
+						}
+
+						snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+						int sensfd = ::open(devname, 0);
+
+						if (sensfd < 0) {
+							warn("failed opening device %s", devname);
+							continue;
+						}
+
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
+						close(sensfd);
+
+						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
+					}
+					closedir(d);
+
+#else
+
+					const char *devname;
+					unsigned int handle = 0;
+					for(;;) {
+						devname = px4_get_device_names(&handle);
+						if (devname == NULL)
+							break;
+
+						/* skip mavlink */
+						if (!strcmp("/dev/mavlink", devname)) {
+							continue;
+						}
+
+
+						int sensfd = px4_open(devname, 0);
+
+						if (sensfd < 0) {
+							warn("failed opening device %s", devname);
+							continue;
+						}
+
+						int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
+						px4_close(sensfd);
+
+							printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
+					}
+#endif
+					ret = TRANSITION_CHANGED;
+					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
+
+				} else {
+					/* failed opening dir */
+					mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
+					ret = TRANSITION_DENIED;
+				}
 			} else {
 				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
 				ret = TRANSITION_DENIED;