hidaka sato / UsusamaSerial

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?

UserRevisionLine numberNew 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 }