fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

frsky_telemetry cleanup: remove err, use px4_getopt

Browse Source
This commit is contained in:
Beat Küng 2018-07-07 09:00:58 +02:00 committed by Lorenz Meier
parent 84770f195d
commit 898eb95623
1 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp
View File

@ -58,6 +58,7 @@
#include <px4_tasks.h>
#include <px4_module.h>
#include <px4_getopt.h>
#include <systemlib/err.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
@ -135,14 +136,15 @@ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, s
const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (uart < 0) {
err(1, "Error opening port: %s", uart_name);
PX4_ERR("Error opening port: %s (%i)", uart_name, errno);
return -1;
}
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
PX4_ERR("tcgetattr %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
@ -168,13 +170,13 @@ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, s
const speed_t speed = B9600;
if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) {
warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
PX4_ERR("%s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) {
warnx("ERR: %s (tcsetattr)\n", uart_name);
PX4_ERR("%s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
@ -208,7 +210,6 @@ static void usage()
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS6", "<file:dev>", "Select Serial Device", true);
PRINT_MODULE_USAGE_COMMAND("stop");
PRINT_MODULE_USAGE_COMMAND("status");
exit(1);
}
/**
@ -216,22 +217,21 @@ static void usage()
*/
static int frsky_telemetry_thread_main(int argc, char *argv[])
{
/* Work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
device_name = "/dev/ttyS6"; /* default USART8 */
while ((ch = getopt(argc, argv, "d:")) != EOF) {
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'd':
device_name = optarg;
device_name = myoptarg;
break;
default:
usage();
return -1;
break;
}
}
@ -242,9 +242,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
if (uart < 0) {
warnx("could not open %s", device_name);
err(1, "could not open %s", device_name);
device_name = NULL;
return -1;
}
/* poll descriptor */
@ -331,7 +330,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
if (frsky_state == SPORT) {
/* Subscribe to topics */
if (!sPort_init()) {
err(1, "could not allocate memory");
PX4_ERR("could not allocate memory");
return -1;
}
PX4_INFO("sending FrSky SmartPort telemetry");
@ -590,7 +590,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* Subscribe to topics */
if (!frsky_init()) {
err(1, "could not allocate memory");
PX4_ERR("could not allocate memory");
return -1;
}
struct adc_linkquality host_frame;
@ -663,15 +664,16 @@ int frsky_telemetry_main(int argc, char *argv[])
if (argc < 2) {
warnx("missing command");
PX4_ERR("missing command");
usage();
return -1;
}
if (!strcmp(argv[1], "start")) {
/* this is not an error */
if (thread_running) {
errx(0, "frsky_telemetry already running");
PX4_INFO("frsky_telemetry already running");
return 0;
}
thread_should_exit = false;
@ -686,26 +688,26 @@ int frsky_telemetry_main(int argc, char *argv[])
usleep(200);
}
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running) {
errx(0, "frsky_telemetry already stopped");
PX4_WARN("frsky_telemetry already stopped");
return 0;
}
thread_should_exit = true;
while (thread_running) {
usleep(1000000);
warnx(".");
PX4_INFO(".");
}
warnx("terminated.");
PX4_INFO("terminated.");
device_name = NULL;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
@ -715,32 +717,30 @@ int frsky_telemetry_main(int argc, char *argv[])
case SCANNING:
PX4_INFO("running: SCANNING");
PX4_INFO("port: %s", device_name);
return 0;
break;
case SPORT:
PX4_INFO("running: SPORT");
PX4_INFO("port: %s", device_name);
PX4_INFO("packets sent: %d", sentPackets);
return 0;
break;
case DTYPE:
PX4_INFO("running: DTYPE");
PX4_INFO("port: %s", device_name);
PX4_INFO("packets sent: %d", sentPackets);
return 0;
break;
}
return 0;
} else {
PX4_INFO("not running");
return 0;
}
}
warnx("unrecognized command");
PX4_ERR("unrecognized command");
usage();
/* not getting here */
return 0;
}