From 69438ad7e42d1937f015a61f58ddcba8d0ae02be Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 May 2018 10:10:30 +1000 Subject: [PATCH 1/2] new buffering strategy this saves on memory and uses UDP more efficiently --- src/mavesp8266.h | 3 +- src/mavesp8266_gcs.cpp | 89 ++++++++++++++---------------- src/mavesp8266_gcs.h | 9 ++- src/mavesp8266_httpd.cpp | 5 ++ src/mavesp8266_vehicle.cpp | 109 ++++++++++--------------------------- src/mavesp8266_vehicle.h | 14 +---- 6 files changed, 85 insertions(+), 144 deletions(-) diff --git a/src/mavesp8266.h b/src/mavesp8266.h index 82e78e39..bf41aaa7 100644 --- a/src/mavesp8266.h +++ b/src/mavesp8266.h @@ -83,6 +83,7 @@ struct linkStatus { uint32_t packets_received; uint32_t packets_lost; uint32_t packets_sent; + uint32_t parse_errors; uint32_t radio_status_sent; uint8_t queue_status; }; @@ -96,7 +97,6 @@ class MavESP8266Bridge { virtual void begin (MavESP8266Bridge* forwardTo); virtual void readMessage () = 0; virtual void readMessageRaw () = 0; - virtual int sendMessage (mavlink_message_t* message, int count) = 0; virtual int sendMessage (mavlink_message_t* message) = 0; virtual int sendMessageRaw (uint8_t *buffer, int len) = 0; virtual bool heardFrom () { return _heard_from; } @@ -107,7 +107,6 @@ class MavESP8266Bridge { mavlink_channel_t _recv_chan; protected: virtual void _checkLinkErrors(mavlink_message_t* msg); - virtual void _sendRadioStatus() = 0; protected: bool _heard_from; uint8_t _system_id; diff --git a/src/mavesp8266_gcs.cpp b/src/mavesp8266_gcs.cpp index 14d9194e..46fd229d 100644 --- a/src/mavesp8266_gcs.cpp +++ b/src/mavesp8266_gcs.cpp @@ -73,12 +73,16 @@ MavESP8266GCS::readMessage() _forwardTo->sendMessage(&_message); memset(&_message, 0, sizeof(_message)); } + uint32_t now = millis(); //-- Update radio status (1Hz) - if(_heard_from && (millis() - _last_status_time > 1000)) { + if(_heard_from && (now - _last_status_time > 1000)) { delay(0); _sendRadioStatus(); _last_status_time = millis(); } + if (_sendbuf_ofs > 0 && (now - _send_start_ms > UDP_QUEUE_TIMEOUT || _packets_queued >= 20)) { + _send_pending(); + } } @@ -97,11 +101,15 @@ MavESP8266GCS::_readMessage() if (result >= 0) { // Parsing + uint8_t last_parse_error = _rxstatus.parse_error; msgReceived = mavlink_frame_char_buffer(&_rxmsg, &_rxstatus, result, &_message, &_mav_status); + if (last_parse_error != _rxstatus.parse_error) { + _status.parse_errors++; + } if(msgReceived) { //-- We no longer need to broadcast _status.packets_received++; @@ -128,14 +136,19 @@ MavESP8266GCS::_readMessage() _checkLinkErrors(&_message); } - if (msgReceived == MAVLINK_FRAMING_BAD_CRC || - msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) { + if (msgReceived == MAVLINK_FRAMING_BAD_CRC) { // we don't process messages locally with bad CRC, // but we do forward them, so when new messages // are added we can bridge them break; } +#ifdef MAVLINK_FRAMING_BAD_SIGNATURE + if (msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) { + break; + } +#endif + //-- Check for message we might be interested if(getWorld()->getComponent()->handleMessage(this, &_message)){ //-- Eat message (don't send it to FC) @@ -192,36 +205,6 @@ MavESP8266GCS::readMessageRaw() { } } -//--------------------------------------------------------------------------------- -//-- Forward message(s) to the GCS -int -MavESP8266GCS::sendMessage(mavlink_message_t* message, int count) { - int sentCount = 0; - _udp.beginPacket(_ip, _udp_port); - for(int i = 0; i < count; i++) { - // Translate message to buffer - char buf[300]; - unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message[i]); - // Send it - _status.packets_sent++; - size_t sent = _udp.write((uint8_t*)(void*)buf, len); - if(sent != len) { - break; - //-- Fibble attempt at not losing data until we get access to the socket TX buffer - // status before we try to send. - _udp.endPacket(); - delay(2); - _udp.beginPacket(_ip, _udp_port); - _udp.write((uint8_t*)(void*)&buf[sent], len - sent); - _udp.endPacket(); - return sentCount; - } - sentCount++; - } - _udp.endPacket(); - return sentCount; -} - //--------------------------------------------------------------------------------- //-- Forward message to the GCS int @@ -231,11 +214,11 @@ MavESP8266GCS::sendMessage(mavlink_message_t* message) { } int -MavESP8266GCS::sendMessageRaw(uint8_t *buffer, int len) { +MavESP8266GCS::sendMessageRaw(uint8_t *buffer, int len) +{ _udp.beginPacket(_ip, _udp_port); size_t sent = _udp.write(buffer, len); _udp.endPacket(); - //_udp.flush(); return sent; } @@ -270,7 +253,7 @@ MavESP8266GCS::_sendRadioStatus() &msg, rssi, // RSSI Only valid in STA mode 0, // We don't have access to Remote RSSI - st->queue_status, // UDP queue status + 100, 0, // We don't have access to noise data lostVehicleMessages, // Percent of lost messages from Vehicle (UART) lostGcsMessages, // Percent of lost messages from GCS (UDP) @@ -289,17 +272,27 @@ MavESP8266GCS::_sendSingleUdpMessage(mavlink_message_t* msg) // Translate message to buffer char buf[300]; unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, msg); - // Send it - _udp.beginPacket(_ip, _udp_port); - size_t sent = _udp.write((uint8_t*)(void*)buf, len); - _udp.endPacket(); - //-- Fibble attempt at not losing data until we get access to the socket TX buffer - // status before we try to send. - if(sent != len) { - delay(1); - _udp.beginPacket(_ip, _udp_port); - _udp.write((uint8_t*)(void*)&buf[sent], len - sent); - _udp.endPacket(); + if (len + _sendbuf_ofs > sizeof(_sendbuf)) { + _send_pending(); + } + memcpy(&_sendbuf[_sendbuf_ofs], buf, len); + if (_sendbuf_ofs == 0) { + _send_start_ms = millis(); + } + _packets_queued++; + _sendbuf_ofs += len; +} + +/* + send pending data in _sendbuf + */ +void MavESP8266GCS::_send_pending(void) +{ + if (_sendbuf_ofs > 0) { + if (sendMessageRaw(_sendbuf, _sendbuf_ofs) == _sendbuf_ofs) { + _sendbuf_ofs = 0; + _status.packets_sent += _packets_queued; + _packets_queued = 0; + } } - _status.packets_sent++; } diff --git a/src/mavesp8266_gcs.h b/src/mavesp8266_gcs.h index 564277d2..2382631b 100644 --- a/src/mavesp8266_gcs.h +++ b/src/mavesp8266_gcs.h @@ -38,6 +38,9 @@ #ifndef MAVESP8266_GCS_H #define MAVESP8266_GCS_H +//-- UDP Outgoing buffer timeout +#define UDP_QUEUE_TIMEOUT 5 // 5ms + #include "mavesp8266.h" class MavESP8266GCS : public MavESP8266Bridge { @@ -47,7 +50,6 @@ class MavESP8266GCS : public MavESP8266Bridge { void begin (MavESP8266Bridge* forwardTo, IPAddress gcsIP); void readMessage (); void readMessageRaw (); - int sendMessage (mavlink_message_t* message, int count); int sendMessage (mavlink_message_t* message); int sendMessageRaw (uint8_t *buffer, int len); protected: @@ -57,6 +59,7 @@ class MavESP8266GCS : public MavESP8266Bridge { bool _readMessage (); void _sendSingleUdpMessage (mavlink_message_t* msg); void _checkUdpErrors (mavlink_message_t* msg); + void _send_pending (); private: WiFiUDP _udp; @@ -64,6 +67,10 @@ class MavESP8266GCS : public MavESP8266Bridge { uint16_t _udp_port; mavlink_message_t _message; unsigned long _last_status_time; + uint8_t _sendbuf[1400]; + uint16_t _sendbuf_ofs; + uint16_t _packets_queued; + uint32_t _send_start_ms; }; #endif diff --git a/src/mavesp8266_httpd.cpp b/src/mavesp8266_httpd.cpp index 48974293..3ae19fb5 100644 --- a/src/mavesp8266_httpd.cpp +++ b/src/mavesp8266_httpd.cpp @@ -134,6 +134,7 @@ void handle_upload_status() { DEBUG_SERIAL.setDebugOutput(true); #endif WiFiUDP::stopAll(); + Serial.end(); #ifdef DEBUG_SERIAL DEBUG_SERIAL.printf("Update: %s\n", upload.filename.c_str()); #endif @@ -321,12 +322,16 @@ static void handle_getStatus() message += gcsStatus->packets_sent; message += "GCS Packets Lost"; message += gcsStatus->packets_lost; + message += "GCS Parse Errors"; + message += gcsStatus->parse_errors; message += "Packets Received from Vehicle"; message += vehicleStatus->packets_received; message += "Packets Sent to Vehicle"; message += vehicleStatus->packets_sent; message += "Vehicle Packets Lost"; message += vehicleStatus->packets_lost; + message += "Vehicle Parse Errors"; + message += vehicleStatus->parse_errors; message += "Radio Messages"; message += gcsStatus->radio_status_sent; message += ""; diff --git a/src/mavesp8266_vehicle.cpp b/src/mavesp8266_vehicle.cpp index a50e8fb4..59b0ef24 100644 --- a/src/mavesp8266_vehicle.cpp +++ b/src/mavesp8266_vehicle.cpp @@ -42,13 +42,9 @@ //--------------------------------------------------------------------------------- MavESP8266Vehicle::MavESP8266Vehicle() - : _queue_count(0) - , _queue_time(0) - , _buffer_status(50.0) { _recv_chan = MAVLINK_COMM_0; _send_chan = MAVLINK_COMM_1; - memset(_message, 0 , sizeof(_message)); } //--------------------------------------------------------------------------------- @@ -66,48 +62,21 @@ MavESP8266Vehicle::begin(MavESP8266Bridge* forwardTo) #endif #endif // raise serial buffer size (default is 256) - Serial.setRxBufferSize(1024); + Serial.setRxBufferSize(4096); } + //--------------------------------------------------------------------------------- //-- Read MavLink message from UAS void MavESP8266Vehicle::readMessage() { - if(_queue_count < UAS_QUEUE_SIZE) { - if(_readMessage()) { - _queue_count++; - } - } - //-- Do we have a message to send and is it time to forward data? - if(_queue_count && (_queue_count >= UAS_QUEUE_THRESHOLD || (millis() - _queue_time) > UAS_QUEUE_TIMEOUT)) { - int sent = _forwardTo->sendMessage(_message, _queue_count); - //-- Sent it all? - if(sent == _queue_count) { - memset(_message, 0, sizeof(_message)); - _queue_count = 0; - _queue_time = millis(); - //-- Sent at least some? - } else if(sent) { - //-- Move the pending ones up the queue - int left = _queue_count - sent; - for(int i = 0; i < left; i++) { - memcpy(&_message[sent+i], &_message[i], sizeof(mavlink_message_t)); - } - _queue_count = left; - } - //-- Maintain buffer status - float cur_status = 0.0; - float buffer_size = (float)UAS_QUEUE_THRESHOLD; - float buffer_left = (float)(UAS_QUEUE_THRESHOLD - _queue_count); - if(buffer_left > 0.0) - cur_status = ((buffer_left / buffer_size) * 100.0f); - _buffer_status = (_buffer_status * 0.05f) + (cur_status * 0.95); + if (_readMessage()) { + _forwardTo->sendMessage(&_msg); } //-- Update radio status (1Hz) if(_heard_from && (millis() - _last_status_time > 1000)) { delay(0); - _sendRadioStatus(); _last_status_time = millis(); } } @@ -130,16 +99,6 @@ MavESP8266Vehicle::readMessageRaw() { _forwardTo->sendMessageRaw((uint8_t*)buf, buf_index); } -//--------------------------------------------------------------------------------- -//-- Send MavLink message to UAS -int -MavESP8266Vehicle::sendMessage(mavlink_message_t* message, int count) { - for(int i = 0; i < count; i++) { - sendMessage(&message[i]); - } - return count; -} - //--------------------------------------------------------------------------------- //-- Send MavLink message to UAS int @@ -148,6 +107,10 @@ MavESP8266Vehicle::sendMessage(mavlink_message_t* message) { char buf[300]; unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, message); // Send it + while (Serial.availableForWrite() < 32) { + // don't spin in the send loop, wait for 25% of the FIFO to be free + delay(1); + } Serial.write((uint8_t*)(void*)buf, len); _status.packets_sent++; return 1; @@ -165,7 +128,7 @@ MavESP8266Vehicle::sendMessageRaw(uint8_t *buffer, int len) { linkStatus* MavESP8266Vehicle::getStatus() { - _status.queue_status = (uint8_t)_buffer_status; + _status.queue_status = 0; return &_status; } @@ -181,40 +144,48 @@ MavESP8266Vehicle::_readMessage() if (result >= 0) { // Parsing + uint8_t last_parse_error = _rxstatus.parse_error; msgReceived = mavlink_frame_char_buffer(&_rxmsg, &_rxstatus, result, - &_message[_queue_count], + &_msg, &_mav_status); + if (last_parse_error != _rxstatus.parse_error) { + _status.parse_errors++; + } if(msgReceived) { _status.packets_received++; //-- Is this the first packet we got? if(!_heard_from) { - if(_message[_queue_count].msgid == MAVLINK_MSG_ID_HEARTBEAT) { + if(_msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { _heard_from = true; - _component_id = _message[_queue_count].compid; - _system_id = _message[_queue_count].sysid; - _seq_expected = _message[_queue_count].seq + 1; + _component_id = _msg.compid; + _system_id = _msg.sysid; + _seq_expected = _msg.seq + 1; _last_heartbeat = millis(); } } else { - if(_message[_queue_count].msgid == MAVLINK_MSG_ID_HEARTBEAT) + if(_msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) _last_heartbeat = millis(); - _checkLinkErrors(&_message[_queue_count]); + _checkLinkErrors(&_msg); } - if (msgReceived == MAVLINK_FRAMING_BAD_CRC || - msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) { + if (msgReceived == MAVLINK_FRAMING_BAD_CRC) { // we don't process messages locally with bad CRC, // but we do forward them, so when new messages // are added we can bridge them break; } +#ifdef MAVLINK_FRAMING_BAD_SIGNATURE + if (msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) { + break; + } +#endif + //-- Check for message we might be interested - if(getWorld()->getComponent()->handleMessage(this, &_message[_queue_count])){ + if(getWorld()->getComponent()->handleMessage(this, &_msg)){ //-- Eat message (don't send it to GCS) - memset(&_message[_queue_count], 0, sizeof(mavlink_message_t)); msgReceived = false; continue; } @@ -232,27 +203,3 @@ MavESP8266Vehicle::_readMessage() return msgReceived; } -//--------------------------------------------------------------------------------- -//-- Send Radio Status -void -MavESP8266Vehicle::_sendRadioStatus() -{ - getStatus(); - //-- Build message - mavlink_message_t msg {}; - mavlink_msg_radio_status_pack_chan( - _forwardTo->systemID(), - MAV_COMP_ID_UDP_BRIDGE, - _send_chan, - &msg, - 0, // We don't have access to RSSI - 0, // We don't have access to Remote RSSI - _status.queue_status, // UDP queue status - 0, // We don't have access to noise data - 0, // We don't have access to remote noise data - (uint16_t)(_status.packets_lost / 10), - 0 // We don't fix anything - ); - sendMessage(&msg); - _status.radio_status_sent++; -} diff --git a/src/mavesp8266_vehicle.h b/src/mavesp8266_vehicle.h index 97437ee0..ba1e8e20 100644 --- a/src/mavesp8266_vehicle.h +++ b/src/mavesp8266_vehicle.h @@ -40,11 +40,6 @@ #include "mavesp8266.h" -//-- UDP Outgoing Packet Queue -#define UAS_QUEUE_SIZE 60 -#define UAS_QUEUE_THRESHOLD 20 -#define UAS_QUEUE_TIMEOUT 5 // 5ms - class MavESP8266Vehicle : public MavESP8266Bridge { public: MavESP8266Vehicle(); @@ -52,22 +47,17 @@ class MavESP8266Vehicle : public MavESP8266Bridge { void begin (MavESP8266Bridge* forwardTo); void readMessage (); void readMessageRaw (); - int sendMessage (mavlink_message_t* message, int count); int sendMessage (mavlink_message_t* message); int sendMessageRaw (uint8_t *buffer, int len); linkStatus* getStatus (); -protected: - void _sendRadioStatus(); - private: bool _readMessage (); + void _send_pending(); private: - int _queue_count; unsigned long _queue_time; - float _buffer_status; - mavlink_message_t _message[UAS_QUEUE_SIZE]; + mavlink_message_t _msg; }; #endif From 909acb0648543ef5fabad3f7c00dcc08ddb53fc4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 May 2018 10:12:03 +1000 Subject: [PATCH 2/2] added git version and date to web UI --- platformio.ini | 7 ++++++- src/mavesp8266_httpd.cpp | 15 +++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index a16a5c76..770e62d5 100644 --- a/platformio.ini +++ b/platformio.ini @@ -20,18 +20,23 @@ # The upload speed below (921600) has worked fine for all modules I tested. If you have upload issues, # try reducing to 115200. +[version] +build_flags = !echo "-DPIO_SRC_REV="$(git rev-parse HEAD) "-DPIO_BUILD_DATE="$(date +%%Y-%%m-%%d) "-DPIO_BUILD_TIME="$(date +%%H:%%M:%%S) + [env:esp12e] platform = espressif8266 framework = arduino board = esp12e -build_flags = -Wl,-Tesp8266.flash.4m.ld +build_flags = ${version.build_flags} -Wl,-Tesp8266.flash.4m.ld [env:esp01_1m] platform = espressif8266 framework = arduino board = esp01_1m +build_flags = ${version.build_flags} [env:esp01] platform = espressif8266 framework = arduino board = esp01 +build_flags = ${version.build_flags} diff --git a/src/mavesp8266_httpd.cpp b/src/mavesp8266_httpd.cpp index 3ae19fb5..2cc2de1c 100644 --- a/src/mavesp8266_httpd.cpp +++ b/src/mavesp8266_httpd.cpp @@ -77,6 +77,13 @@ const char* kFlashMaps[7] = { "4MB (1024/1024)" }; +// convert git version and build date to string +#define str(s) #s +#define xstr(s) str(s) +#define GIT_VERSION_STRING xstr(PIO_SRC_REV) +#define BUILD_DATE_STRING xstr(PIO_BUILD_DATE) +#define BUILD_TIME_STRING xstr(PIO_BUILD_TIME) + static uint32_t flash = 0; static char paramCRC[12] = {""}; @@ -208,6 +215,14 @@ static void handle_root() char vstr[30]; snprintf(vstr, sizeof(vstr), "%u.%u.%u", MAVESP8266_VERSION_MAJOR, MAVESP8266_VERSION_MINOR, MAVESP8266_VERSION_BUILD); message += vstr; + message += "
\n"; + message += "Git Version: "; + message += GIT_VERSION_STRING; + message += "
\n"; + message += "Build Date: "; + message += BUILD_DATE_STRING; + message += " "; + message += BUILD_TIME_STRING; message += "

\n"; message += "