diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde index 80b6138b95..d6b3b109ad 100644 --- a/Tools/AntennaTracker/AntennaTracker.pde +++ b/Tools/AntennaTracker/AntennaTracker.pde @@ -183,8 +183,8 @@ static RC_Channel channel_pitch(CH_2); //////////////////////////////////////////////////////////////////////////////// // GCS selection //////////////////////////////////////////////////////////////////////////////// -static GCS_MAVLINK gcs0; -static GCS_MAVLINK gcs3; +static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; +static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde index baa9e5540b..f7b76f3cc2 100644 --- a/Tools/AntennaTracker/GCS_Mavlink.pde +++ b/Tools/AntennaTracker/GCS_Mavlink.pde @@ -212,15 +212,12 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) static void NOINLINE send_waypoint_request(mavlink_channel_t chan) { - if (chan == MAVLINK_COMM_0) - gcs0.queued_waypoint_send(); - else - gcs3.queued_waypoint_send(); + gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); } 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, @@ -294,11 +291,7 @@ 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) { - gcs0.queued_param_send(); - } else if (gcs3.initialised) { - gcs3.queued_param_send(); - } + gcs[chan-MAVLINK_COMM_0].queued_param_send(); break; case MSG_NEXT_WAYPOINT: @@ -403,7 +396,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); @@ -1042,7 +1035,7 @@ mission_failed: 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; @@ -1070,9 +1063,10 @@ static void mavlink_delay_cb() */ static void gcs_send_message(enum ap_message id) { - gcs0.send_message(id); - if (gcs3.initialised) { - gcs3.send_message(id); + for (uint8_t i=0; i<num_gcs; i++) { + if (gcs[i].initialised) { + gcs[i].send_message(id); + } } } @@ -1081,9 +1075,10 @@ static void gcs_send_message(enum ap_message id) */ static void gcs_data_stream_send(void) { - gcs0.data_stream_send(); - if (gcs3.initialised) { - gcs3.data_stream_send(); + for (uint8_t i=0; i<num_gcs; i++) { + if (gcs[i].initialised) { + gcs[i].data_stream_send(); + } } } @@ -1092,17 +1087,19 @@ static void gcs_data_stream_send(void) */ static void gcs_update(void) { - gcs0.update(); - if (gcs3.initialised) { - gcs3.update(); + for (uint8_t i=0; i<num_gcs; i++) { + if (gcs[i].initialised) { + gcs[i].update(); + } } } static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) { - gcs0.send_text_P(severity, str); - if (gcs3.initialised) { - gcs3.send_text_P(severity, str); + for (uint8_t i=0; i<num_gcs; i++) { + if (gcs[i].initialised) { + gcs[i].send_text_P(severity, str); + } } #if LOGGING_ENABLED == ENABLED DataFlash.Log_Write_Message_P(str); @@ -1117,18 +1114,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, - sizeof(gcs0.pending_status.text), fmt, arg_list); + hal.util->vsnprintf_P((char *)gcs[0].pending_status.text, + sizeof(gcs[0].pending_status.text), fmt, arg_list); va_end(arg_list); #if LOGGING_ENABLED == ENABLED - DataFlash.Log_Write_Message(gcs0.pending_status.text); + DataFlash.Log_Write_Message(gcs[0].pending_status.text); #endif - gcs3.pending_status = gcs0.pending_status; mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); - if (gcs3.initialised) { - mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + for (uint8_t i=1; i<num_gcs; i++) { + if (gcs[i].initialised) { + gcs[i].pending_status = gcs[0].pending_status; + mavlink_send_message((mavlink_channel_t)i, MSG_STATUSTEXT, 0); + } } } diff --git a/Tools/AntennaTracker/Parameters.h b/Tools/AntennaTracker/Parameters.h index 2597bb8b9a..a3f7e9e94e 100644 --- a/Tools/AntennaTracker/Parameters.h +++ b/Tools/AntennaTracker/Parameters.h @@ -51,12 +51,12 @@ public: k_param_format_version = 0, k_param_software_type, - k_param_gcs0 = 100, // stream rates for port0 - k_param_gcs3, // stream rates for port3 + k_param_gcs0 = 100, // stream rates for uartA + 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_serial1_baud, k_param_imu, k_param_compass_enabled, k_param_compass, @@ -67,6 +67,8 @@ public: k_param_sitl, k_param_pidPitch2Srv, k_param_pidYaw2Srv, + k_param_gcs2, // stream rates for uartD + k_param_serial2_baud, k_param_channel_yaw = 200, k_param_channel_pitch, @@ -87,7 +89,10 @@ public: AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; AP_Int8 serial0_baud; - AP_Int8 serial3_baud; + AP_Int8 serial1_baud; +#if MAVLINK_COMM_NUM_BUFFERS > 2 + AP_Int8 serial2_baud; +#endif AP_Int8 compass_enabled; diff --git a/Tools/AntennaTracker/Parameters.pde b/Tools/AntennaTracker/Parameters.pde index 6af1ce39fc..d235ad6e37 100644 --- a/Tools/AntennaTracker/Parameters.pde +++ b/Tools/AntennaTracker/Parameters.pde @@ -9,6 +9,7 @@ #define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.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", 0), @@ -30,19 +31,26 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: SERIAL0_BAUD // @DisplayName: USB Console Baud Rate - // @Description: The baud rate used on the main uart + // @Description: The baud rate used on the USB console // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 // @User: Standard GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000), - // @Param: SERIAL3_BAUD + // @Param: SERIAL1_BAUD // @DisplayName: Telemetry Baud Rate - // @Description: The baud rate used on the telemetry port + // @Description: The baud rate used on the first telemetry port // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 // @User: Standard - GSCALAR(serial3_baud, "SERIAL3_BAUD", SERIAL3_BAUD/1000), - + GSCALAR(serial1_baud, "SERIAL1_BAUD", SERIAL1_BAUD/1000), +#if MAVLINK_COMM_NUM_BUFFERS > 2 + // @Param: SERIAL2_BAUD + // @DisplayName: Telemetry Baud Rate + // @Description: The baud rate used on the second telemetry port + // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 + // @User: Standard + GSCALAR(serial2_baud, "SERIAL2_BAUD", SERIAL2_BAUD/1000), +#endif // @Param: MAG_ENABLE // @DisplayName: Enable Compass @@ -67,11 +75,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: INS_ // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp diff --git a/Tools/AntennaTracker/config.h b/Tools/AntennaTracker/config.h index 48ed1b8e46..f4dca8f41a 100644 --- a/Tools/AntennaTracker/config.h +++ b/Tools/AntennaTracker/config.h @@ -69,15 +69,21 @@ #ifndef SERIAL0_BAUD # define SERIAL0_BAUD 115200 #endif -#ifndef SERIAL3_BAUD - # define SERIAL3_BAUD 57600 +#ifndef SERIAL1_BAUD + # define SERIAL1_BAUD 57600 +#endif +#ifndef SERIAL2_BAUD + # define SERIAL2_BAUD 57600 #endif - #ifndef SERIAL_BUFSIZE # define SERIAL_BUFSIZE 512 #endif +#ifndef SERIAL1_BUFSIZE + # define SERIAL1_BUFSIZE 256 +#endif + #ifndef SERIAL2_BUFSIZE # define SERIAL2_BUFSIZE 256 #endif diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde index ab5c042fc0..fe659beb53 100644 --- a/Tools/AntennaTracker/system.pde +++ b/Tools/AntennaTracker/system.pde @@ -21,15 +21,16 @@ static void init_tracker() barometer.init(); // 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 hal.scheduler->register_delay_callback(mavlink_delay_cb, 5); // we have a 2nd serial port for telemetry - hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), - 128, SERIAL2_BUFSIZE); - gcs3.init(hal.uartC); + hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD), + 128, SERIAL1_BUFSIZE); + gcs[1].init(hal.uartC); mavlink_system.sysid = g.sysid_this_mav; @@ -117,7 +118,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) case 111: return 111100; case 115: return 115200; } - cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD")); + cliSerial->println_P(PSTR("Invalid baudrate")); return default_baud; }