Fix #29. Autostart mavlink via USB on PX4

Changes mavconn interface, adds new parameter.
mavlink · Jun 11, 2014 · dd2b9a3 · dd2b9a3
1 parent b45d13f
commit dd2b9a3
Show file tree

Hide file tree

Showing 7 changed files with 50 additions and 23 deletions.
diff --git a/README.md b/README.md
@@ -38,6 +38,7 @@ General node parametars:
   * ~/component\_id -- Node MAVLink Component ID (default: 240 == UDP\_BRIDGE)
   * ~/target\_system\_id -- FCU System ID (default: 1)
   * ~/target\_component\_id -- FCU Component ID (default: 1)
+  * ~/startup\_px4\_usb\_quirk -- autostart mavlink via USB on PX4
 
 Plugins parameters:
 

diff --git a/include/mavros/mavconn_interface.h b/include/mavros/mavconn_interface.h
@@ -49,6 +49,7 @@ class MAVConnInterface {
 		send_message(message, sys_id, comp_id);
 	};
 	virtual void send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid) = 0;
+	virtual void send_bytes(const uint8_t *bytes, size_t length) = 0;
 
 	sig2::signal<void(const mavlink_message_t *message, uint8_t system_id, uint8_t component_id)> message_received;
 	sig2::signal<void()> port_closed;

diff --git a/src/mavconn/mavconn_serial.cpp b/src/mavconn/mavconn_serial.cpp
@@ -56,6 +56,15 @@ MAVConnSerial::~MAVConnSerial()
 	serial_dev.close();
 }
 
+void MAVConnSerial::send_bytes(const uint8_t *bytes, size_t length)
+{
+	{
+		boost::recursive_mutex::scoped_lock lock(mutex);
+		tx_q.insert(tx_q.end(), bytes, bytes + length);
+	}
+	io_service.post(boost::bind(&MAVConnSerial::do_write, this));
+}
+
 void MAVConnSerial::send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid)
 {
 	ROS_ASSERT(message != NULL);
@@ -70,12 +79,7 @@ void MAVConnSerial::send_message(const mavlink_message_t *message, uint8_t sysid
 	size_t length = mavlink_msg_to_send_buffer(buffer, &msg);
 
 	ROS_DEBUG_NAMED("mavconn", "serial::send_message: Message-ID: %d [%zu bytes]", message->msgid, length);
-
-	{
-		boost::recursive_mutex::scoped_lock lock(mutex);
-		tx_q.insert(tx_q.end(), buffer, buffer + length);
-	}
-	io_service.post(boost::bind(&MAVConnSerial::do_write, this));
+	send_bytes(buffer, length);
 }
 
 void MAVConnSerial::do_read(void)

diff --git a/src/mavconn/mavconn_serial.h b/src/mavconn/mavconn_serial.h
@@ -36,6 +36,7 @@ class MAVConnSerial : public MAVConnInterface {
 
 	using MAVConnInterface::send_message;
 	void send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid);
+	void send_bytes(const uint8_t *bytes, size_t length);
 
 	inline mavlink_status_t get_status() { return *mavlink_get_channel_status(channel); };
 	inline bool is_open() { return serial_dev.is_open(); };

diff --git a/src/mavconn/mavconn_udp.cpp b/src/mavconn/mavconn_udp.cpp
@@ -81,6 +81,15 @@ MAVConnUDP::~MAVConnUDP()
 	io_service.stop();
 }
 
+void MAVConnUDP::send_bytes(const uint8_t *bytes, size_t length)
+{
+	{
+		boost::recursive_mutex::scoped_lock lock(mutex);
+		tx_q.insert(tx_q.end(), bytes, bytes + length);
+	}
+	io_service.post(boost::bind(&MAVConnUDP::do_write, this));
+}
+
 void MAVConnUDP::send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid)
 {
 	ROS_ASSERT(message != NULL);
@@ -96,12 +105,7 @@ void MAVConnUDP::send_message(const mavlink_message_t *message, uint8_t sysid, u
 
 	ROS_DEBUG_NAMED("mavconn", "udp::send_message: Message-ID: %d [%zu bytes] Sys-Id: %d Comp-Id: %d",
 			message->msgid, length, sysid, compid);
-
-	{
-		boost::recursive_mutex::scoped_lock lock(mutex);
-		tx_q.insert(tx_q.end(), buffer, buffer + length);
-	}
-	io_service.post(boost::bind(&MAVConnUDP::do_write, this));
+	send_bytes(buffer, length);
 }
 
 void MAVConnUDP::do_read(void)

diff --git a/src/mavconn/mavconn_udp.h b/src/mavconn/mavconn_udp.h
@@ -37,6 +37,7 @@ class MAVConnUDP : public MAVConnInterface {
 
 	using MAVConnInterface::send_message;
 	void send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid);
+	void send_bytes(const uint8_t *bytes, size_t length);
 
 	inline mavlink_status_t get_status() { return *mavlink_get_channel_status(channel); };
 	inline bool is_open() { return socket.is_open(); };

diff --git a/src/mavros_node.cpp b/src/mavros_node.cpp
@@ -70,12 +70,11 @@ class MavlinkDiag : public diagnostic_updater::DiagnosticTask
 		} else {
 			stat.summary(2, "not connected");
 		}
-	};
+	}
 
-	void set_mavconn(const boost::shared_ptr<MAVConnInterface> &link)
-	{
+	void set_mavconn(const boost::shared_ptr<MAVConnInterface> &link) {
 		weak_link = link;
-	};
+	}
 
 private:
 	boost::weak_ptr<MAVConnInterface> weak_link;
@@ -102,6 +101,7 @@ class MavRos
 		int gcs_port;
 		int system_id, component_id;
 		int tgt_system_id, tgt_component_id;
+		bool px4_usb_quirk;
 
 		node_handle.param<std::string>("serial_port", serial_port, "/dev/ttyACM0");
 		node_handle.param("serial_baud", serial_baud, 57600);
@@ -113,6 +113,7 @@ class MavRos
 		node_handle.param<int>("component_id", component_id, MAV_COMP_ID_UDP_BRIDGE);
 		node_handle.param("target_system_id", tgt_system_id, 1);
 		node_handle.param("target_component_id", tgt_component_id, 1);
+		node_handle.param("startup_px4_usb_quirk", px4_usb_quirk, false);
 
 		diag_updater.setHardwareID("Mavlink");
 		diag_updater.add(serial_link_diag);
@@ -145,8 +146,11 @@ class MavRos
 				++it)
 			add_plugin(*it);
 
+		if (px4_usb_quirk)
+			startup_px4_usb_quirk();
+
 		ROS_INFO("MAVROS started on MAV %d (component %d)", system_id, component_id);
-	};
+	}
 
 	~MavRos() {};
 
@@ -160,7 +164,7 @@ class MavRos
 		}
 
 		mav_uas.stop();
-	};
+	}
 
 private:
 	ros::NodeHandle node_handle;
@@ -197,7 +201,7 @@ class MavRos
 			rmsg->payload64.push_back(mmsg->payload64[i]);
 
 		mavlink_pub.publish(rmsg);
-	};
+	}
 
 	void mavlink_sub_cb(const Mavlink::ConstPtr &rmsg) {
 		mavlink_message_t mmsg;
@@ -207,11 +211,11 @@ class MavRos
 		copy(rmsg->payload64.begin(), rmsg->payload64.end(), mmsg.payload64); // TODO: add paranoic checks
 
 		serial_link->send_message(&mmsg, rmsg->sysid, rmsg->compid);
-	};
+	}
 
 	void plugin_route_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
 		message_route_table[mmsg->msgid](mmsg, sysid, compid);
-	};
+	}
 
 	void add_plugin(std::string pl_name) {
 		boost::shared_ptr<mavplugin::MavRosPlugin> plugin;
@@ -237,12 +241,23 @@ class MavRos
 		} catch (pluginlib::PluginlibException& ex) {
 			ROS_ERROR_STREAM("Plugin load exception: " << ex.what());
 		}
-	};
+	}
 
 	void terminate_cb() {
 		ROS_ERROR("Serial port closed. mavros will be terminated.");
 		ros::requestShutdown();
-	};
+	}
+
+	void startup_px4_usb_quirk(void) {
+		/* sample code from QGC */
+		const uint8_t init[] = {0x0d, 0x0d, 0x0d, 0};
+		const uint8_t nsh[] = "sh /etc/init.d/rc.usb\n";
+
+		ROS_INFO("Autostarting mavlink via USB on PX4");
+		serial_link->send_bytes(init, 3);
+		serial_link->send_bytes(nsh, sizeof(nsh) - 1);
+		serial_link->send_bytes(init, 4);	/* NOTE in original init[3] */
+	}
 };