Rover: added support for uartD

APMrover2/APMrover2.pde

@@ -270,8 +270,8 @@ SITL sitl;
 // GCS selection
 ////////////////////////////////////////////////////////////////////////////////
 //
-GCS_MAVLINK	gcs0;
+static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
-GCS_MAVLINK	gcs3;
+static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
 
 // a pin for reading the receiver RSSI voltage. The scaling by 0.25 
 // is to take the 0 to 1024 range down to an 8 bit range for MAVLink

APMrover2/GCS.h

@@ -41,7 +41,6 @@ public:
 	///
 	void		init(AP_HAL::UARTDriver *port) {
         _port = port;
-        initialised = true;
     }
 
 	/// Update GCS state.

APMrover2/GCS_Mavlink.pde

@@ -452,7 +452,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 
 static void NOINLINE send_statustext(mavlink_channel_t chan)
 {
-    mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
+    mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
     mavlink_msg_statustext_send(
         chan,
         s->severity,
@@ -575,20 +575,12 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
 
     case MSG_NEXT_PARAM:
         CHECK_PAYLOAD_SIZE(PARAM_VALUE);
-        if (chan == MAVLINK_COMM_0) {
+        gcs[chan-MAVLINK_COMM_0].queued_param_send();
-            gcs0.queued_param_send();
-        } else if (gcs3.initialised) {
-            gcs3.queued_param_send();
-        }
         break;
 
     case MSG_NEXT_WAYPOINT:
         CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
-        if (chan == MAVLINK_COMM_0) {
+        gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
-            gcs0.queued_waypoint_send();
-        } else if (gcs3.initialised) {
-            gcs3.queued_waypoint_send();
-        }
         break;
 
     case MSG_STATUSTEXT:
@@ -628,7 +620,7 @@ static struct mavlink_queue {
     enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
     uint8_t next_deferred_message;
     uint8_t num_deferred_messages;
-} mavlink_queue[2];
+} mavlink_queue[MAVLINK_COMM_NUM_BUFFERS];
 
 // send a message using mavlink
 static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
@@ -690,7 +682,7 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
 
     if (severity == SEVERITY_LOW) {
         // send via the deferred queuing system
-        mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
+        mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
         s->severity = (uint8_t)severity;
         strncpy((char *)s->text, str, sizeof(s->text));
         mavlink_send_message(chan, MSG_STATUSTEXT, 0);
@@ -803,9 +795,17 @@ GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
     if (port == (AP_HAL::BetterStream*)hal.uartA) {
         mavlink_comm_0_port = port;
         chan = MAVLINK_COMM_0;
-    }else{
+        initialised = true;
+    } else if (port == (AP_HAL::BetterStream*)hal.uartC) {
         mavlink_comm_1_port = port;
         chan = MAVLINK_COMM_1;
+        initialised = true;
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    } else if (port == (AP_HAL::BetterStream*)hal.uartD) {
+        mavlink_comm_2_port = port;
+        chan = MAVLINK_COMM_2;
+        initialised = true;
+#endif
     }
     _queued_parameter = NULL;
     reset_cli_timeout();
@@ -1691,7 +1691,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 } else if (var_type == AP_PARAM_INT32) {
                     if (packet.param_value < 0) rounding_addition = -rounding_addition;
                     float v = packet.param_value+rounding_addition;
-                    v = constrain_float(v, -2147483648.0f, 2147483647.0f);
+                v = constrain_float(v, -2147483648.0, 2147483647.0);
 					((AP_Int32 *)vp)->set_and_save(v);
                 } else if (var_type == AP_PARAM_INT16) {
                     if (packet.param_value < 0) rounding_addition = -rounding_addition;
@@ -1857,14 +1857,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     default:
         // forward unknown messages to the other link if there is one
-        if ((chan == MAVLINK_COMM_1 && gcs0.initialised) ||
+        for (uint8_t i=0; i<num_gcs; i++) {
-            (chan == MAVLINK_COMM_0 && gcs3.initialised)) {
+            if (gcs[i].initialised && i != (uint8_t)chan) {
-            mavlink_channel_t out_chan = (mavlink_channel_t)(((uint8_t)chan)^1);
+                mavlink_channel_t out_chan = (mavlink_channel_t)i;
-            // only forward if it would fit in our transmit buffer
+                // only forward if it would fit in the transmit buffer
             if (comm_get_txspace(out_chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
                 _mavlink_resend_uart(out_chan, msg);
             }
         }
+        }
         break;
 
     } // end switch
@@ -1893,7 +1894,7 @@ GCS_MAVLINK::_count_parameters()
 void
 GCS_MAVLINK::queued_param_send()
 {
-    if (_queued_parameter == NULL) {
+    if (!initialised || _queued_parameter == NULL) {
         return;
     }
 
@@ -1943,7 +1944,8 @@ GCS_MAVLINK::queued_param_send()
 void
 GCS_MAVLINK::queued_waypoint_send()
 {
-    if (waypoint_receiving &&
+    if (initialised &&
+        waypoint_receiving &&
         waypoint_request_i <= waypoint_request_last) {
         mavlink_msg_mission_request_send(
             chan,
@@ -1965,7 +1967,7 @@ void GCS_MAVLINK::reset_cli_timeout() {
 static void mavlink_delay_cb()
 {
     static uint32_t last_1hz, last_50hz, last_5s;
-    if (!gcs0.initialised) return;
+    if (!gcs[0].initialised) return;
 
     in_mavlink_delay = true;
 
@@ -1995,9 +1997,10 @@ static void mavlink_delay_cb()
  */
 static void gcs_send_message(enum ap_message id)
 {
-    gcs0.send_message(id);
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.send_message(id);
+            gcs[i].send_message(id);
+        }
     }
 }
 
@@ -2006,9 +2009,10 @@ static void gcs_send_message(enum ap_message id)
  */
 static void gcs_data_stream_send(void)
 {
-    gcs0.data_stream_send();
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.data_stream_send();
+            gcs[i].data_stream_send();
+        }
     }
 }
 
@@ -2017,19 +2021,23 @@ static void gcs_data_stream_send(void)
  */
 static void gcs_update(void)
 {
-	gcs0.update();
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.update();
+            gcs[i].update();
+        }
     }
 }
 
 static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
 {
-    gcs0.send_text_P(severity, str);
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.send_text_P(severity, str);
+            gcs[i].send_text_P(severity, str);
     }
+    }
+#if LOGGING_ENABLED == ENABLED
     DataFlash.Log_Write_Message_P(str);
+#endif
 }
 
 /*
@@ -2040,16 +2048,20 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
 void gcs_send_text_fmt(const prog_char_t *fmt, ...)
 {
     va_list arg_list;
-    gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW;
+    gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW;
     va_start(arg_list, fmt);
-    hal.util->vsnprintf_P((char *)gcs0.pending_status.text,
+    hal.util->vsnprintf_P((char *)gcs[0].pending_status.text,
-            sizeof(gcs0.pending_status.text), fmt, arg_list);
+            sizeof(gcs[0].pending_status.text), fmt, arg_list);
     va_end(arg_list);
-    DataFlash.Log_Write_Message(gcs0.pending_status.text);
+#if LOGGING_ENABLED == ENABLED
-    gcs3.pending_status = gcs0.pending_status;
+    DataFlash.Log_Write_Message(gcs[0].pending_status.text);
+#endif
     mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
-    if (gcs3.initialised) {
+    for (uint8_t i=1; i<num_gcs; i++) {
-        mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
+        if (gcs[i].initialised) {
+            gcs[i].pending_status = gcs[0].pending_status;
+            mavlink_send_message((mavlink_channel_t)i, MSG_STATUSTEXT, 0);
+        }
     }
 }
 

APMrover2/Parameters.h

@@ -43,14 +43,15 @@ public:
 
         // 110: Telemetry control
         //
-        k_param_gcs0 = 110, // stream rates for port0
+        k_param_gcs0 = 110, // stream rates for uartA
-        k_param_gcs3,       // stream rates for port3
+        k_param_gcs1,       // stream rates for uartC
         k_param_sysid_this_mav,
         k_param_sysid_my_gcs,
         k_param_serial0_baud,
         k_param_serial3_baud,
         k_param_telem_delay,
         k_param_skip_gyro_cal,
+        k_param_gcs2,       // stream rates for uartD
 
         //
         // 130: Sensor parameters

APMrover2/Parameters.pde

@@ -7,6 +7,7 @@
 #define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value:def} }
 #define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info:class::var_info} }
 #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info:class::var_info} }
+#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
 
 const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(format_version,         "FORMAT_VERSION",   1),
@@ -416,11 +417,17 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Group: SR0_
     // @Path: GCS_Mavlink.pde
-	GOBJECT(gcs0,					"SR0_",     GCS_MAVLINK),
+    GOBJECTN(gcs[0], gcs0,        "SR0_",     GCS_MAVLINK),
 
-    // @Group: SR3_
+    // @Group: SR1_
     // @Path: GCS_Mavlink.pde
-	GOBJECT(gcs3,					"SR3_",     GCS_MAVLINK),
+    GOBJECTN(gcs[1],  gcs1,       "SR1_",     GCS_MAVLINK),
+
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    // @Group: SR2_
+    // @Path: GCS_Mavlink.pde
+    GOBJECTN(gcs[2],  gcs2,       "SR2_",     GCS_MAVLINK),
+#endif
 
     // @Group: NAVL1_
     // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp

APMrover2/system.pde

@@ -114,7 +114,7 @@ static void init_ardupilot()
     g.num_resets.set_and_save(g.num_resets+1);
 
 	// init the GCS
-	gcs0.init(hal.uartA);
+	gcs[0].init(hal.uartA);
 
     // Register mavlink_delay_cb, which will run anytime you have
     // more than 5ms remaining in your call to hal.scheduler->delay
@@ -127,7 +127,13 @@ static void init_ardupilot()
 
     // we have a 2nd serial port for telemetry
     hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
-	gcs3.init(hal.uartC);
+	gcs[1].init(hal.uartC);
+
+    // we may have a 3rd serial port for telemetry
+    if (hal.uartD != NULL) {
+        hal.uartD->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+        gcs[2].init(hal.uartD);
+    }
 
 	mavlink_system.sysid = g.sysid_this_mav;
 
@@ -192,9 +198,12 @@ static void init_ardupilot()
 	//
     const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
     cliSerial->println_P(msg);
-    if (gcs3.initialised) {
+    if (gcs[1].initialised) {
         hal.uartC->println_P(msg);
     }
+    if (num_gcs > 2 && gcs[2].initialised) {
+        hal.uartD->println_P(msg);
+    }
 
 	startup_ground();