fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 01:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix format

Browse Source
This commit is contained in:
Mark Whitehorn
2016-02-10 09:21:52 +01:00
committed by Lorenz Meier
parent de39b63402
commit e25b26e2a8
3 changed files with 19 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/frsky_telemetry/frsky_data.c
+16 -4
View File
@@ -289,7 +289,7 @@ void frsky_send_frame3(int uart)
} }
/* parse 11 byte frames */ /* 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; bool data_ready = false;
static int dcount = 0; static int dcount = 0;
@@ -302,36 +302,46 @@ bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v)
TRAILER TRAILER
} state = HEADER; } state = HEADER;
for (int i=0; i<nbytes; i++) { for (int i = 0; i < nbytes; i++) {
switch (state) { switch (state) {
case HEADER: case HEADER:
if (sbuf[i] == 0x7E) { if (sbuf[i] == 0x7E) {
state = TYPE; state = TYPE;
} }
break; break;
case TYPE: case TYPE:
if (sbuf[i] != 0x7E) { if (sbuf[i] != 0x7E) {
state = DATA; state = DATA;
type = sbuf[i]; type = sbuf[i];
dcount = 0; dcount = 0;
} }
break; break;
case DATA: case DATA:
/* read 8 data bytes */ /* read 8 data bytes */
if (dcount < 7) { if (dcount < 7) {
data[dcount++] = sbuf[i]; data[dcount++] = sbuf[i];
}
else { } else {
/* received all data bytes */ /* received all data bytes */
state = TRAILER; state = TRAILER;
} }
break; break;
case TRAILER: case TRAILER:
state = HEADER; state = HEADER;
if (sbuf[i] != 0x7E) { if (sbuf[i] != 0x7E) {
warnx("host packet error: %x", sbuf[i]); warnx("host packet error: %x", sbuf[i]);
} else { } else {
data_ready = true; data_ready = true;
if (type == 0xFE) { if (type == 0xFE) {
/* this is an adc_linkquality packet */ /* this is an adc_linkquality packet */
v->ad1 = data[0]; v->ad1 = data[0];
@@ -339,9 +349,11 @@ bool frsky_parse_host(uint8_t * sbuf, int nbytes, struct adc_linkquality * v)
v->linkq = data[2]; v->linkq = data[2];
} }
} }
break; break;
} }
} }
return data_ready; return data_ready;
} }
src/drivers/frsky_telemetry/frsky_data.h
+1 -1
View File
@@ -55,6 +55,6 @@ struct adc_linkquality {
uint8_t ad2; uint8_t ad2;
uint8_t linkq; uint8_t linkq;
}; };
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);
#endif /* _FRSKY_TELEMETRY_H */ #endif /* _FRSKY_TELEMETRY_H */
src/drivers/frsky_telemetry/frsky_telemetry.c
+2 -1
View File
@@ -344,9 +344,10 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* parse incoming data */ /* parse incoming data */
int nbytes = read(uart, &dbuf[0], sizeof(dbuf)); int nbytes = read(uart, &dbuf[0], sizeof(dbuf));
bool new_input = frsky_parse_host(&dbuf[0], nbytes, &host_frame); bool new_input = frsky_parse_host(&dbuf[0], nbytes, &host_frame);
if (new_input) { if (new_input) {
warnx("host frame: ad1:%u, ad2: %u, rssi: %u", warnx("host frame: ad1:%u, ad2: %u, rssi: %u",
host_frame.ad1, host_frame.ad2, host_frame.linkq); host_frame.ad1, host_frame.ad2, host_frame.linkq);
} }
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */