lms_buffer.h
Go to the documentation of this file.
00001 /*
00002  * lms_buffer.h
00003  *
00004  *  Author: Mike Purvis <mpurvis@clearpathrobotics.com>
00005  ***************************************************************************
00006  *   This library is free software; you can redistribute it and/or         *
00007  *   modify it under the terms of the GNU Lesser General Public            *
00008  *   License as published by the Free Software Foundation; either          *
00009  *   version 2.1 of the License, or (at your option) any later version.    *
00010  *                                                                         *
00011  *   This library is distributed in the hope that it will be useful,       *
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00014  *   Lesser General Public License for more details.                       *
00015  *                                                                         *
00016  *   You should have received a copy of the GNU Lesser General Public      *
00017  *   License along with this library; if not, write to the Free Software   *
00018  *   Foundation, Inc., 59 Temple Place,                                    *
00019  *   Suite 330, Boston, MA  02111-1307  USA                                *
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       // Buffer is empty, no scan data present.
00063       logDebug("Empty buffer, nothing to return.");
00064       return NULL;
00065     }
00066 
00067     // The objective is to have a message starting at the start of the buffer, so if that's not
00068     // the case, then we look for a start-of-message character and shift the buffer back, discarding
00069     // any characters in the middle.
00070     char* start_of_message = (char*)memchr(buffer_, LMS_STX, total_length_);
00071     if (start_of_message == NULL)
00072     {
00073       // None found, buffer reset.
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       // Start of message found, ahead of the start of buffer. Therefore shift the buffer back.
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     // Now look for the end of message character.
00086     end_of_first_message_ = (char*)memchr(buffer_, LMS_ETX, total_length_);
00087     if (end_of_first_message_ == NULL)
00088     {
00089       // No end of message found, therefore no message to parse and return.
00090       logDebug("No ETX found, nothing to return.");
00091       return NULL;
00092     }
00093 
00094     // Null-terminate buffer.
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     // Shift back anything remaining in the buffer.
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_


lms1xx
Author(s): Konrad Banachowicz
autogenerated on Thu Jun 6 2019 18:52:14