DataChunk.cpp
Go to the documentation of this file.
1 /*
2 MIT LICENSE
3 
4 Copyright (c) 2014-2020 Inertial Sense, Inc. - http://inertialsense.com
5 
6 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files(the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions :
7 
8 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
9 
10 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
11 */
12 
13 #include <stdio.h>
14 #include <string.h>
15 #include <stdarg.h>
16 
17 #include "DataChunk.h"
18 #include "ISLogFileBase.h"
19 #include "ISLogFileFactory.h"
20 
22 {
23  Clear();
25  m_hdr.version = 1;
26  m_hdr.classification = ' ' << 8 | 'U';
27  m_hdr.grpNum = 0;
28  m_hdr.devSerialNum = 0;
29  m_hdr.reserved = 0;
33 
34  SetName("PDAT");
35 }
36 
37 
39 {
40 
41 }
42 
43 
44 void cDataChunk::SetName(const char name[4])
45 {
46  if (name == NULL)
47  {
48  return;
49  }
50 
51  memcpy(m_hdr.name, name, 4);
52  m_hdr.invName[0] = ~m_hdr.name[0];
53  m_hdr.invName[1] = ~m_hdr.name[1];
54  m_hdr.invName[2] = ~m_hdr.name[2];
55  m_hdr.invName[3] = ~m_hdr.name[3];
56 }
57 
58 
59 bool cDataChunk::PushBack(uint8_t* d1, uint32_t d1Size, uint8_t* d2, uint32_t d2Size)
60 {
61  // Ensure data will fit
62  uint32_t count = d1Size + d2Size;
63  if (m_dataHead == NULLPTR ||
64  count > GetBuffFree())
65  {
66  return false;
67  }
68  if (d1Size > 0)
69  {
70  memcpy(m_dataTail, d1, d1Size);
71  m_dataTail += d1Size;
72  m_hdr.dataSize += d1Size;
73  }
74  if (d2Size > 0)
75  {
76  memcpy(m_dataTail, d2, d2Size);
77  m_dataTail += d2Size;
78  m_hdr.dataSize += d2Size;
79  }
81  return true;
82 }
83 
84 
86 {
87  return (GetDataSize() == 0 ? NULLPTR : m_dataHead);
88 }
89 
90 
91 bool cDataChunk::PopFront(uint32_t size)
92 {
93  assert(m_dataHead != NULLPTR);
94 
95  if (size <= GetDataSize())
96  {
97  m_dataHead += size;
98  m_hdr.dataSize -= size;
100  return true;
101  }
102  else
103  {
104  Clear();
105  return false;
106  }
107 }
108 
109 
111 {
112  m_hdr.dataSize = 0;
113  m_hdr.invDataSize = (uint32_t)~0;
116 
117 #if LOG_CHUNK_STATS
118  memset(m_stats, 0, sizeof(m_stats));
119 #endif
120 
121 }
122 
123 // Returns number of bytes written, or -1 for error
124 int32_t cDataChunk::WriteToFile(cISLogFileBase* pFile, int groupNumber)
125 {
126  if (pFile == NULLPTR)
127  {
128  return -1;
129  }
130 
131  assert(m_dataHead != NULLPTR);
132 
133  m_hdr.grpNum = groupNumber;
134 
135  // Write chunk header to file
136  int32_t nBytes = static_cast<int32_t>(pFile->write(&m_hdr, sizeof(sChunkHeader)));
137 
138  // Write any additional chunk header
139  nBytes += WriteAdditionalChunkHeader(pFile);
140 
141  // Write chunk data to file
142  nBytes += static_cast<int32_t>(pFile->write(m_dataHead, m_hdr.dataSize));
143 
144 #if LOG_DEBUG_WRITE
145  static int totalBytes = 0;
146  totalBytes += nBytes;
147  printf("%d : %d - %x %d", totalBytes, nBytes, m_Hdr.marker, m_Hdr.dataSize);
148  if (nBytes != (headerSize + (int)m_Hdr.dataSize))
149  printf("ERROR WRITING!");
150  printf("\n");
151 #endif
152 
153  // Error writing to file
154  if (nBytes != GetHeaderSize() + (int)m_hdr.dataSize)
155  {
156  return -1;
157  }
158 
159  // Clear chunk data
160  Clear();
161 
162  return nBytes;
163 }
164 
165 
166 // Returns number of bytes read, or -1 for error
168 {
169  if (pFile == NULLPTR)
170  {
171  return -1;
172  }
173 
174  // reset state, prepare to read from file
175  Clear();
176 
177  // Read chunk header
178  int32_t nBytes = static_cast<int32_t>(pFile->read(&m_hdr, sizeof(sChunkHeader)));
179 
180  // Error checking
181  if (m_hdr.dataSize != ~(m_hdr.invDataSize) || nBytes <= 0)
182  {
183  return -1;
184  }
185 
186  // Read additional chunk header
187  nBytes += ReadAdditionalChunkHeader(pFile);
188 
189 // // Error check data size
190 // if (m_hdr.dataSize > MAX_DATASET_SIZE)
191 // {
192 // return -1;
193 // }
194 
195  // Read chunk data
196  m_dataTail += static_cast<int32_t>(pFile->read(m_buffHead, m_hdr.dataSize));
197  nBytes += GetDataSize();
198 
199 #if LOG_DEBUG_READ
200  static int totalBytes = 0;
201  totalBytes += nBytes;
202  printf("%d : %d - %x %d ", totalBytes, nBytes, m_Hdr.marker, m_Hdr.dataSize);
203  if ((nBytes > 0) && (m_Hdr.marker != DATA_CHUNK_MARKER))
204  {
205  printf("MARKER MISMATCH!");
206 }
207  printf("\n");
208 #endif
209 
210  if (m_hdr.marker == DATA_CHUNK_MARKER && nBytes == static_cast<int>(GetHeaderSize() + m_hdr.dataSize))
211  {
212 
213 #if LOG_CHUNK_STATS
214  static bool start = true;
215  int totalBytes = 0;
216  for (int id = 0; id < DID_COUNT; id++)
217  if (m_stats[id].total)
218  {
219  if (start)
220  {
221  start = false;
222  logStats("------------------------------------------------\n");
223  logStats("Chunk Data Summary\n");
224  logStats(" ID: Count: Sizeof: Total:\n");
225  }
226  logStats(" %4d%8d %5d %7d\n", id, m_stats[id].count, m_stats[id].size, m_stats[id].total);
227  totalBytes += m_stats[id].total;
228  }
229  start = true;
230  if (totalBytes)
231  {
232  logStats("------------------------------------------------\n");
233  logStats(" %8d Total bytes\n", totalBytes);
234  }
235  logStats("\n");
236  logStats("================================================\n");
237  logStats(" *.dat Data Log File\n");
238  logStats("================================================\n");
239  m_Hdr.print();
240 #if LOG_CHUNK_STATS==2
241  logStats("------------------------------------------------\n");
242  logStats("Chunk Data\n");
243 #endif
244  memset(m_stats, 0, sizeof(m_stats));
245 #endif
246 
247  return nBytes;
248  }
249  else
250  {
251  Clear();
252 
253  // Error reading from file
254  return -1;
255  }
256 }
257 
258 
260 {
261  return 0;
262 }
263 
264 
266 {
267  return 0;
268 }
269 
270 
272 {
273  return (int32_t)sizeof(sChunkHeader);
274 }
275 
276 
277 // LOG_CHUNK_STATS
278 void logStats( const char *format, ... )
279 {
280 #if !defined(PLATFORM_IS_EVB_2) || !PLATFORM_IS_EVB_2
281  static cISLogFileBase* pFile = NULL;
282 #endif
283 
284  va_list args;
285  va_start( args, format );
286 
287 #if PLATFORM_IS_EVB_2
288  vprintf( format, args ); // print to terminal
289 #else
290  if( pFile == NULL ) {
291  pFile = CreateISLogFile("STATS_.txt", "w");
292  }
293  pFile->vprintf(format, args); // print to file
294 #endif
295  va_end( args );
296 }
uint32_t invDataSize
Bitwise inverse of chunk data length.
Definition: DataChunk.h:49
virtual std::size_t read(void *bytes, std::size_t len)=0
ROSCPP_DECL void start()
#define DEFAULT_CHUNK_DATA_SIZE
Definition: DataChunk.h:20
#define vprintf
Definition: test_suite.cpp:98
#define printf(...)
uint8_t * m_dataTail
Definition: DataChunk.h:114
uint32_t GetBuffFree()
Definition: DataChunk.h:83
size_t count(InputIterator first, InputIterator last, T const &item)
Definition: catch.hpp:3206
int32_t WriteToFile(cISLogFileBase *pFile, int groupNumber=0)
Definition: DataChunk.cpp:124
#define DID_COUNT
Definition: data_sets.h:138
#define NULL
Definition: nm_bsp.h:52
uint32_t devSerialNum
Device serial number.
Definition: DataChunk.h:51
char name[4]
Chunk name.
Definition: DataChunk.h:46
cISLogFileBase * CreateISLogFile()
uint16_t version
Chunk Version.
Definition: DataChunk.h:44
uint32_t grpNum
Chunk Group Number: 0 = serial data, 1 = sorted data...
Definition: DataChunk.h:50
bool PushBack(uint8_t *d1, uint32_t d1Size, uint8_t *d2=NULL, uint32_t d2Size=0)
Definition: DataChunk.cpp:59
#define NULLPTR
Definition: ISConstants.h:426
void SetName(const char name[4])
Definition: DataChunk.cpp:44
uint8_t * m_buffTail
Definition: DataChunk.h:112
uint32_t GetDataSize()
Definition: DataChunk.h:84
uint32_t dataSize
Chunk data length in bytes.
Definition: DataChunk.h:48
uint8_t * m_dataHead
Definition: DataChunk.h:113
uint16_t classification
Chunk classification.
Definition: DataChunk.h:45
int32_t ReadFromFile(cISLogFileBase *pFile)
Definition: DataChunk.cpp:167
char invName[4]
Bitwise inverse of chunk name.
Definition: DataChunk.h:47
virtual int vprintf(const char *format, va_list args)=0
virtual ~cDataChunk()
Definition: DataChunk.cpp:38
#define DATA_CHUNK_MARKER
Definition: DataChunk.h:23
virtual int32_t WriteAdditionalChunkHeader(cISLogFileBase *pFile)
Definition: DataChunk.cpp:259
virtual std::size_t write(const void *bytes, std::size_t len)=0
virtual int32_t ReadAdditionalChunkHeader(cISLogFileBase *pFile)
Definition: DataChunk.cpp:265
uint8_t m_buffHead[DEFAULT_CHUNK_DATA_SIZE]
Definition: DataChunk.h:111
void logStats(const char *format,...)
Chunk Header.
Definition: DataChunk.cpp:278
virtual void Clear()
Definition: DataChunk.cpp:110
uint8_t * GetDataPtr()
Definition: DataChunk.cpp:85
uint32_t reserved
Unused.
Definition: DataChunk.h:53
virtual int32_t GetHeaderSize()
Definition: DataChunk.cpp:271
bool PopFront(uint32_t size)
Definition: DataChunk.cpp:91
uint32_t marker
Chunk marker (0xFC05EA32)
Definition: DataChunk.h:43
sChunkHeader m_hdr
Definition: DataChunk.h:94


inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04