amiro-os / components / bluetooth / checkutility.cpp @ 58fe0e0b
History | View | Annotate | Download (6.04 KB)
| 1 | 58fe0e0b | Thomas Schöpping | #include <amiro/bluetooth/checkutility.hpp> |
|---|---|---|---|
| 2 | |||
| 3 | #include <chprintf.h> |
||
| 4 | #include <hal.h> |
||
| 5 | |||
| 6 | #include <cstring> |
||
| 7 | |||
| 8 | using namespace chibios_rt; |
||
| 9 | using namespace amiro; |
||
| 10 | |||
| 11 | WT12CheckUtility::WT12CheckUtility() |
||
| 12 | {}
|
||
| 13 | |||
| 14 | WT12CheckUtility::~WT12CheckUtility() |
||
| 15 | {}
|
||
| 16 | |||
| 17 | void
|
||
| 18 | WT12CheckUtility::reset() |
||
| 19 | {
|
||
| 20 | palWritePad(GPIOC, GPIOC_BT_RST, PAL_HIGH); |
||
| 21 | BaseThread::sleep(MS2ST(10));
|
||
| 22 | palWritePad(GPIOC, GPIOC_BT_RST, PAL_LOW); |
||
| 23 | |||
| 24 | return;
|
||
| 25 | } |
||
| 26 | |||
| 27 | bool
|
||
| 28 | WT12CheckUtility::waitForReady(const bool print) |
||
| 29 | {
|
||
| 30 | char buffer[8]; |
||
| 31 | const uint16_t length = this->receiveMessage(buffer, 8); |
||
| 32 | |||
| 33 | if (print) {
|
||
| 34 | msgPrint(buffer, length); |
||
| 35 | } |
||
| 36 | |||
| 37 | return (strcmp(buffer, "READY.\r\n") == 0); |
||
| 38 | } |
||
| 39 | |||
| 40 | uint8_t |
||
| 41 | WT12CheckUtility::selftest() |
||
| 42 | {
|
||
| 43 | return 0; |
||
| 44 | } |
||
| 45 | |||
| 46 | void
|
||
| 47 | WT12CheckUtility::msgPrint(const char* message, const uint16_t length) |
||
| 48 | {
|
||
| 49 | if (length > 0) |
||
| 50 | {
|
||
| 51 | chSequentialStreamWrite((BaseSequentialStream*)&SD1, (uint8_t*)message, length); |
||
| 52 | // chprintf((BaseSequentialStream*) &SD1, "[%d]\n", length);
|
||
| 53 | } |
||
| 54 | return;
|
||
| 55 | } |
||
| 56 | |||
| 57 | void
|
||
| 58 | WT12CheckUtility::msgPrintMulti(const char* message_buf, const uint16_t* length_buf, const uint16_t num_messages) |
||
| 59 | {
|
||
| 60 | uint16_t pos = 0;
|
||
| 61 | for (uint16_t i = 0; i < num_messages; ++i) |
||
| 62 | {
|
||
| 63 | this->msgPrint(&message_buf[pos], length_buf[i]);
|
||
| 64 | pos += length_buf[i]; |
||
| 65 | } |
||
| 66 | |||
| 67 | return;
|
||
| 68 | } |
||
| 69 | |||
| 70 | void
|
||
| 71 | WT12CheckUtility::sendMessage(char* message, uint8_t link_id)
|
||
| 72 | {
|
||
| 73 | const uint16_t length = strlen(message);
|
||
| 74 | if (length >= 1 << 10) { |
||
| 75 | chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message too long!\n", __FILE__ , __LINE__);
|
||
| 76 | return;
|
||
| 77 | } |
||
| 78 | |||
| 79 | Message msg; |
||
| 80 | msg.head.start_of_frame = MUX_StartOfFrame; |
||
| 81 | msg.head.link_id = link_id; |
||
| 82 | msg.head.flags = 0x00u;
|
||
| 83 | msg.head.length = length; |
||
| 84 | msg.message = message; |
||
| 85 | msg.n_link = ~link_id; |
||
| 86 | |||
| 87 | chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.start_of_frame); |
||
| 88 | chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.link_id); |
||
| 89 | chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.flags | (msg.head.length >> 8));
|
||
| 90 | chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.length & 0xFFu);
|
||
| 91 | chSequentialStreamWrite((BaseSequentialStream*)&SD3, (uint8_t*)msg.message, msg.head.length); |
||
| 92 | chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.n_link); |
||
| 93 | |||
| 94 | return;
|
||
| 95 | } |
||
| 96 | |||
| 97 | void
|
||
| 98 | WT12CheckUtility::sendMessageRaw(const char* message) |
||
| 99 | {
|
||
| 100 | chSequentialStreamWrite((BaseSequentialStream*)&SD3, (uint8_t*)message, strlen(message)); |
||
| 101 | return;
|
||
| 102 | } |
||
| 103 | |||
| 104 | uint16_t |
||
| 105 | WT12CheckUtility::receiveMessage(char* message, const uint16_t max_length) |
||
| 106 | {
|
||
| 107 | Message msg; |
||
| 108 | msg.message = message; |
||
| 109 | |||
| 110 | msg.head.start_of_frame = chSequentialStreamGet((BaseSequentialStream*)&SD3); |
||
| 111 | msg.head.link_id = chSequentialStreamGet((BaseSequentialStream*)&SD3); |
||
| 112 | msg.head.flags = chSequentialStreamGet((BaseSequentialStream*)&SD3); |
||
| 113 | msg.head.length = (msg.head.flags & 0x03u) << 8; |
||
| 114 | msg.head.flags = msg.head.flags >> 2;
|
||
| 115 | msg.head.length += uint8_t(chSequentialStreamGet((BaseSequentialStream*)&SD3)); |
||
| 116 | |||
| 117 | if (msg.head.length > max_length)
|
||
| 118 | {
|
||
| 119 | chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message too long (%d of max %d bytes)!\n", __FILE__ , __LINE__, msg.head.length, max_length);
|
||
| 120 | |||
| 121 | for (uint16_t i = 0; i < msg.head.length; ++i) { |
||
| 122 | chSequentialStreamGet((BaseSequentialStream*)&SD3); |
||
| 123 | } |
||
| 124 | msg.head.length = 0;
|
||
| 125 | } |
||
| 126 | else
|
||
| 127 | {
|
||
| 128 | chSequentialStreamRead((BaseSequentialStream*)&SD3, (uint8_t*)msg.message, msg.head.length); |
||
| 129 | } |
||
| 130 | |||
| 131 | msg.n_link = chSequentialStreamGet((BaseSequentialStream*)&SD3); |
||
| 132 | |||
| 133 | if (uint8_t(msg.head.link_id ^ msg.n_link) != 0xFFu) |
||
| 134 | {
|
||
| 135 | chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message link id does not match (0x%02X vs 0x%02X)!\n", __FILE__ , __LINE__, msg.head.link_id, msg.n_link);
|
||
| 136 | } |
||
| 137 | |||
| 138 | return msg.head.length;
|
||
| 139 | } |
||
| 140 | |||
| 141 | uint16_t |
||
| 142 | WT12CheckUtility::receiveMessages(char* message_buf, const uint16_t msg_buf_size, uint16_t* length_buf, const uint16_t len_buf_size, const char* last_msg, const char* filter_in, const char* filter_out) |
||
| 143 | {
|
||
| 144 | if (msg_buf_size == 0 || len_buf_size == 0) { |
||
| 145 | return 0; |
||
| 146 | } |
||
| 147 | |||
| 148 | for (uint16_t i = 0; i < len_buf_size; ++i) { |
||
| 149 | length_buf[i] = 0;
|
||
| 150 | } |
||
| 151 | |||
| 152 | uint16_t msg_pos = 0;
|
||
| 153 | uint16_t msg_count = 0;
|
||
| 154 | while (true) |
||
| 155 | {
|
||
| 156 | length_buf[msg_count] = this->receiveMessage(&message_buf[msg_pos], msg_buf_size-msg_pos);
|
||
| 157 | |||
| 158 | // if (msg_count >= 0) {
|
||
| 159 | // msgPrint(&message_buf[msg_pos], length_buf[msg_count]);
|
||
| 160 | // }
|
||
| 161 | |||
| 162 | if (length_buf[msg_count] == 0) { |
||
| 163 | chprintf((BaseSequentialStream*) &SD1, "%s(%d): WARNING: receiving aborted after %d messages (%d bytes)\n", __FILE__ , __LINE__, msg_count, msg_pos);
|
||
| 164 | return msg_count;
|
||
| 165 | } |
||
| 166 | |||
| 167 | if (strncmp(&message_buf[msg_pos], last_msg, strlen(last_msg)) == 0) { |
||
| 168 | return msg_count+1; |
||
| 169 | } |
||
| 170 | |||
| 171 | if (strncmp(&message_buf[msg_pos], filter_in, strlen(filter_in)) == 0) |
||
| 172 | {
|
||
| 173 | if (filter_out == NULL || strncmp(&message_buf[msg_pos], filter_out, strlen(filter_out)) != 0) |
||
| 174 | {
|
||
| 175 | msg_pos += length_buf[msg_count]; |
||
| 176 | ++msg_count; |
||
| 177 | } |
||
| 178 | } |
||
| 179 | |||
| 180 | if (msg_count > len_buf_size) {
|
||
| 181 | chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: maximum messages received (%d)!\n", __FILE__ , __LINE__, len_buf_size);
|
||
| 182 | return len_buf_size;
|
||
| 183 | } |
||
| 184 | |||
| 185 | } |
||
| 186 | |||
| 187 | return 0xFFFFu; // unreachable |
||
| 188 | } |
||
| 189 | |||
| 190 | void
|
||
| 191 | WT12CheckUtility::receiveRaw(uint8_t* buffer, uint16_t num_bytes, const bool print) |
||
| 192 | {
|
||
| 193 | uint16_t i; |
||
| 194 | |||
| 195 | for (i = 0; i < num_bytes; ++i) |
||
| 196 | {
|
||
| 197 | buffer[i] = chSequentialStreamGet((BaseSequentialStream*)&SD3); |
||
| 198 | } |
||
| 199 | |||
| 200 | if (print)
|
||
| 201 | {
|
||
| 202 | for (i = 0; i < num_bytes; ++i) |
||
| 203 | {
|
||
| 204 | chprintf((BaseSequentialStream*) &SD1, "%02X ", buffer[i]);
|
||
| 205 | } |
||
| 206 | chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
| 207 | } |
||
| 208 | |||
| 209 | return;
|
||
| 210 | } |
||
| 211 | |||
| 212 | void
|
||
| 213 | WT12CheckUtility::receiveRawLine(const bool print) |
||
| 214 | {
|
||
| 215 | uint8_t byte = 0;;
|
||
| 216 | while (byte != '\n') |
||
| 217 | {
|
||
| 218 | byte = chSequentialStreamGet((BaseSequentialStream*) &SD3); |
||
| 219 | if (print) {
|
||
| 220 | chprintf((BaseSequentialStream*) &SD1, "%c", byte);
|
||
| 221 | } |
||
| 222 | } |
||
| 223 | |||
| 224 | return;
|
||
| 225 | } |
||
| 226 | |||
| 227 | void
|
||
| 228 | WT12CheckUtility::clearMessageBuffer(const bool print) |
||
| 229 | {
|
||
| 230 | uint8_t byte; |
||
| 231 | while (!sdGetWouldBlock(&SD3))
|
||
| 232 | {
|
||
| 233 | byte = sdGet(&SD3); |
||
| 234 | //byte = chSequentialStreamGet((BaseSequentialStream*) &SD3);
|
||
| 235 | if (print) {
|
||
| 236 | chprintf((BaseSequentialStream*) &SD1, "0x%02X <%c>\n", byte, byte);
|
||
| 237 | } |
||
| 238 | BaseThread::sleep(MS2ST(1));
|
||
| 239 | } |
||
| 240 | |||
| 241 | return;
|
||
| 242 | } |