Merged mavlink2_hil

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/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_so3_params so3_comp_params;

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);
+						/* skip serial ports */
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						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

@@ -201,6 +201,7 @@ Mavlink::Mavlink() :
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
+	_is_usb_uart(false),
 	_main_loop_delay(1000),
 	_subscriptions(nullptr),
 	_streams(nullptr),
@@ -577,6 +578,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		return -1;
 	}
 
+	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.
@@ -589,6 +591,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		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) {
@@ -1701,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);

src/modules/mavlink/mavlink_main.h

@@ -208,6 +208,7 @@ private:
 
 	/* states */
 	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_messages.cpp

@@ -216,8 +216,8 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
+		(void)status_sub->update(t);
-		pos_sp_triplet_sub->update(t);
+		(void)pos_sp_triplet_sub->update(t);
 
 		uint8_t mavlink_state = 0;
 		uint8_t mavlink_base_mode = 0;
@@ -261,7 +261,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
+		if (status_sub->update(t)) {
 
 			mavlink_msg_sys_status_send(_channel,
 						    status->onboard_control_sensors_present,
@@ -278,6 +278,7 @@ protected:
 						    status->errors_count3,
 						    status->errors_count4);
 		}
+	}
 };
 
 
@@ -316,7 +317,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		sensor_sub->update(t);
+		if (sensor_sub->update(t)) {
 
 			uint16_t fields_updated = 0;
 
@@ -353,6 +354,7 @@ protected:
 						     sensor->baro_alt_meter, sensor->baro_temp_celcius,
 						     fields_updated);
 		}
+	}
 };
 
 
@@ -382,13 +384,14 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		if (att_sub->update(t)) {
 
 			mavlink_msg_attitude_send(_channel,
 						  att->timestamp / 1000,
 						  att->roll, att->pitch, att->yaw,
 						  att->rollspeed, att->pitchspeed, att->yawspeed);
 		}
+	}
 };
 
 
@@ -418,7 +421,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		if (att_sub->update(t)) {
 
 			mavlink_msg_attitude_quaternion_send(_channel,
 							     att->timestamp / 1000,
@@ -430,6 +433,7 @@ protected:
 							     att->pitchspeed,
 							     att->yawspeed);
 		}
+	}
 };
 
 
@@ -483,11 +487,13 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		bool updated = att_sub->update(t);
-		pos_sub->update(t);
+		updated |= pos_sub->update(t);
-		armed_sub->update(t);
+		updated |= armed_sub->update(t);
-		act_sub->update(t);
+		updated |= act_sub->update(t);
-		airspeed_sub->update(t);
+		updated |= airspeed_sub->update(t);
+
+		if (updated) {
 
 			float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
 			uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
@@ -501,6 +507,7 @@ protected:
 						 pos->alt,
 						 -pos->vel_d);
 		}
+	}
 };
 
 
@@ -530,7 +537,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		gps_sub->update(t);
+		if (gps_sub->update(t)) {
 
 			mavlink_msg_gps_raw_int_send(_channel,
 						     gps->timestamp_position,
@@ -544,6 +551,7 @@ protected:
 						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
 						     gps->satellites_visible);
 		}
+	}
 };
 
 
@@ -579,9 +587,10 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sub->update(t);
+		bool updated = pos_sub->update(t);
-		home_sub->update(t);
+		updated |= home_sub->update(t);
 
+		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
 						     pos->timestamp / 1000,
 						     pos->lat * 1e7,
@@ -593,6 +602,7 @@ protected:
 						     pos->vel_d * 100.0f,
 						     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
+	}
 };
 
 
@@ -622,7 +632,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sub->update(t);
+		if (pos_sub->update(t)) {
 
 			mavlink_msg_local_position_ned_send(_channel,
 							    pos->timestamp / 1000,
@@ -633,6 +643,7 @@ protected:
 							    pos->vy,
 							    pos->vz);
 		}
+	}
 };
 
 
@@ -662,6 +673,10 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
+
+		/* we're sending the GPS home periodically to ensure the
+		 * the GCS does pick it up at one point */
+		if (home_sub->is_published()) {
 			home_sub->update(t);
 
 			mavlink_msg_gps_global_origin_send(_channel,
@@ -669,6 +684,7 @@ protected:
 							   (int32_t)(home->lon * 1e7),
 							   (int32_t)(home->alt) * 1000.0f);
 		}
+	}
 };
 
 
@@ -713,7 +729,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		act_sub->update(t);
+		if (act_sub->update(t)) {
 
 			mavlink_msg_servo_output_raw_send(_channel,
 							  act->timestamp / 1000,
@@ -727,6 +743,7 @@ protected:
 							  act->output[6],
 							  act->output[7]);
 		}
+	}
 };
 
 
@@ -768,9 +785,11 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
+		bool updated = status_sub->update(t);
-		pos_sp_triplet_sub->update(t);
+		updated |= pos_sp_triplet_sub->update(t);
-		act_sub->update(t);
+		updated |= act_sub->update(t);
+
+		if (updated) {
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
@@ -820,6 +839,7 @@ protected:
 						      mavlink_base_mode,
 						      0);
 		}
+	}
 };
 
 
@@ -849,7 +869,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sp_triplet_sub->update(t);
+		if (pos_sp_triplet_sub->update(t)) {
 
 			mavlink_msg_global_position_setpoint_int_send(_channel,
 					MAV_FRAME_GLOBAL,
@@ -858,6 +878,7 @@ protected:
 					(int32_t)(pos_sp_triplet->current.alt * 1000),
 					(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
 		}
+	}
 };
 
 
@@ -925,7 +946,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sp_sub->update(t);
+		if (att_sp_sub->update(t)) {
 
 			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
 					att_sp->timestamp / 1000,
@@ -934,6 +955,7 @@ protected:
 					att_sp->yaw_body,
 					att_sp->thrust);
 		}
+	}
 };
 
 
@@ -963,7 +985,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_rates_sp_sub->update(t);
+		if (att_rates_sp_sub->update(t)) {
 
 			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
 					att_rates_sp->timestamp / 1000,
@@ -972,6 +994,7 @@ protected:
 					att_rates_sp->yaw,
 					att_rates_sp->thrust);
 		}
+	}
 };
 
 
@@ -1001,7 +1024,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		rc_sub->update(t);
+		if (rc_sub->update(t)) {
 
 			const unsigned port_width = 8;
 
@@ -1021,6 +1044,7 @@ protected:
 								 rc->rssi);
 			}
 		}
+	}
 };
 
 
@@ -1050,7 +1074,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		manual_sub->update(t);
+		if (manual_sub->update(t)) {
 
 			mavlink_msg_manual_control_send(_channel,
 							mavlink_system.sysid,
@@ -1060,6 +1084,7 @@ protected:
 							manual->throttle * 1000,
 							0);
 		}
+	}
 };
 
 
@@ -1089,7 +1114,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		flow_sub->update(t);
+		if (flow_sub->update(t)) {
 
 			mavlink_msg_optical_flow_send(_channel,
 						      flow->timestamp,
@@ -1099,6 +1124,7 @@ protected:
 						      flow->quality,
 						      flow->ground_distance_m);
 		}
+	}
 };
 
 

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -46,11 +46,15 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+	_fd(orb_subscribe(_topic)),
+	_published(false),
+	_topic(topic),
+	_last_check(0),
+	next(nullptr)
 {
 	_data = malloc(topic->o_size);
 	memset(_data, 0, topic->o_size);
-	_fd = orb_subscribe(_topic);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
@@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
 
 	return false;
 }
+
+bool
+MavlinkOrbSubscription::is_published()
+{
+	bool updated;
+	orb_check(_fd, &updated);
+
+	if (updated) {
+		_published = true;
+	}
+
+	return _published;
+}

src/modules/mavlink/mavlink_orb_subscription.h

@@ -54,12 +54,21 @@ public:
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
+
+	/**
+	 * Check if the topic has been published.
+	 *
+	 * This call will return true if the topic was ever published.
+	 * @param true if the topic has been published at least once.
+	 */
+	bool is_published();
 	void *get_data();
 	const orb_id_t get_topic();
 
 private:
 	const orb_id_t _topic;
 	int _fd;
+	bool _published;
 	void *_data;
 	hrt_abstime _last_check;
 };