added MAVLink message queueing

this ensures we never block while writing a MAVLink message to a
serial port, by checking the number of available bytes in the serial
transmit buffer and deferring any message that would cause a blocking
write.

This should prevent the main loop from clagging up due to excessive
telemetry data being sent

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3251 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/ArduCopterMega.pde

@@ -601,6 +601,13 @@ static void fast_loop()
 	// ----------
 	read_radio();
 
+    // try to send any deferred messages if the serial port now has
+    // some space available    
+    gcs.send_message(MSG_RETRY_DEFERRED);
+#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
+    hil.send_message(MSG_RETRY_DEFERRED);
+#endif
+
 	// custom code/exceptions for flight modes
 	// ---------------------------------------
 	update_current_flight_mode();

ArduCopterMega/GCS_Mavlink.pde

@@ -88,12 +88,15 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 	if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
 
 		if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
-			send_message(MSG_RAW_IMU);
+			send_message(MSG_RAW_IMU1);
+			send_message(MSG_RAW_IMU2);
+			send_message(MSG_RAW_IMU3);
 			//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
 		}
 
 		if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
-			send_message(MSG_EXTENDED_STATUS);
+			send_message(MSG_EXTENDED_STATUS1);
+			send_message(MSG_EXTENDED_STATUS2);
 			send_message(MSG_GPS_STATUS);
 			send_message(MSG_CURRENT_WAYPOINT);
 			send_message(MSG_GPS_RAW);			// TODO - remove this message after location message is working
@@ -141,7 +144,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 void
 GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
 {
-	mavlink_send_message(chan,id,param,packet_drops);
+	mavlink_send_message(chan,id,packet_drops);
 }
 
 void

ArduCopterMega/HIL_Xplane.pde

@@ -102,7 +102,9 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param)
         break;
     case MSG_RADIO_OUT:
         break;
-    case MSG_RAW_IMU:
+    case MSG_RAW_IMU1:
+    case MSG_RAW_IMU2:
+    case MSG_RAW_IMU3:
         break;
     case MSG_GPS_STATUS:
         break;

ArduCopterMega/Mavlink_Common.h

@@ -25,21 +25,36 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
     }
 }
 
-
-void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
+// try to send a message, return false if it won't fit in the serial tx buffer
+static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
 {
     uint64_t timeStamp = micros();
+    int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
+
+    if (chan == MAVLINK_COMM_1 && millis() < 1000) {
+        // defer any messages on the telemetry port for 1 second after
+        // bootup, to try to prevent bricking of Xbees
+        return false;
+    }
+
 	switch(id) {
 
 		case MSG_HEARTBEAT:
+        {
+            CHECK_PAYLOAD_SIZE(HEARTBEAT);
 			mavlink_msg_heartbeat_send(
 					chan,
 					mavlink_system.type,
 					MAV_AUTOPILOT_ARDUPILOTMEGA);
 			break;
+        }
 
-		case MSG_EXTENDED_STATUS:
+		case MSG_EXTENDED_STATUS1:
 		{
+            CHECK_PAYLOAD_SIZE(SYS_STATUS);
+
 			uint8_t mode 	 = MAV_MODE_UNINIT;
 			uint8_t nav_mode = MAV_NAV_VECTOR;
 
@@ -76,17 +91,21 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					battery_voltage * 1000,
 					battery_remaining,
 					packet_drops);
+            break;
+		}
 
-#ifdef MAVLINK_MSG_ID_MEMINFO
+		case MSG_EXTENDED_STATUS2:
+        {
+            CHECK_PAYLOAD_SIZE(MEMINFO);
             extern unsigned __brkval;
             mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
-#endif
 			break;
 		}
 
 		case MSG_ATTITUDE:
 		{
 			//Vector3f omega = dcm.get_gyro();
+            CHECK_PAYLOAD_SIZE(ATTITUDE);
 			mavlink_msg_attitude_send(
 					chan,
 					timeStamp,
@@ -101,6 +120,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_LOCATION:
 		{
+            CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
 			Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
 			mavlink_msg_global_position_int_send(
 					chan,
@@ -117,6 +137,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 		case MSG_NAV_CONTROLLER_OUTPUT:
         {
             //if (control_mode != MANUAL) {
+                CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
                 mavlink_msg_nav_controller_output_send(
 					chan,
                     nav_roll / 1.0e2,
@@ -133,6 +154,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_LOCAL_LOCATION:
 		{
+            CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
 			Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
 			mavlink_msg_local_position_send(
 					chan,
@@ -148,6 +170,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_GPS_RAW:
 		{
+            CHECK_PAYLOAD_SIZE(GPS_RAW);
 			mavlink_msg_gps_raw_send(
 					chan,
 					timeStamp,
@@ -164,6 +187,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_SERVO_OUT:
 		{
+            CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
 			uint8_t rssi = 1;
 			// normalized values scaled to -10000 to 10000
 			// This is used for HIL.  Do not change without discussing with HIL maintainers
@@ -183,6 +207,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_RADIO_IN:
 		{
+            CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
 			uint8_t rssi = 1;
 			mavlink_msg_rc_channels_raw_send(
 					chan,
@@ -200,6 +225,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_RADIO_OUT:
 		{
+            CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
 			mavlink_msg_servo_output_raw_send(
 					chan,
 					motor_out[0],
@@ -215,6 +241,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_VFR_HUD:
 		{
+            CHECK_PAYLOAD_SIZE(VFR_HUD);
 			mavlink_msg_vfr_hud_send(
 					chan,
 					(float)airspeed / 100.0,
@@ -228,8 +255,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 		}
 
 		#if HIL_MODE != HIL_MODE_ATTITUDE
-		case MSG_RAW_IMU:
+		case MSG_RAW_IMU1:
 		{
+            CHECK_PAYLOAD_SIZE(RAW_IMU);
 			Vector3f accel = imu.get_accel();
 			Vector3f gyro = imu.get_gyro();
 			//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
@@ -246,21 +274,42 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					compass.mag_x,
 					compass.mag_y,
 					compass.mag_z);
+            break;
+        }
 
-			/*  This message is not working.  Why?
+		case MSG_RAW_IMU2:
+		{
+            CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
 			mavlink_msg_scaled_pressure_send(
 					chan,
 					timeStamp,
-					(float)barometer.Press/100.,
-					(float)adc.Ch(AIRSPEED_CH),		// We don't calculate the differential pressure value anywhere, so use raw
-					(int)(barometer.Temp*100));
-			*/
+                    (float)barometer.Press/100.0,
+                    (float)(barometer.Press-ground_pressure)/100.0,
+                    (int)(barometer.Temp*10));
+			break;
+		}
+
+		case MSG_RAW_IMU3:
+		{
+            CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
+            Vector3f mag_offsets = compass.get_offsets();
+
+            mavlink_msg_sensor_offsets_send(chan, 
+                                            mag_offsets.x,
+                                            mag_offsets.y,
+                                            mag_offsets.z,
+                                            compass.get_declination(),
+                                            barometer.RawPress, 
+                                            barometer.RawTemp,
+                                            imu.gx(), imu.gy(), imu.gz(), 
+                                            imu.ax(), imu.ay(), imu.az());
 			break;
 		}
 		#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
 
 		case MSG_GPS_STATUS:
 		{
+            CHECK_PAYLOAD_SIZE(GPS_STATUS);
 			mavlink_msg_gps_status_send(
 					chan,
 					g_gps->num_sats,
@@ -274,6 +323,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 		case MSG_CURRENT_WAYPOINT:
 		{
+            CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
 			mavlink_msg_waypoint_current_send(
 					chan,
 					g.waypoint_index);
@@ -283,10 +333,77 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 		default:
 			break;
 	}
+    return true;
+}
+
+
+#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of
+                                 // switch types in mavlink_try_send_message()
+static struct mavlink_queue {
+    uint8_t deferred_messages[MAX_DEFERRED_MESSAGES];
+    uint8_t next_deferred_message;
+    uint8_t num_deferred_messages;
+} mavlink_queue[2];
+
+// send a message using mavlink
+static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
+{
+    uint8_t i, nextid;
+    struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
+
+    // see if we can send the deferred messages, if any
+    while (q->num_deferred_messages != 0) {
+        if (!mavlink_try_send_message(chan, 
+                                      q->deferred_messages[q->next_deferred_message], 
+                                      packet_drops)) {
+            break;
+        }
+        q->next_deferred_message++;
+        if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
+            q->next_deferred_message = 0;
+        }
+        q->num_deferred_messages--;
+    }
+
+    if (id == MSG_RETRY_DEFERRED) {
+        return;
+    }
+
+    // this message id might already be deferred
+    for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
+        if (q->deferred_messages[nextid] == id) {
+            // its already deferred, discard
+            return;
+        }
+        nextid++;
+        if (nextid == MAX_DEFERRED_MESSAGES) {
+            nextid = 0;
+        }
+    }
+
+    if (q->num_deferred_messages != 0 ||
+        !mavlink_try_send_message(chan, id, packet_drops)) {
+        // can't send it now, so defer it
+        if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
+            // the defer buffer is full, discard
+            return;
+        }
+        nextid = q->next_deferred_message + q->num_deferred_messages;
+        if (nextid >= MAX_DEFERRED_MESSAGES) {
+            nextid -= MAX_DEFERRED_MESSAGES;
+        }
+        q->deferred_messages[nextid] = id;
+        q->num_deferred_messages++;
+    }
 }
 
 void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
 {
+    if (chan == MAVLINK_COMM_0 && millis() < 1000) {
+        // don't send status MAVLink messages for 1 second after
+        // bootup, to try to prevent Xbee bricking
+        return;
+    }
 	mavlink_msg_statustext_send(
 				chan,
 				severity,

ArduCopterMega/defines.h

@@ -196,9 +196,10 @@
 #define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
 #define MSG_VERSION_REQUEST 0x08
 #define MSG_VERSION 0x09
-#define MSG_EXTENDED_STATUS 0x0a
-#define MSG_CPU_LOAD 0x0b
-#define MSG_NAV_CONTROLLER_OUTPUT 0x0c
+#define MSG_EXTENDED_STATUS1 0x0a
+#define MSG_EXTENDED_STATUS2 0x0b
+#define MSG_CPU_LOAD 0x0c
+#define MSG_NAV_CONTROLLER_OUTPUT 0x0d
 
 #define MSG_COMMAND_REQUEST 0x20
 #define MSG_COMMAND_UPLOAD 0x21
@@ -221,9 +222,11 @@
 #define MSG_RADIO_OUT 0x53
 #define MSG_RADIO_IN  0x54
 
-#define MSG_RAW_IMU 0x60
-#define MSG_GPS_STATUS 0x61
-#define MSG_GPS_RAW 0x62
+#define MSG_RAW_IMU1 0x60
+#define MSG_RAW_IMU2 0x61
+#define MSG_RAW_IMU3 0x62
+#define MSG_GPS_STATUS 0x63
+#define MSG_GPS_RAW 0x64
 
 #define MSG_SERVO_OUT 0x70
 
@@ -241,6 +244,7 @@
 #define MSG_POSITION_SET 0xb2
 #define MSG_ATTITUDE_SET 0xb3
 #define MSG_LOCAL_LOCATION 0xb4
+#define MSG_RETRY_DEFERRED 0xff
 
 #define SEVERITY_LOW 1
 #define SEVERITY_MEDIUM 2