yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

clean up structure

Browse Source
This commit is contained in:
Mark Whitehorn
2016-01-23 08:59:17 -07:00
committed by Lorenz Meier
parent 42f9079683
commit 29d759768e
1 changed files with 50 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
src/drivers/frsky_telemetry/frsky_telemetry.c
View File

@@ -193,56 +193,13 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* Main thread loop */
char sbuf[20];
/* look for polling frames indicating SmartPort telemetry
/* look for polling bytes indicating SmartPort telemetry
* if not found, send D type telemetry frames instead
*/
int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000);
if (status < 1) {
/* timed out: reconfigure UART and send D type telemetry */
warnx("SmartPort receiver not detected, sending FrSky D type telemetry");
status = set_uart_speed(uart, &uart_config, B9600);
if (status < 0) {
warnx("error setting speed for %s, quitting", device_name);
/* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original);
close(uart);
thread_running = false;
return 0;
}
int iteration = 0;
/* Subscribe to topics */
frsky_init();
while (!thread_should_exit) {
/* Sleep 200 ms */
usleep(200000);
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
frsky_send_frame1(uart);
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
if (iteration % 5 == 0) {
frsky_send_frame2(uart);
}
/* Send frame 3 (every 5000ms): date, time */
if (iteration % 25 == 0) {
frsky_send_frame3(uart);
iteration = 0;
}
iteration++;
}
} else {
if (status > 0) {
/* traffic on the port, assume it's a SmartPort master */
/* Subscribe to topics */
sPort_init();
@@ -340,12 +297,57 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
}
break;
}
}
} else {
/* timed out: reconfigure UART and send D type telemetry */
warnx("SmartPort receiver not detected, sending FrSky D type telemetry");
status = set_uart_speed(uart, &uart_config, B9600);
if (status < 0) {
warnx("error setting speed for %s, quitting", device_name);
/* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original);
close(uart);
thread_running = false;
return 0;
}
int iteration = 0;
/* Subscribe to topics */
frsky_init();
/* send D8 mode telemetry */
while (!thread_should_exit) {
/* Sleep 200 ms */
usleep(200000);
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
frsky_send_frame1(uart);
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
if (iteration % 5 == 0) {
frsky_send_frame2(uart);
}
/* TODO: flush the input buffer if in full duplex mode */
read(uart, &sbuf[0], sizeof(sbuf));
/* Send frame 3 (every 5000ms): date, time */
if (iteration % 25 == 0) {
frsky_send_frame3(uart);
iteration = 0;
}
iteration++;
}
/* TODO: flush the input buffer if in full duplex mode */
read(uart, &sbuf[0], sizeof(sbuf));
}
/* Reset the UART flags to original state */