Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef LMS1XX_LMS_BUFFER_H_
00024 #define LMS1XX_LMS_BUFFER_H_
00025
00026 #include "console_bridge/console.h"
00027 #include <stdint.h>
00028 #include <string.h>
00029 #include <unistd.h>
00030
00031 #define LMS_BUFFER_SIZE 50000
00032 #define LMS_STX 0x02
00033 #define LMS_ETX 0x03
00034
00035 class LMSBuffer
00036 {
00037 public:
00038 LMSBuffer() : total_length_(0), end_of_first_message_(0)
00039 {
00040 }
00041
00042 void readFrom(int fd)
00043 {
00044 int ret = read(fd, buffer_ + total_length_, sizeof(buffer_) - total_length_);
00045
00046 if (ret > 0)
00047 {
00048 total_length_ += ret;
00049 logDebug("Read %d bytes from fd, total length is %d.", ret, total_length_);
00050 }
00051 else
00052 {
00053
00054 logWarn("Buffer read() returned error.");
00055 }
00056 }
00057
00058 char* getNextBuffer()
00059 {
00060 if (total_length_ == 0)
00061 {
00062
00063 logDebug("Empty buffer, nothing to return.");
00064 return NULL;
00065 }
00066
00067
00068
00069
00070 char* start_of_message = (char*)memchr(buffer_, LMS_STX, total_length_);
00071 if (start_of_message == NULL)
00072 {
00073
00074 logWarn("No STX found, dropping %d bytes from buffer.", total_length_);
00075 total_length_ = 0;
00076 }
00077 else if (buffer_ != start_of_message)
00078 {
00079
00080 logWarn("Shifting buffer, dropping %d bytes, %d bytes remain.",
00081 (start_of_message - buffer_), total_length_ - (start_of_message - buffer_));
00082 shiftBuffer(start_of_message);
00083 }
00084
00085
00086 end_of_first_message_ = (char*)memchr(buffer_, LMS_ETX, total_length_);
00087 if (end_of_first_message_ == NULL)
00088 {
00089
00090 logDebug("No ETX found, nothing to return.");
00091 return NULL;
00092 }
00093
00094
00095 *end_of_first_message_ = 0;
00096 return buffer_;
00097 }
00098
00099 void popLastBuffer()
00100 {
00101 if (end_of_first_message_)
00102 {
00103 shiftBuffer(end_of_first_message_ + 1);
00104 end_of_first_message_ = NULL;
00105 }
00106 }
00107
00108 private:
00109 void shiftBuffer(char* new_start)
00110 {
00111
00112 uint16_t remaining_length = total_length_ - (new_start - buffer_);
00113
00114 if (remaining_length > 0)
00115 {
00116 memmove(buffer_, new_start, remaining_length);
00117 }
00118 total_length_ = remaining_length;
00119 }
00120
00121 char buffer_[LMS_BUFFER_SIZE];
00122 uint16_t total_length_;
00123
00124 char* end_of_first_message_;
00125 };
00126
00127 #endif // LMS1XX_LMS_BUFFER_H_