dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Changed mavlink and sensors apps to deamons, now started with mavlink start and sensors start.

This commit is contained in:
Lorenz Meier 2012-08-19 01:26:42 +02:00
parent 7874bbd37f
commit 9a56be6907
2 changed files with 120 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
apps/mavlink/mavlink.c
View File

@ -78,6 +78,11 @@
__EXPORT int mavlink_main(int argc, char *argv[]);
int mavlink_thread_main(int argc, char *argv[]);
static bool thread_should_exit = false;
static bool thread_running = false;
static int mavlink_task;
/* terminate MAVLink on user request - disabled by default */
static bool mavlink_link_termination_allowed = false;
@ -170,6 +175,11 @@ mavlink_wpm_storage *wpm;
#include "mavlink_parameters.h"
/**
* Print the usage
*/
static void usage(const char *reason);
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
void mavlink_missionlib_send_message(mavlink_message_t *msg)
@ -1194,7 +1204,7 @@ int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_
/**
* MAVLink Protocol main function.
*/
int mavlink_main(int argc, char *argv[])
int mavlink_thread_main(int argc, char *argv[])
{
wpm = &wpm_s;
@ -1218,46 +1228,38 @@ int mavlink_main(int argc, char *argv[])
/* default values for arguments */
char *uart_name = "/dev/ttyS0";
int baudrate = 57600;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
/* read program arguments */
int i;
for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
}
/* UART device ID */
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
usage("");
return 0;
} else if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
uart_name = argv[i + 1];
i++;
} else {
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
usage("missing argument for device (-d)");
return 1;
}
}
/* baud rate */
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
} else if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
if (argc > i + 1) {
baudrate = atoi(argv[i + 1]);
i++;
} else {
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
usage("missing argument for baud rate (-b)");
return 1;
}
}
/* terminating MAVLink is allowed - yes/no */
if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
} else if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
mavlink_link_termination_allowed = true;
}
if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
} else if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
} else {
usage("out of order or invalid argument");
return 1;
}
}
@ -1392,7 +1394,54 @@ int mavlink_main(int argc, char *argv[])
fflush(stdout);
fflush(stderr);
thread_running = false;
return 0;
}
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: mavlink {start|stop|status} [-d <devicename>] [-b <baudrate>] [-e/--exit-allowed]\n\n");
exit(1);
}
int mavlink_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("mavlink already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4096, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmavlink app is running\n");
} else {
printf("\tmavlink app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}

52
apps/sensors/sensors.c
View File

@ -105,11 +105,14 @@ static pthread_mutex_t sensors_read_ready_mutex;
static int sensors_timer_loop_counter = 0;
/* File descriptors for all sensors */
static int fd_gyro = -1;
static int fd_accelerometer = -1;
static int fd_magnetometer = -1;
static int fd_barometer = -1;
static int fd_adc = -1;
static int fd_gyro = -1;
static int fd_accelerometer = -1;
static int fd_magnetometer = -1;
static int fd_barometer = -1;
static int fd_adc = -1;
static bool thread_should_exit = false;
static int sensors_task;
/* Private functions declared static */
static void sensors_timer_loop(void *arg);
@ -243,7 +246,7 @@ static void sensors_timer_loop(void *arg)
pthread_cond_broadcast(&sensors_read_ready);
}
int sensors_main(int argc, char *argv[])
int sensors_thread_main(int argc, char *argv[])
{
/* inform about start */
printf("[sensors] Initializing..\n");
@ -922,3 +925,40 @@ int sensors_main(int argc, char *argv[])
return ret;
}
/**
* Print the usage
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: sensors {start|stop}\n");
exit(1);
}
int sensors_main(int argc, char *argv[])
{
int ch;
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
thread_should_exit = false;
sensors_task = task_create("sensors", SCHED_PRIORITY_MAX - 5, 4096, sensors_thread_main, NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
usage("unrecognized command");
exit(1);
}