Serial library for Ususama
Dependents: ususama_serial_demo WRS2021_mecanum_driver
ususama_serial.cpp@4:fd646595de66, 2021-08-25 (annotated)
- Committer:
- sgrsn
- Date:
- Wed Aug 25 12:11:35 2021 +0000
- Revision:
- 4:fd646595de66
- Parent:
- 2:92e2ae1b1886
Update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 0:a0ec74fb2cb0 | 1 | #include "ususama_serial.h" |
sgrsn | 0:a0ec74fb2cb0 | 2 | |
sgrsn | 0:a0ec74fb2cb0 | 3 | UsusamaSerial::UsusamaSerial(PinName tx, PinName rx, int32_t* registar, int baudrate) : port_(tx, rx) |
sgrsn | 0:a0ec74fb2cb0 | 4 | { |
sgrsn | 0:a0ec74fb2cb0 | 5 | _register = registar; |
sgrsn | 0:a0ec74fb2cb0 | 6 | set_bufferedserial(baudrate); |
sgrsn | 0:a0ec74fb2cb0 | 7 | } |
sgrsn | 0:a0ec74fb2cb0 | 8 | |
sgrsn | 0:a0ec74fb2cb0 | 9 | void UsusamaSerial::set_bufferedserial(int baudrate) |
sgrsn | 0:a0ec74fb2cb0 | 10 | { |
sgrsn | 0:a0ec74fb2cb0 | 11 | port_.set_baud(baudrate); |
sgrsn | 0:a0ec74fb2cb0 | 12 | port_.set_format( |
sgrsn | 0:a0ec74fb2cb0 | 13 | 8, |
sgrsn | 0:a0ec74fb2cb0 | 14 | BufferedSerial::None, |
sgrsn | 0:a0ec74fb2cb0 | 15 | 1 |
sgrsn | 0:a0ec74fb2cb0 | 16 | ); |
sgrsn | 0:a0ec74fb2cb0 | 17 | } |
sgrsn | 0:a0ec74fb2cb0 | 18 | void UsusamaSerial::set_unbufferedserial(int baudrate) |
sgrsn | 0:a0ec74fb2cb0 | 19 | { |
sgrsn | 0:a0ec74fb2cb0 | 20 | /*port_.baud(baudrate); |
sgrsn | 0:a0ec74fb2cb0 | 21 | port_.format( |
sgrsn | 0:a0ec74fb2cb0 | 22 | 8, |
sgrsn | 0:a0ec74fb2cb0 | 23 | SerialBase::None, |
sgrsn | 0:a0ec74fb2cb0 | 24 | 1 |
sgrsn | 0:a0ec74fb2cb0 | 25 | );*/ |
sgrsn | 0:a0ec74fb2cb0 | 26 | } |
sgrsn | 0:a0ec74fb2cb0 | 27 | |
sgrsn | 0:a0ec74fb2cb0 | 28 | void UsusamaSerial::writeData(int32_t data, uint8_t reg) |
sgrsn | 0:a0ec74fb2cb0 | 29 | { |
sgrsn | 0:a0ec74fb2cb0 | 30 | uint8_t dataBytes[4] = |
sgrsn | 0:a0ec74fb2cb0 | 31 | { |
sgrsn | 0:a0ec74fb2cb0 | 32 | (uint8_t)((data >> 24) & 0xFF), |
sgrsn | 0:a0ec74fb2cb0 | 33 | (uint8_t)((data >> 16) & 0xFF), |
sgrsn | 0:a0ec74fb2cb0 | 34 | (uint8_t)((data >> 8) & 0xFF), |
sgrsn | 0:a0ec74fb2cb0 | 35 | (uint8_t)((data >> 0) & 0xFF) |
sgrsn | 0:a0ec74fb2cb0 | 36 | }; |
sgrsn | 0:a0ec74fb2cb0 | 37 | |
sgrsn | 0:a0ec74fb2cb0 | 38 | std::vector<uint8_t> buf; |
sgrsn | 0:a0ec74fb2cb0 | 39 | buf.push_back(HEAD_BYTE); |
sgrsn | 0:a0ec74fb2cb0 | 40 | uint8_t checksum = 0; |
sgrsn | 0:a0ec74fb2cb0 | 41 | buf.push_back(reg); |
sgrsn | 0:a0ec74fb2cb0 | 42 | checksum += reg; |
sgrsn | 0:a0ec74fb2cb0 | 43 | for (uint8_t i = 0; i < 4; ++i) |
sgrsn | 0:a0ec74fb2cb0 | 44 | { |
sgrsn | 0:a0ec74fb2cb0 | 45 | if ((dataBytes[i] == ESCAPE_BYTE) || (dataBytes[i] == HEAD_BYTE)) |
sgrsn | 0:a0ec74fb2cb0 | 46 | { |
sgrsn | 0:a0ec74fb2cb0 | 47 | buf.push_back(ESCAPE_BYTE); |
sgrsn | 0:a0ec74fb2cb0 | 48 | checksum += ESCAPE_BYTE; |
sgrsn | 0:a0ec74fb2cb0 | 49 | buf.push_back(dataBytes[i] ^ ESCAPE_MASK); |
sgrsn | 0:a0ec74fb2cb0 | 50 | checksum += dataBytes[i] ^ ESCAPE_MASK; |
sgrsn | 0:a0ec74fb2cb0 | 51 | } |
sgrsn | 0:a0ec74fb2cb0 | 52 | else |
sgrsn | 0:a0ec74fb2cb0 | 53 | { |
sgrsn | 0:a0ec74fb2cb0 | 54 | buf.push_back(dataBytes[i]); |
sgrsn | 0:a0ec74fb2cb0 | 55 | checksum += dataBytes[i]; |
sgrsn | 0:a0ec74fb2cb0 | 56 | } |
sgrsn | 0:a0ec74fb2cb0 | 57 | } |
sgrsn | 0:a0ec74fb2cb0 | 58 | |
sgrsn | 0:a0ec74fb2cb0 | 59 | // 末尾にチェックサムを追加で送信する |
sgrsn | 0:a0ec74fb2cb0 | 60 | buf.push_back(checksum); |
sgrsn | 0:a0ec74fb2cb0 | 61 | |
sgrsn | 0:a0ec74fb2cb0 | 62 | // HEADを抜いたデータサイズ |
sgrsn | 0:a0ec74fb2cb0 | 63 | uint8_t size = buf.size() - 1; |
sgrsn | 0:a0ec74fb2cb0 | 64 | |
sgrsn | 0:a0ec74fb2cb0 | 65 | // HEADの後にsizeを挿入 |
sgrsn | 0:a0ec74fb2cb0 | 66 | buf.insert(buf.begin()+1, size); |
sgrsn | 0:a0ec74fb2cb0 | 67 | |
sgrsn | 0:a0ec74fb2cb0 | 68 | for(int i = 0; i < buf.size(); i++) |
sgrsn | 0:a0ec74fb2cb0 | 69 | { |
sgrsn | 0:a0ec74fb2cb0 | 70 | buffer_w_[i] = buf[i]; |
sgrsn | 0:a0ec74fb2cb0 | 71 | } |
sgrsn | 0:a0ec74fb2cb0 | 72 | port_.write(buffer_w_, buf.size()); |
sgrsn | 0:a0ec74fb2cb0 | 73 | } |
sgrsn | 0:a0ec74fb2cb0 | 74 | |
sgrsn | 1:33d6c6f43306 | 75 | uint8_t UsusamaSerial::readData() |
sgrsn | 0:a0ec74fb2cb0 | 76 | { |
sgrsn | 1:33d6c6f43306 | 77 | if(!port_.readable()) return 0; |
sgrsn | 0:a0ec74fb2cb0 | 78 | port_.read(buffer_r_, 1); |
sgrsn | 1:33d6c6f43306 | 79 | if (buffer_r_[0] != HEAD_BYTE) return 0; |
sgrsn | 0:a0ec74fb2cb0 | 80 | |
sgrsn | 1:33d6c6f43306 | 81 | if(!port_.readable()) return 0; |
sgrsn | 0:a0ec74fb2cb0 | 82 | port_.read(buffer_r_+1, 1); |
sgrsn | 0:a0ec74fb2cb0 | 83 | |
sgrsn | 0:a0ec74fb2cb0 | 84 | // data + 1byte(reg) + 1byte(checksum) |
sgrsn | 0:a0ec74fb2cb0 | 85 | int size = buffer_r_[1]; |
sgrsn | 0:a0ec74fb2cb0 | 86 | |
sgrsn | 1:33d6c6f43306 | 87 | if(!port_.readable()) return 0; |
sgrsn | 0:a0ec74fb2cb0 | 88 | port_.read(buffer_r_+2, size); |
sgrsn | 0:a0ec74fb2cb0 | 89 | |
sgrsn | 1:33d6c6f43306 | 90 | uint8_t bytes[8] = {0}; |
sgrsn | 2:92e2ae1b1886 | 91 | uint8_t checksum = 0; |
sgrsn | 0:a0ec74fb2cb0 | 92 | uint8_t reg = buffer_r_[2]; |
sgrsn | 2:92e2ae1b1886 | 93 | checksum += reg; |
sgrsn | 0:a0ec74fb2cb0 | 94 | int index = 3; |
sgrsn | 1:33d6c6f43306 | 95 | int data_size = size - 2; |
sgrsn | 1:33d6c6f43306 | 96 | for (int i = 0; i < data_size; ++i) |
sgrsn | 0:a0ec74fb2cb0 | 97 | { |
sgrsn | 0:a0ec74fb2cb0 | 98 | uint8_t d = buffer_r_[index++]; |
sgrsn | 0:a0ec74fb2cb0 | 99 | if (d == ESCAPE_BYTE) |
sgrsn | 0:a0ec74fb2cb0 | 100 | { |
sgrsn | 2:92e2ae1b1886 | 101 | checksum += (uint8_t)d; |
sgrsn | 0:a0ec74fb2cb0 | 102 | uint8_t nextByte = buffer_r_[index++]; |
sgrsn | 0:a0ec74fb2cb0 | 103 | bytes[i] = (uint8_t)((int)nextByte ^ (int)ESCAPE_MASK); |
sgrsn | 2:92e2ae1b1886 | 104 | checksum += (uint8_t)nextByte; |
sgrsn | 1:33d6c6f43306 | 105 | data_size--; |
sgrsn | 0:a0ec74fb2cb0 | 106 | } |
sgrsn | 0:a0ec74fb2cb0 | 107 | else |
sgrsn | 0:a0ec74fb2cb0 | 108 | { |
sgrsn | 0:a0ec74fb2cb0 | 109 | bytes[i] = d; |
sgrsn | 0:a0ec74fb2cb0 | 110 | checksum += d; |
sgrsn | 0:a0ec74fb2cb0 | 111 | } |
sgrsn | 0:a0ec74fb2cb0 | 112 | } |
sgrsn | 0:a0ec74fb2cb0 | 113 | |
sgrsn | 0:a0ec74fb2cb0 | 114 | uint8_t checksum_recv = buffer_r_[index++]; |
sgrsn | 0:a0ec74fb2cb0 | 115 | int32_t DATA = 0x00; |
sgrsn | 0:a0ec74fb2cb0 | 116 | for(int i = 0; i < 4; i++) |
sgrsn | 0:a0ec74fb2cb0 | 117 | { |
sgrsn | 0:a0ec74fb2cb0 | 118 | DATA |= (((int32_t)bytes[i]) << (24 - (i*8))); |
sgrsn | 0:a0ec74fb2cb0 | 119 | } |
sgrsn | 0:a0ec74fb2cb0 | 120 | if (checksum == checksum_recv) |
sgrsn | 0:a0ec74fb2cb0 | 121 | { |
sgrsn | 0:a0ec74fb2cb0 | 122 | *(_register + reg) = DATA; |
sgrsn | 1:33d6c6f43306 | 123 | /*for(int i = 0; i < data_size; i++) |
sgrsn | 1:33d6c6f43306 | 124 | dst_p[i] = bytes[i];*/ |
sgrsn | 1:33d6c6f43306 | 125 | return reg; |
sgrsn | 0:a0ec74fb2cb0 | 126 | } |
sgrsn | 0:a0ec74fb2cb0 | 127 | else |
sgrsn | 0:a0ec74fb2cb0 | 128 | { |
sgrsn | 0:a0ec74fb2cb0 | 129 | // data error |
sgrsn | 1:33d6c6f43306 | 130 | return 0; |
sgrsn | 0:a0ec74fb2cb0 | 131 | } |
sgrsn | 1:33d6c6f43306 | 132 | return 0; |
sgrsn | 0:a0ec74fb2cb0 | 133 | } |