BufferedLogger.cpp

00001 #include "BufferedLogger.h"
00002 
00003 BufferedLogger::BufferedLogger(int packet_size, int buffer_size, PinName tx, PinName rx, int rate) {
00004     _log = new Serial(tx, rx);
00005     _log->baud(rate);
00006     
00007     _packet_size = packet_size;
00008     _buffer_size = buffer_size;
00009     _page_size = (_packet_size + 1) * _buffer_size;
00010     
00011     _buf1 = (unsigned char*)malloc((_page_size + _packet_size + 1)*sizeof(unsigned char));
00012     _buf2 = (unsigned char*)malloc((_page_size + _packet_size + 1)*sizeof(unsigned char));
00013     
00014     _front = _buf1;
00015     _back = _buf2;
00016     _index = 0;
00017     
00018     _enabled = true;
00019 }
00020 
00021 int BufferedLogger::log(float *pkt) {
00022     if (!_enabled) return 1;
00023     _front[_index] = 0xff;
00024     _index++;
00025     for (int i = 0; i < _packet_size; i++) {
00026         _front[_index + i] = __clip(pkt[i]);
00027     }
00028     _index += _packet_size;
00029     if (_index >= _page_size + _packet_size + 1) {
00030         _index = 0;
00031         return 0;
00032     }
00033     return 1;
00034 }
00035 
00036 int BufferedLogger::log(unsigned char *pkt) {
00037     if (!_enabled) return 1;
00038     _front[_index] = 0xff;
00039     _index++;
00040     for (int i = 0; i < _packet_size; i++) {
00041         _front[_index + i] = __clip(pkt[i]);
00042     }
00043     _index += _packet_size;
00044     if (_index >= _page_size + _packet_size + 1) {
00045         _index = 0;
00046         return 0;
00047     }
00048     return 1;
00049 }
00050 
00051 void BufferedLogger::flush() {
00052     if (_index >= _page_size) {
00053         _index = 0;
00054         _tmp = _front;
00055         _front = _back;
00056         _back = _tmp;
00057         for (int i = 0; i < _page_size; i++) _log->putc(_back[i]);
00058     }
00059 }
00060 
00061 unsigned char __clip(float x) {
00062     unsigned char u = (unsigned char) x;
00063     if (u >= 0xff) u = 0xfe;
00064     return u;
00065 }
00066 
00067 unsigned char __clip(unsigned char x) {
00068     unsigned char u = x;
00069     if (u >= 0xff) u = 0xfe;
00070     return u;
00071 }