|
|
|
@ -289,7 +289,7 @@ void frsky_send_frame3(int uart)
@@ -289,7 +289,7 @@ void frsky_send_frame3(int uart)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* parse 11 byte frames */ |
|
|
|
|
bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v) |
|
|
|
|
bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v) |
|
|
|
|
{ |
|
|
|
|
bool data_ready = false; |
|
|
|
|
static int dcount = 0; |
|
|
|
@ -302,36 +302,46 @@ bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v)
@@ -302,36 +302,46 @@ bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v)
|
|
|
|
|
TRAILER |
|
|
|
|
} state = HEADER; |
|
|
|
|
|
|
|
|
|
for (int i=0; i<nbytes; i++) { |
|
|
|
|
for (int i = 0; i < nbytes; i++) { |
|
|
|
|
switch (state) { |
|
|
|
|
case HEADER: |
|
|
|
|
if (sbuf[i] == 0x7E) { |
|
|
|
|
state = TYPE; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case TYPE: |
|
|
|
|
if (sbuf[i] != 0x7E) { |
|
|
|
|
state = DATA; |
|
|
|
|
type = sbuf[i]; |
|
|
|
|
dcount = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case DATA: |
|
|
|
|
|
|
|
|
|
/* read 8 data bytes */ |
|
|
|
|
if (dcount < 7) { |
|
|
|
|
data[dcount++] = sbuf[i]; |
|
|
|
|
} |
|
|
|
|
else { |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
/* received all data bytes */ |
|
|
|
|
state = TRAILER; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case TRAILER: |
|
|
|
|
state = HEADER; |
|
|
|
|
|
|
|
|
|
if (sbuf[i] != 0x7E) { |
|
|
|
|
warnx("host packet error: %x", sbuf[i]); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
data_ready = true; |
|
|
|
|
|
|
|
|
|
if (type == 0xFE) { |
|
|
|
|
/* this is an adc_linkquality packet */ |
|
|
|
|
v->ad1 = data[0]; |
|
|
|
@ -339,9 +349,11 @@ bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v)
@@ -339,9 +349,11 @@ bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v)
|
|
|
|
|
v->linkq = data[2]; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return data_ready; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|