|
|
@ -83,6 +83,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or |
|
|
|
|
|
|
|
|
|
|
|
/* Back up the original UART configuration to restore it after exit */ |
|
|
|
/* Back up the original UART configuration to restore it after exit */ |
|
|
|
int termios_state; |
|
|
|
int termios_state; |
|
|
|
|
|
|
|
|
|
|
|
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { |
|
|
|
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { |
|
|
|
warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); |
|
|
|
warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); |
|
|
|
close(uart); |
|
|
|
close(uart); |
|
|
@ -139,6 +140,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) |
|
|
|
argv += 2; |
|
|
|
argv += 2; |
|
|
|
|
|
|
|
|
|
|
|
int ch; |
|
|
|
int ch; |
|
|
|
|
|
|
|
|
|
|
|
while ((ch = getopt(argc, argv, "d:")) != EOF) { |
|
|
|
while ((ch = getopt(argc, argv, "d:")) != EOF) { |
|
|
|
switch (ch) { |
|
|
|
switch (ch) { |
|
|
|
case 'd': |
|
|
|
case 'd': |
|
|
@ -155,8 +157,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) |
|
|
|
struct termios uart_config_original; |
|
|
|
struct termios uart_config_original; |
|
|
|
const int uart = frsky_open_uart(device_name, &uart_config_original); |
|
|
|
const int uart = frsky_open_uart(device_name, &uart_config_original); |
|
|
|
|
|
|
|
|
|
|
|
if (uart < 0) |
|
|
|
if (uart < 0) { |
|
|
|
err(1, "could not open %s", device_name); |
|
|
|
err(1, "could not open %s", device_name); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/* Subscribe to topics */ |
|
|
|
/* Subscribe to topics */ |
|
|
|
frsky_init(); |
|
|
|
frsky_init(); |
|
|
@ -165,6 +168,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) |
|
|
|
|
|
|
|
|
|
|
|
/* Main thread loop */ |
|
|
|
/* Main thread loop */ |
|
|
|
unsigned int iteration = 0; |
|
|
|
unsigned int iteration = 0; |
|
|
|
|
|
|
|
|
|
|
|
while (!thread_should_exit) { |
|
|
|
while (!thread_should_exit) { |
|
|
|
|
|
|
|
|
|
|
|
/* Sleep 200 ms */ |
|
|
|
/* Sleep 200 ms */ |
|
|
@ -174,14 +178,12 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) |
|
|
|
frsky_send_frame1(uart); |
|
|
|
frsky_send_frame1(uart); |
|
|
|
|
|
|
|
|
|
|
|
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ |
|
|
|
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ |
|
|
|
if (iteration % 5 == 0) |
|
|
|
if (iteration % 5 == 0) { |
|
|
|
{ |
|
|
|
|
|
|
|
frsky_send_frame2(uart); |
|
|
|
frsky_send_frame2(uart); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/* Send frame 3 (every 5000ms): date, time */ |
|
|
|
/* Send frame 3 (every 5000ms): date, time */ |
|
|
|
if (iteration % 25 == 0) |
|
|
|
if (iteration % 25 == 0) { |
|
|
|
{ |
|
|
|
|
|
|
|
frsky_send_frame3(uart); |
|
|
|
frsky_send_frame3(uart); |
|
|
|
|
|
|
|
|
|
|
|
iteration = 0; |
|
|
|
iteration = 0; |
|
|
@ -212,8 +214,9 @@ int frsky_telemetry_main(int argc, char *argv[]) |
|
|
|
if (!strcmp(argv[1], "start")) { |
|
|
|
if (!strcmp(argv[1], "start")) { |
|
|
|
|
|
|
|
|
|
|
|
/* this is not an error */ |
|
|
|
/* this is not an error */ |
|
|
|
if (thread_running) |
|
|
|
if (thread_running) { |
|
|
|
errx(0, "frsky_telemetry already running"); |
|
|
|
errx(0, "frsky_telemetry already running"); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
thread_should_exit = false; |
|
|
|
thread_should_exit = false; |
|
|
|
frsky_task = px4_task_spawn_cmd("frsky_telemetry", |
|
|
|
frsky_task = px4_task_spawn_cmd("frsky_telemetry", |
|
|
@ -221,7 +224,7 @@ int frsky_telemetry_main(int argc, char *argv[]) |
|
|
|
SCHED_PRIORITY_DEFAULT, |
|
|
|
SCHED_PRIORITY_DEFAULT, |
|
|
|
2000, |
|
|
|
2000, |
|
|
|
frsky_telemetry_thread_main, |
|
|
|
frsky_telemetry_thread_main, |
|
|
|
(char * const *)argv); |
|
|
|
(char *const *)argv); |
|
|
|
|
|
|
|
|
|
|
|
while (!thread_running) { |
|
|
|
while (!thread_running) { |
|
|
|
usleep(200); |
|
|
|
usleep(200); |
|
|
@ -233,8 +236,9 @@ int frsky_telemetry_main(int argc, char *argv[]) |
|
|
|
if (!strcmp(argv[1], "stop")) { |
|
|
|
if (!strcmp(argv[1], "stop")) { |
|
|
|
|
|
|
|
|
|
|
|
/* this is not an error */ |
|
|
|
/* this is not an error */ |
|
|
|
if (!thread_running) |
|
|
|
if (!thread_running) { |
|
|
|
errx(0, "frsky_telemetry already stopped"); |
|
|
|
errx(0, "frsky_telemetry already stopped"); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
thread_should_exit = true; |
|
|
|
thread_should_exit = true; |
|
|
|
|
|
|
|
|
|
|
|