Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

BufferedLogger/BufferedLogger.cpp@176:83968cff687a, 2018-02-08 (annotated)

Committer:
bwang
Date:
Thu Feb 08 00:52:17 2018 +0000
Revision:
176:83968cff687a
Parent:
175:7987198a8592
Child:
180:a783a972a867
bugfixes to BufferedLogger

Who changed what in which revision?

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