Merge pull request #726 from PX4/mavlink2_hil

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");

src/modules/mavlink/mavlink_main.cpp

@@ -200,7 +200,8 @@ Mavlink::Mavlink() :
 	_task_should_exit(false),
 	_mavlink_fd(-1),
 	_task_running(false),
-	_mavlink_hil_enabled(false),
+	_hil_enabled(false),
+	_is_usb_uart(false),
 	_main_loop_delay(1000),
 	_subscriptions(nullptr),
 	_streams(nullptr),
@@ -577,17 +578,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		return -1;
 	}
 
-	/*
-	 * Setup hardware flow control. If the port has no RTS pin this call will fail,
-	 * which is not an issue, but requires a separate call so we can fail silently.
-	 */
-	(void)tcgetattr(_uart_fd, &uart_config);
-	uart_config.c_cflag |= CRTS_IFLOW;
-	(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
-
-	/* setup output flow control */
-	if (enable_flow_control(true)) {
-		warnx("ERR FLOW CTRL EN");
+	if (!_is_usb_uart) {
+		/*
+		 * Setup hardware flow control. If the port has no RTS pin this call will fail,
+		 * which is not an issue, but requires a separate call so we can fail silently.
+		 */
+		(void)tcgetattr(_uart_fd, &uart_config);
+		uart_config.c_cflag |= CRTS_IFLOW;
+		(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+		/* setup output flow control */
+		if (enable_flow_control(true)) {
+			warnx("ERR FLOW CTRL EN");
+		}
 	}
 
 	return _uart_fd;
@@ -596,6 +599,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 int
 Mavlink::enable_flow_control(bool enabled)
 {
+	// We can't do this on USB - skip
+	if (_is_usb_uart) {
+		return OK;
+	}
+
 	struct termios uart_config;
 	int ret = tcgetattr(_uart_fd, &uart_config);
 	if (enabled) {
@@ -617,38 +625,17 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 {
 	int ret = OK;
 
-	/* Enable HIL */
-	if (hil_enabled && !_mavlink_hil_enabled) {
-
-		_mavlink_hil_enabled = true;
-
-		/* ramp up some HIL-related subscriptions */
-		unsigned hil_rate_interval;
-
-		if (_baudrate < 19200) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (_baudrate < 38400) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (_baudrate < 115200) {
-			/* 20 Hz */
-			hil_rate_interval = 50;
-
-		} else {
-			/* 200 Hz */
-			hil_rate_interval = 5;
-		}
-
-//		orb_set_interval(subs.spa_sub, hil_rate_interval);
-//		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
+	/* enable HIL */
+	if (hil_enabled && !_hil_enabled) {
+		_hil_enabled = true;
+		float rate_mult = _datarate / 1000.0f;
+		configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
 	}
 
-	if (!hil_enabled && _mavlink_hil_enabled) {
-		_mavlink_hil_enabled = false;
-//		orb_set_interval(subs.spa_sub, 200);
+	/* disable HIL */
+	if (!hil_enabled && _hil_enabled) {
+		_hil_enabled = false;
+		configure_stream("HIL_CONTROLS", 0.0f);
 
 	} else {
 		ret = ERROR;
@@ -1514,10 +1501,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 	if (i > 1) {
 		/* Enforce null termination */
 		statustext.text[i] = '\0';
-		mavlink_message_t msg;
 
-		mavlink_msg_statustext_encode_chan(mavlink_system.sysid, mavlink_system.compid, _channel, &msg, &statustext);
-		mavlink_missionlib_send_message(&msg);
+		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
 		return OK;
 
 	} else {
@@ -1625,8 +1610,7 @@ Mavlink::task_main(int argc, char *argv[])
 	int ch;
 	_baudrate = 57600;
 	_datarate = 0;
-
-	_mode = MODE_OFFBOARD;
+	_mode = MAVLINK_MODE_NORMAL;
 
 	/* work around some stupidity in task_create's argv handling */
 	argc -= 2;
@@ -1667,17 +1651,8 @@ Mavlink::task_main(int argc, char *argv[])
 //			break;
 
 		case 'm':
-			if (strcmp(optarg, "offboard") == 0) {
-				_mode = MODE_OFFBOARD;
-
-			} else if (strcmp(optarg, "onboard") == 0) {
-				_mode = MODE_ONBOARD;
-
-			} else if (strcmp(optarg, "hil") == 0) {
-				_mode = MODE_HIL;
-
-			} else if (strcmp(optarg, "custom") == 0) {
-				_mode = MODE_CUSTOM;
+			if (strcmp(optarg, "custom") == 0) {
+				_mode = MAVLINK_MODE_CUSTOM;
 			}
 
 			break;
@@ -1713,20 +1688,12 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* inform about mode */
 	switch (_mode) {
-	case MODE_CUSTOM:
-		warnx("mode: CUSTOM");
+	case MAVLINK_MODE_NORMAL:
+		warnx("mode: NORMAL");
 		break;
 
-	case MODE_OFFBOARD:
-		warnx("mode: OFFBOARD");
-		break;
-
-	case MODE_ONBOARD:
-		warnx("mode: ONBOARD");
-		break;
-
-	case MODE_HIL:
-		warnx("mode: HIL");
+	case MAVLINK_MODE_CUSTOM:
+		warnx("mode: CUSTOM");
 		break;
 
 	default:
@@ -1734,21 +1701,7 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	switch (_mode) {
-	case MODE_OFFBOARD:
-	case MODE_HIL:
-	case MODE_CUSTOM:
-		_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
-		break;
-
-	case MODE_ONBOARD:
-		_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
-		break;
-
-	default:
-		_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
-		break;
-	}
+	_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
 	warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
 
@@ -1756,10 +1709,9 @@ Mavlink::task_main(int argc, char *argv[])
 	fflush(stdout);
 
 	struct termios uart_config_original;
-	bool usb_uart;
 
 	/* default values for arguments */
-	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart);
+	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);
 
 	if (_uart_fd < 0) {
 		warn("could not open %s", _device_name);
@@ -1801,7 +1753,7 @@ Mavlink::task_main(int argc, char *argv[])
 	configure_stream("HEARTBEAT", 1.0f);
 
 	switch (_mode) {
-	case MODE_OFFBOARD:
+	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
 		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
@@ -1813,20 +1765,6 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
 		break;
 
-	case MODE_HIL:
-		/* HIL mode normally runs at high data rate, rate_mult ~ 10 */
-		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
-		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
-		configure_stream("ATTITUDE", 2.0f * rate_mult);
-		configure_stream("VFR_HUD", 2.0f * rate_mult);
-		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
-		configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult);
-		configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult);
-		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
-		configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
-		break;
-
 	default:
 		break;
 	}
@@ -1855,12 +1793,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 		if (status_sub->update(t)) {
 			/* switch HIL mode if required */
-			if (status->hil_state == HIL_STATE_ON) {
-				set_hil_enabled(true);
-
-			} else if (status->hil_state == HIL_STATE_OFF) {
-				set_hil_enabled(false);
-			}
+			set_hil_enabled(status->hil_state == HIL_STATE_ON);
 		}
 
 		/* check for requested subscriptions */

src/modules/mavlink/mavlink_main.h

@@ -145,16 +145,14 @@ public:
 	const char *_device_name;
 
 	enum MAVLINK_MODE {
-		MODE_CUSTOM = 0,
-		MODE_OFFBOARD,
-		MODE_ONBOARD,
-		MODE_HIL
+		MAVLINK_MODE_NORMAL = 0,
+		MAVLINK_MODE_CUSTOM
 	};
 
 	void		set_mode(enum MAVLINK_MODE);
 	enum MAVLINK_MODE		get_mode() { return _mode; }
 
-	bool		get_hil_enabled() { return _mavlink_hil_enabled; };
+	bool		get_hil_enabled() { return _hil_enabled; };
 
 	bool		get_flow_control_enabled() { return _flow_control_enabled; }
 
@@ -209,7 +207,8 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	/* states */
-	bool		_mavlink_hil_enabled;		/**< Hardware In the Loop mode */
+	bool		_hil_enabled;		/**< Hardware In the Loop mode */
+	bool		_is_usb_uart;		/**< Port is USB */
 
 	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -108,8 +108,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-
-	_hil_counter(0),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
@@ -647,7 +645,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	}
 
 	/* increment counters */
-	_hil_counter++;
 	_hil_frames++;
 
 	/* print HIL sensors rate */
@@ -854,7 +851,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 		hil_battery_status.timestamp = timestamp;
 		hil_battery_status.voltage_v = 11.1f;
+		hil_battery_status.voltage_filtered_v = 11.1f;
 		hil_battery_status.current_a = 10.0f;
+		hil_battery_status.discharged_mah = -1.0f;
 
 		if (_battery_pub > 0) {
 			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);

src/modules/mavlink/mavlink_receiver.h

@@ -139,7 +139,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	int _hil_counter;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;

src/modules/sensors/sensors.cpp

@@ -1592,8 +1592,7 @@ Sensors::task_main()
 		/* check parameters for updates */
 		parameter_update_poll();
 
-		/* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */
-		raw.timestamp = hrt_absolute_time();
+		/* the timestamp of the raw struct is updated by the gyro_poll() method */
 
 		/* copy most recent sensor data */
 		gyro_poll(raw);