Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

BufferedLogger/BufferedLogger.cpp@175:7987198a8592, 2018-02-07 (annotated)

Committer:
bwang
Date:
Wed Feb 07 02:42:53 2018 +0000
Revision:
175:7987198a8592
Child:
176:83968cff687a
probably not working BufferedLogger

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 175:7987198a8592 1 #include "BufferedLogger.h"
bwang 175:7987198a8592 2
bwang 175:7987198a8592 3 BufferedLogger::BufferedLogger(int packet_size, int buffer_size, PinName tx, PinName rx) {
bwang 175:7987198a8592 4 _log = new Serial(tx, rx);
bwang 175:7987198a8592 5
bwang 175:7987198a8592 6 _packet_size = packet_size;
bwang 175:7987198a8592 7 _buffer_size = buffer_size;
bwang 175:7987198a8592 8 _page_size = _packet_size * _buffer_size;
bwang 175:7987198a8592 9
bwang 175:7987198a8592 10 _buf1 = (unsigned char*)malloc((_page_size + _packet_size + 1)*sizeof(unsigned char));
bwang 175:7987198a8592 11 _buf2 = (unsigned char*)malloc((_page_size + _packet_size + 1)*sizeof(unsigned char));
bwang 175:7987198a8592 12
bwang 175:7987198a8592 13 _front = _buf1;
bwang 175:7987198a8592 14 _back = _buf2;
bwang 175:7987198a8592 15 _index = 0;
bwang 175:7987198a8592 16 }
bwang 175:7987198a8592 17
bwang 175:7987198a8592 18 int BufferedLogger::log(float *pkt) {
bwang 175:7987198a8592 19 _front[_index] = 0xff;
bwang 175:7987198a8592 20 _index++;
bwang 175:7987198a8592 21 for (int i = 0; i < _packet_size; i++) {
bwang 175:7987198a8592 22 _front[_index + i] = __clip(pkt[i]);
bwang 175:7987198a8592 23 }
bwang 175:7987198a8592 24 _index += _packet_size;
bwang 175:7987198a8592 25 if (_index >= _page_size) {
bwang 175:7987198a8592 26 _index = 0;
bwang 175:7987198a8592 27 return 0;
bwang 175:7987198a8592 28 }
bwang 175:7987198a8592 29 return 1;
bwang 175:7987198a8592 30 }
bwang 175:7987198a8592 31
bwang 175:7987198a8592 32 int BufferedLogger::log(unsigned char *pkt) {
bwang 175:7987198a8592 33 _front[_index] = 0xff;
bwang 175:7987198a8592 34 _index++;
bwang 175:7987198a8592 35 for (int i = 0; i < _packet_size; i++) {
bwang 175:7987198a8592 36 _front[_index + i] = __clip(pkt[i]);
bwang 175:7987198a8592 37 }
bwang 175:7987198a8592 38 _index += _packet_size;
bwang 175:7987198a8592 39 if (_index >= _page_size) {
bwang 175:7987198a8592 40 _index = 0;
bwang 175:7987198a8592 41 return 0;
bwang 175:7987198a8592 42 }
bwang 175:7987198a8592 43 return 1;
bwang 175:7987198a8592 44 }
bwang 175:7987198a8592 45
bwang 175:7987198a8592 46 void BufferedLogger::flush() {
bwang 175:7987198a8592 47 if (_index >= _page_size) {
bwang 175:7987198a8592 48 _index = 0;
bwang 175:7987198a8592 49 _tmp = _front;
bwang 175:7987198a8592 50 _front = _back;
bwang 175:7987198a8592 51 _back = _tmp;
bwang 175:7987198a8592 52 for (int i = 0; i < _page_size; i++) _log->putc(_back[i]);
bwang 175:7987198a8592 53 }
bwang 175:7987198a8592 54 }
bwang 175:7987198a8592 55
bwang 175:7987198a8592 56 unsigned char __clip(float x) {
bwang 175:7987198a8592 57 unsigned char u = (unsigned char) x;
bwang 175:7987198a8592 58 if (u >= 0xff) u = 0xfe;
bwang 175:7987198a8592 59 return u;
bwang 175:7987198a8592 60 }
bwang 175:7987198a8592 61
bwang 175:7987198a8592 62 unsigned char __clip(unsigned char x) {
bwang 175:7987198a8592 63 unsigned char u = x;
bwang 175:7987198a8592 64 if (u >= 0xff) u = 0xfe;
bwang 175:7987198a8592 65 return u;
bwang 175:7987198a8592 66 }