blob: 7f9e42242024c45e4250171cad4d7c502665004a (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
|
/**
* Serial communication module.
*/
#include <stdbool.h>
#include "definitions.h"
#include "dataTypes.h"
#include "comm.h"
#include <stdint.h>
serial_state_t serial_state = SERIAL_UNKNOWN;
static void handle_recv(int r);
static void handle_ctrl(int r) {
// detect reset procedure
switch (r) {
case RESET1:
// reset request received, ack and wait for RESET2
Serial.write(ACK1);
serial_state = SERIAL_INIT;
return;
case RESET2:
if (serial_state == SERIAL_INIT) {
// Handshake completed!
Serial.write(ACK2);
serial_state = SERIAL_READY;
} else {
// invalid state, reject it
Serial.write(OUTOFSYNC);
serial_state = SERIAL_UNKNOWN;
}
return;
case PONG:
// TODO mark as alive if PING was previously sent
return;
}
// TODO commands for navigation? Use DATA_LEN, etc?
}
void handle_serial(data_t *data, int changedBits) {
// first attempt sync
while (Serial.available()) {
int r = Serial.read();
if ((r & 0xC0) == 0x80) {
// handle control packets
handle_ctrl(r);
// TODO if DATA_ESCAPE or DATA_LEN1, react appropriately
} else if (serial_state == SERIAL_READY) {
// handle normal data only when the handshake has completed.
handle_recv(r);
}
}
if (serial_state != SERIAL_READY) {
// Hardware is not ready, do not send data.
return;
}
// then send queued data
}
// writes data of length len
static void serial_write_len(unsigned len) {
len--; // length on wire is one smaller (00 means 1 byte, 01 means 2, etc.)
if (len < 56) {
Serial.write(0x80 + len + 7);
} else if (len < DATA_MAX_SIZE) {
Serial.write(0x88 | (len >> 5));
Serial.write(0xA0 | (len & 0x1F));
} else {
// zero length or too long.
}
}
// write remainder of data
static void serial_write_data(const uint8_t *data, unsigned len) {
for (unsigned i = 0; i < len; i++) {
uint8_t c = data[i];
if (c & 0x80) {
Serial.write(DATA_ESCAPE);
Serial.write(c & ~0x80);
} else {
Serial.write(c);
}
}
}
void serial_print_debug(const char *str) {
unsigned len = strlen(str);
const uint8_t c = DATA_DEBUG_PREFIX;
// only write data when the serial connection is ready
if (serial_state != SERIAL_READY) {
return;
}
while (len > 0) {
// len is no more than DATA_MAX_SIZE - 1 because of debug prefix
unsigned datalen = len >= DATA_MAX_SIZE ? DATA_MAX_SIZE - 1 : len;
// add one for the debug prefix
serial_write_len(1 + datalen);
serial_write_data(&c, 1); /* write prefix */
serial_write_data((uint8_t *)str, datalen); /* write actual data payload */
len -= datalen;
}
}
static void handle_recv(int r) {
}
|