From 2fa5e6310d0865be0ed6061c49f33cc91ef65b85 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 Nov 2013 19:18:19 +1100 Subject: [PATCH] Plane: added 3rd telemetry stream on platforms that have it this generalises the gcs object usage --- ArduPlane/ArduPlane.pde | 4 +- ArduPlane/GCS.h | 1 - ArduPlane/GCS_Mavlink.pde | 100 ++++++++++++++++++++------------------ ArduPlane/Parameters.h | 5 +- ArduPlane/Parameters.pde | 13 +++-- ArduPlane/system.pde | 19 ++++++-- 6 files changed, 82 insertions(+), 60 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 5598a70c3d..b4bbf9878b 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -289,8 +289,8 @@ static bool guided_throttle_passthru; //////////////////////////////////////////////////////////////////////////////// // 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]; // selected navigation controller static AP_Navigation *nav_controller = &L1_controller; diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 6b7f061547..3b88a02d95 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -41,7 +41,6 @@ public: /// void init(AP_HAL::UARTDriver *port) { _port = port; - initialised = true; } /// Update GCS state. diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index ff3ebf6e4c..e9f6589158 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -542,7 +542,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, @@ -670,20 +670,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) { - 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: CHECK_PAYLOAD_SIZE(MISSION_REQUEST); - if (chan == MAVLINK_COMM_0) { - gcs0.queued_waypoint_send(); - } else if (gcs3.initialised) { - gcs3.queued_waypoint_send(); - } + gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); break; case MSG_STATUSTEXT: @@ -792,7 +784,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); @@ -905,9 +897,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(); @@ -2137,12 +2137,13 @@ mission_failed: default: // forward unknown messages to the other link if there is one - if ((chan == MAVLINK_COMM_1 && gcs0.initialised) || - (chan == MAVLINK_COMM_0 && gcs3.initialised)) { - mavlink_channel_t out_chan = (mavlink_channel_t)(((uint8_t)chan)^1); - // only forward if it would fit in our transmit buffer - if (comm_get_txspace(out_chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) { - _mavlink_resend_uart(out_chan, msg); + for (uint8_t i=0; i ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) { + _mavlink_resend_uart(out_chan, msg); + } } } break; @@ -2173,7 +2174,7 @@ GCS_MAVLINK::_count_parameters() void GCS_MAVLINK::queued_param_send() { - if (_queued_parameter == NULL) { + if (!initialised || _queued_parameter == NULL) { return; } @@ -2223,7 +2224,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, @@ -2245,7 +2247,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; @@ -2275,9 +2277,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; ivsnprintf_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= - MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN) { - airspeed.log_mavlink_send(MAVLINK_COMM_0, vg); - } - if (gcs3.initialised) { - if (comm_get_txspace(MAVLINK_COMM_1) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= + for (uint8_t i=0; i= MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN) { - airspeed.log_mavlink_send(MAVLINK_COMM_1, vg); + airspeed.log_mavlink_send((mavlink_channel_t)i, vg); } } } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 97be09f15c..c795e49cb5 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -101,13 +101,14 @@ public: // 110: Telemetry control // - k_param_gcs0 = 110, // stream rates for port0 - k_param_gcs3, // stream rates for port3 + k_param_gcs0 = 110, // stream rates for uartA + k_param_gcs1, // stream rates for uartC k_param_sysid_this_mav, k_param_sysid_my_gcs, k_param_serial3_baud, k_param_telem_delay, k_param_serial0_baud, + k_param_gcs2, // stream rates for uartD // 120: Fly-by-wire control // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 4830ab09fd..3dcea3498b 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/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), @@ -854,11 +855,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/ArduPlane/system.pde b/ArduPlane/system.pde index 25332ef24e..84df734854 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -105,7 +105,7 @@ static void init_ardupilot() 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); @@ -118,7 +118,13 @@ static void init_ardupilot() // 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); + gcs[1].init(hal.uartC); + + if (num_gcs > 2) { + hal.uartD->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), + 128, SERIAL2_BUFSIZE); + gcs[2].init(hal.uartD); + } mavlink_system.sysid = g.sysid_this_mav; @@ -130,7 +136,9 @@ static void init_ardupilot() } else if (DataFlash.NeedErase()) { gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS")); do_erase_logs(); - gcs0.reset_cli_timeout(); + for (uint8_t i=0; iprintln_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(); if (g.log_bitmask & MASK_LOG_CMD)