Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
protocol.c
Go to the documentation of this file.
1 
2 
3 #include "protocol.h"
4 
6 
7 
10 // PPRZ parsing state machine
11 #define UNINIT 0
12 #define GOT_STX 1
13 #define GOT_LENGTH 2
14 #define GOT_MSGID 3
15 #define GOT_PAYLOAD 4
16 #define GOT_CRC1 5
17 
19 
20 void parse_mora(struct mora_transport *t, uint8_t c)
21 {
22 //printf("%02X %d %d\n",c, t->status, t->error);
23 
24  switch (t->status) {
25  case UNINIT:
26  if (c == STX) {
27  t->status++;
28  }
29  break;
30  case GOT_STX:
31  if (t->msg_received) {
32  t->error++;
33  goto error;
34  }
35  t->payload_len = c - 5; /* Counting STX, LENGTH and CRC1 and CRC2 */
36  t->ck_a = t->ck_b = c;
37  t->status++;
38  t->payload_idx = 0;
39  break;
40  case GOT_LENGTH:
41  t->msg_id = c;
42  t->ck_a += c; t->ck_b += t->ck_a;
43  t->status++;
44  if (t->payload_len == 0) {
45  t->status++;
46  }
47  break;
48  case GOT_MSGID:
49  t->payload[t->payload_idx] = c;
50  t->ck_a += c; t->ck_b += t->ck_a;
51  t->payload_idx++;
52  if (t->payload_idx == t->payload_len) {
53  t->status++;
54  }
55  break;
56  case GOT_PAYLOAD:
57  if (c != t->ck_a) {
58  goto error;
59  }
60  t->status++;
61  break;
62  case GOT_CRC1:
63  if (c != t->ck_b) {
64  goto error;
65  }
66  t->msg_received = true;
67  goto restart;
68  default:
69  goto error;
70  }
71  return;
72 error:
73  t->error++;
74 restart:
75  t->status = UNINIT;
76  return;
77 }
uint8_t payload_len
Definition: protocol.h:136
#define GOT_LENGTH
Definition: protocol.c:13
uint8_t error
Definition: protocol.h:133
struct mora_transport mora_protocol
Definition: protocol.c:18
|STX|length|...
uint8_t msg_id
Definition: protocol.h:134
uint8_t ck_b
Definition: protocol.h:140
uint8_t payload[256]
Definition: protocol.h:132
#define GOT_MSGID
Definition: protocol.c:14
#define GOT_PAYLOAD
Definition: protocol.c:15
bool msg_received
Definition: protocol.h:135
#define GOT_STX
Definition: protocol.c:12
#define STX
Definition: protocol.h:99
uint8_t payload_idx
Definition: protocol.h:139
unsigned char uint8_t
Definition: types.h:14
uint8_t ck_a
Definition: protocol.h:140
uint8_t mora_ck_b
Definition: protocol.c:5
uint8_t mora_ck_a
Definition: protocol.c:5
#define GOT_CRC1
Definition: protocol.c:16
void parse_mora(struct mora_transport *t, uint8_t c)
Definition: protocol.c:20
#define UNINIT
Receiving pprz messages.
Definition: protocol.c:11
uint8_t status
Definition: protocol.h:138