mavlink HIL with normal autostart scripts

ROMFS/px4fmu_common/init.d/rc.usb

@@ -5,7 +5,7 @@
 
 echo "Starting MAVLink on this USB console"
 
-mavlink start -r 5000 -d /dev/ttyACM0
+mavlink start -r 10000 -d /dev/ttyACM0
 
 # Exit shell to make it available to MAVLink
 exit 

ROMFS/px4fmu_common/init.d/rcS

@@ -393,7 +393,7 @@ then
 		if [ $HIL == yes ]
 		then
 			sleep 1
-			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil"
+			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
 			usleep 5000
 		else
 			# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s

src/drivers/device/cdev.cpp

@@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname)
 			if (ret == OK) break;
 		} else {
 			char name[32];
-			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
 			ret = register_driver(name, &fops, 0666, (void *)this);
 			if (ret == OK) break;
 		}

src/drivers/gps/gps.cpp

@@ -232,6 +232,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCRESET:
 		cmd_reset();
 		break;
+
+	default:
+		/* give it to parent if no one wants it */
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
 	}
 
 	unlock();

src/drivers/px4io/px4io.cpp

@@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 	battery_status.discharged_mah = _battery_mamphour_total;
 	_battery_last_timestamp = battery_status.timestamp;
 
-	/* lazily publish the battery voltage */
-	if (_to_battery > 0) {
-		orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+	/* the announced battery status would conflict with the simulated battery status in HIL */
+	if (!(_pub_blocked)) {
+		/* lazily publish the battery voltage */
+		if (_to_battery > 0) {
+			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
 
-	} else {
-		_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		} else {
+			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		}
 	}
 }
 
@@ -1959,8 +1962,7 @@ PX4IO::print_status()
 }
 
 int
-PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
-/* Make it obvious that file * isn't used here */
+PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
@@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	default:
-		/* not a recognized value */
-		ret = -ENOTTY;
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filep, cmd, arg);
+		break;
 	}
 
 	return ret;

src/drivers/rgbled/rgbled.cpp

@@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	default:
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filp, cmd, arg);
 		break;
 	}
 

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -393,7 +393,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					hrt_abstime vel_t = 0;
@@ -445,7 +445,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					z_k[6] = raw.magnetometer_ga[0];

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -539,7 +539,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					acc[0] = raw.accelerometer_m_s2[0];
@@ -550,7 +550,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					mag[0] = raw.magnetometer_ga[0];

src/modules/commander/state_machine_helper.cpp

@@ -44,6 +44,7 @@
 #include <stdbool.h>
 #include <dirent.h>
 #include <fcntl.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -309,10 +310,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 	bool valid_transition = false;
 	int ret = ERROR;
 
-	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
-
 	if (current_status->hil_state == new_state) {
-		warnx("Hil state not changed");
 		valid_transition = true;
 
 	} else {
@@ -340,23 +338,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 				/* list directory */
 				DIR		*d;
-				struct dirent	*direntry;
 				d = opendir("/dev");
 				if (d) {
 
+					struct dirent	*direntry;
+					char devname[24];
+
 					while ((direntry = readdir(d)) != NULL) {
 
-						int sensfd = ::open(direntry->d_name, 0);
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						/* 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\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
 
 					closedir(d);
 
-					warnx("directory listing ok (FS mounted and readable)");
-
 				} else {
 					/* failed opening dir */
 					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");