This library assign registar and send 4bytes data in serial. Auto attachment interruption.
Dependents: WRS_mechanamu_test
Diff: PacketSerail.cpp
- Revision:
- 0:56fce4868747
- Child:
- 1:ee6809d3cee3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PacketSerail.cpp Fri Jun 29 08:31:42 2018 +0000 @@ -0,0 +1,100 @@ +#include "PacketSerial.h" + +PacketSerial::PacketSerial(PinName tx, PinName rx, int32_t* registar) : Serial(tx, rx) +{ + _registar = registar; + Serial::baud(9600); + Serial::attach(this, &PacketSerial::readData, Serial::RxIrq); +} + +void PacketSerial::writeData(int32_t data, uint8_t reg) +{ + uint8_t dataBytes[4] = + { + (data >> 24) & 0xFF, + (data >> 16) & 0xFF, + (data >> 8) & 0xFF, + (data >> 0) & 0xFF + }; + + Serial::putc(HEAD_BYTE); + Serial::putc(reg); + uint8_t checksum = 0; + for (uint8_t i = 0; i < 4; ++i) + { + if ((dataBytes[i] == ESCAPE_BYTE) || (dataBytes[i] == HEAD_BYTE)) + { + Serial::putc(ESCAPE_BYTE); + checksum += ESCAPE_BYTE; + Serial::putc(dataBytes[i] ^ ESCAPE_MASK); + checksum += dataBytes[i] ^ ESCAPE_MASK; + } + else + { + Serial::putc(dataBytes[i]); + checksum += dataBytes[i]; + } + } + + // 末尾にチェックサムを追加で送信する + Serial::putc(checksum); +} + +void PacketSerial::writeFloatData(float data, uint8_t reg) +{ + int integer = (int)data; + uint32_t decimal = (uint32_t)((data-integer)*1000000); + writeData(integer, reg); + writeData(decimal, reg+1); +} + +float PacketSerial::getFloatData(uint8_t reg) +{ + return *(_registar + reg) + ((float)*(_registar + reg+1))/1000000; +} + +void PacketSerial::readData() +{ + uint8_t bytes[4] = {0,0,0,0}; + int8_t checksum = 0; + wait_us(500); + uint8_t data = Serial::getc(); + wait_us(500); + + if (data == HEAD_BYTE) + { + uint8_t reg = Serial::getc(); + for (int i = 0; i < 4; ++i) + { + uint8_t d = Serial::getc(); + wait_us(500); + if (d == ESCAPE_BYTE) + { + uint8_t nextByte = Serial::getc(); + bytes[i] = nextByte ^ ESCAPE_MASK; + checksum += (d + nextByte); + } + else + { + bytes[i] = d; + checksum += d; + } + } + int8_t checksum_recv = Serial::getc(); + wait_us(500); + int32_t DATA = 0x00; + for(int i = 0; i < 4; i++) + { + DATA |= (((int32_t)bytes[i]) << (24 - (i*8))); + } + + if (checksum == checksum_recv) + { + *(_registar + reg) = DATA; + } + else + { + // data error + } + } +} \ No newline at end of file