From a49710f20e33add97960f0f23902921841d8d063 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 23 Nov 2013 21:45:42 +1100 Subject: [PATCH] Copter: added uartD support this also brings GCS_Mavlink.pde closer to the plane implementation --- ArduCopter/ArduCopter.pde | 4 +- ArduCopter/GCS.h | 28 ++-- ArduCopter/GCS_Mavlink.pde | 254 +++++++++++++++++-------------------- ArduCopter/Parameters.h | 3 +- ArduCopter/Parameters.pde | 13 +- ArduCopter/motors.pde | 5 +- ArduCopter/system.pde | 15 ++- 7 files changed, 164 insertions(+), 158 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 39271afee3..fd75c4eb6d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -346,8 +346,8 @@ static AP_OpticalFlow optflow; //////////////////////////////////////////////////////////////////////////////// // 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]; //////////////////////////////////////////////////////////////////////////////// // SONAR selection diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index bfd3ecc133..961bd32950 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -8,7 +8,9 @@ #define __GCS_H #include +#include #include +#include /// /// @class GCS @@ -39,7 +41,6 @@ public: /// void init(AP_HAL::UARTDriver *port) { _port = port; - initialised = true; } /// Update GCS state. @@ -60,6 +61,14 @@ public: void send_message(enum ap_message id) { } + /// Send a text message. + /// + /// @param severity A value describing the importance of the message. + /// @param str The text to be sent. + /// + void send_text(gcs_severity severity, const char *str) { + } + /// Send a text message with a PSTR() /// /// @param severity A value describing the importance of the message. @@ -122,6 +131,10 @@ public: // see if we should send a stream now. Called at 50Hz bool stream_trigger(enum streams stream_num); + // this costs us 51 bytes per instance, but means that low priority + // messages don't block the CPU + mavlink_statustext_t pending_status; + // call to reset the timeout window for entering the cli void reset_cli_timeout(); private: @@ -140,6 +153,7 @@ private: uint16_t _queued_parameter_count; ///< saved count of // parameters for // queued send + uint32_t _queued_parameter_send_time_ms; /// Count the number of reportable parameters. /// @@ -179,16 +193,8 @@ private: uint16_t waypoint_send_timeout; // milliseconds uint16_t waypoint_receive_timeout; // milliseconds - // data stream rates - AP_Int16 streamRateRawSensors; - AP_Int16 streamRateExtendedStatus; - AP_Int16 streamRateRCChannels; - AP_Int16 streamRateRawController; - AP_Int16 streamRatePosition; - AP_Int16 streamRateExtra1; - AP_Int16 streamRateExtra2; - AP_Int16 streamRateExtra3; - AP_Int16 streamRateParams; + // saveable rate of each stream + AP_Int16 streamRates[NUM_STREAMS]; // number of 50Hz ticks until we next send this stream uint8_t stream_ticks[NUM_STREAMS]; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index acacf31a3a..37dfaa3d6f 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -6,11 +6,6 @@ // use this to prevent recursion during sensor init static bool in_mavlink_delay; - -// this costs us 51 bytes, but means that low priority -// messages don't block the CPU -static mavlink_statustext_t pending_status; - // true when we have received at least 1 MAVLink packet static bool mavlink_active; @@ -105,7 +100,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) MAV_TYPE_OCTOROTOR, #elif (FRAME_CONFIG == HELI_FRAME) MAV_TYPE_HELICOPTER, -#elif (FRAME_CONFIG == SINGLE_FRAME) //because mavlink did not define a singlecopter, we use a rocket +#elif (FRAME_CONFIG == SINGLE_FRAME) //because mavlink did not define a singlecopter, we use a rocket MAV_TYPE_ROCKET, #else #error Unrecognised frame type @@ -502,10 +497,11 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) static void NOINLINE send_statustext(mavlink_channel_t chan) { + mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status; mavlink_msg_statustext_send( chan, - pending_status.severity, - pending_status.text); + s->severity, + s->text); } // are we still delaying telemetry to try to avoid Xbee bricking? @@ -629,20 +625,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: @@ -687,7 +675,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) @@ -749,15 +737,13 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char if (severity == SEVERITY_LOW) { // send via the deferred queuing system - pending_status.severity = (uint8_t)severity; - strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); + 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); } else { // send immediately - mavlink_msg_statustext_send( - chan, - severity, - str); + mavlink_msg_statustext_send(chan, severity, str); } } @@ -769,7 +755,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRateRawSensors, 0), + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0], 0), // @Param: EXT_STAT // @DisplayName: Extended status stream rate to ground station @@ -778,7 +764,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus, 0), + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1], 0), // @Param: RC_CHAN // @DisplayName: RC Channel stream rate to ground station @@ -787,7 +773,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRateRCChannels, 0), + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[2], 0), // @Param: RAW_CTRL // @DisplayName: Raw Control stream rate to ground station @@ -796,7 +782,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController, 0), + AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3], 0), // @Param: POSITION // @DisplayName: Position stream rate to ground station @@ -805,7 +791,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition, 0), + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 0), // @Param: EXTRA1 // @DisplayName: Extra data type 1 stream rate to ground station @@ -814,7 +800,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1, 0), + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 0), // @Param: EXTRA2 // @DisplayName: Extra data type 2 stream rate to ground station @@ -823,7 +809,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2, 0), + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 0), // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate to ground station @@ -832,7 +818,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3, 0), + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 0), // @Param: PARAMS // @DisplayName: Parameter stream rate to ground station @@ -841,7 +827,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRateParams, 0), + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 0), AP_GROUPEND }; @@ -858,12 +844,20 @@ void GCS_MAVLINK::init(AP_HAL::UARTDriver* port) { GCS_Class::init(port); - if (port == hal.uartA) { + 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(); @@ -879,7 +873,8 @@ GCS_MAVLINK::update(void) // process received bytes uint16_t nbytes = comm_get_available(chan); - for (uint16_t i=0; i= NUM_STREAMS) { + return false; + } + float rate = (uint8_t)streamRates[stream_num].get(); + + // send at a much lower rate while handling waypoints and + // parameter sends + if ((stream_num != STREAM_PARAMS) && + (waypoint_receiving || _queued_parameter != NULL)) { + rate *= 0.25; } - if (rate == 0) { + if (rate <= 0) { return false; } @@ -1001,14 +975,12 @@ GCS_MAVLINK::data_stream_send(void) gcs_out_of_time = false; if (_queued_parameter != NULL) { - if (streamRateParams.get() <= 0) { - streamRateParams.set(50); + if (streamRates[STREAM_PARAMS].get() <= 0) { + streamRates[STREAM_PARAMS].set(10); } if (stream_trigger(STREAM_PARAMS)) { send_message(MSG_NEXT_PARAM); } - // don't send anything else at the same time as parameters - return; } if (gcs_out_of_time) return; @@ -1102,9 +1074,10 @@ GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { struct Location tell_command = {}; // command for telemetry + switch (msg->msgid) { - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66 + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { // decode mavlink_request_data_stream_t packet; @@ -1125,32 +1098,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch(packet.req_stream_id) { case MAV_DATA_STREAM_ALL: - streamRateRawSensors = freq; - streamRateExtendedStatus = freq; - streamRateRCChannels = freq; - streamRateRawController = freq; - streamRatePosition = freq; - streamRateExtra1 = freq; - streamRateExtra2 = freq; - //streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. - streamRateExtra3 = freq; // Don't save!! + // note that we don't set STREAM_PARAMS - that is internal only + for (uint8_t i=0; i comm_get_txspace(chan)) { + bytes_allowed = comm_get_txspace(chan); + } + count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + + while (_queued_parameter != NULL && count--) { AP_Param *vp; float value; @@ -2128,6 +2099,8 @@ GCS_MAVLINK::queued_param_send() _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type); _queued_parameter_index++; } + _queued_parameter_send_time_ms = tnow; +} /** * queued_waypoint_send - Send the next pending waypoint, called from deferred message @@ -2159,8 +2132,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; @@ -2191,9 +2163,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 *)pending_status.text, - sizeof(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(gcs[0].pending_status.text); +#endif 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 2 + // @Group: SR2_ + // @Path: GCS_Mavlink.pde + GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK), +#endif // @Group: AHRS_ // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 853386d138..c84a749a2a 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -141,8 +141,9 @@ static void init_arm_motors() // mid-flight, so set the serial ports non-blocking once we arm // the motors hal.uartA->set_blocking_writes(false); - if (gcs3.initialised) { - hal.uartC->set_blocking_writes(false); + hal.uartC->set_blocking_writes(false); + if (hal.uartD != NULL) { + hal.uartD->set_blocking_writes(false); } #if COPTER_LEDS == ENABLED diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index a772395c73..45805b0142 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -134,7 +134,7 @@ static void init_ardupilot() #endif // init the GCS - gcs0.init(hal.uartA); + gcs[0].init(hal.uartA); // Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to @@ -151,8 +151,12 @@ static void init_ardupilot() // APM2. We actually do have one on APM2 but it isn't necessary as // a MUX is used hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); - gcs3.init(hal.uartC); + gcs[1].init(hal.uartC); #endif + if (hal.uartD != NULL) { + hal.uartD->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + gcs[2].init(hal.uartD); + } // identify ourselves correctly with the ground station mavlink_system.sysid = g.sysid_this_mav; @@ -166,7 +170,7 @@ static void init_ardupilot() } else if (DataFlash.NeedErase()) { gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS")); do_erase_logs(); - gcs0.reset_cli_timeout(); + gcs[0].reset_cli_timeout(); } #endif @@ -209,9 +213,12 @@ static void init_ardupilot() #if CLI_ENABLED == ENABLED 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); + } #endif // CLI_ENABLED #if HIL_MODE != HIL_MODE_DISABLED