diff --git a/makefiles/linux.mk b/makefiles/linux.mk index b424521a4b..d050259c57 100644 --- a/makefiles/linux.mk +++ b/makefiles/linux.mk @@ -34,5 +34,7 @@ # building firmware. # -MODULES += platforms/linux/px4_layer +MODULES += \ + platforms/common \ + platforms/linux/px4_layer diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 4ee61a1277..9e99d926e2 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -1194,21 +1195,6 @@ usage() } // namespace -static int getopt(int argc, char *argv[], const char *options, int *myoptind) -{ - char *p = argv[*myoptind]; - int idx = 0; - if (p && options && myoptind && p[0] == '-') { - while (options[idx] != 0 && p[1] != options[idx]) - ++idx; - if (options[idx] == 0) - return (int)'?'; - *myoptind += 1; - return options[idx]; - } - return -1; -} - int ms5611_main(int argc, char *argv[]) { @@ -1223,7 +1209,8 @@ ms5611_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; - while ((ch = getopt(argc, argv, "XIS", &myoptind)) != EOF) { + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) { printf("ch = %d\n", ch); switch (ch) { case 'X': diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index 3a5684110b..e9bb7b1308 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -1199,38 +1200,36 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = 0; _mode = MAVLINK_MODE_NORMAL; - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; + int myoptind=1; + const char *myoptarg = NULL; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': - _baudrate = strtoul(optarg, NULL, 10); + _baudrate = strtoul(myoptarg, NULL, 10); if (_baudrate < 9600 || _baudrate > 921600) { - warnx("invalid baud rate '%s'", optarg); + warnx("invalid baud rate '%s'", myoptarg); err_flag = true; } break; case 'r': - _datarate = strtoul(optarg, NULL, 10); + _datarate = strtoul(myoptarg, NULL, 10); if (_datarate < 10 || _datarate > MAX_DATA_RATE) { - warnx("invalid data rate '%s'", optarg); + warnx("invalid data rate '%s'", myoptarg); err_flag = true; } break; case 'd': - _device_name = optarg; + _device_name = myoptarg; break; // case 'e': @@ -1238,13 +1237,13 @@ Mavlink::task_main(int argc, char *argv[]) // break; case 'm': - if (strcmp(optarg, "custom") == 0) { + if (strcmp(myoptarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; - } else if (strcmp(optarg, "camera") == 0) { + } else if (strcmp(myoptarg, "camera") == 0) { // left in here for compatibility _mode = MAVLINK_MODE_ONBOARD; - } else if (strcmp(optarg, "onboard") == 0) { + } else if (strcmp(myoptarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; } diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk new file mode 100644 index 0000000000..0e5f463e69 --- /dev/null +++ b/src/platforms/common/module.mk @@ -0,0 +1,6 @@ +# +# Common OS porting APIs +# + +SRCS = px4_getopt.c + diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c new file mode 100644 index 0000000000..3078351727 --- /dev/null +++ b/src/platforms/common/px4_getopt.c @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_getopt.cpp + * Minimal, thread safe version of getopt + */ + +#include +#include + +// check if p is a valid option and if the option takes an arg +static char isvalidopt(char p, const char *options, int *takesarg) +{ + int idx = 0; + *takesarg = 0; + while (options[idx] != 0 && p != options[idx]) + ++idx; + if (options[idx] == 0) + return '?'; + if (options[idx+1] == ':') { + *takesarg = 1; + } + return options[idx]; +} + +// reorder argv and put non-options at the end +static int reorder(int argc, char **argv, const char *options) +{ + char *tmp_argv[argc]; + char c; + int idx = 1; + int tmpidx = 1; + int takesarg; + + // move the options to the front + while (idx < argc && argv[idx] != 0) { + if (argv[idx][0] == '-') { + c = isvalidopt(argv[idx][1], options, &takesarg); + if (c == '?') + return 1; + tmp_argv[tmpidx] = argv[idx]; + tmpidx++; + if (takesarg) { + tmp_argv[tmpidx] = argv[idx+1]; + printf("tmp_argv[%d] = %s\n", tmpidx, tmp_argv[tmpidx]); + tmpidx++; + idx++; + } + } + idx++; + } + + // Add non-options to the end + idx = 1; + while (idx < argc && argv[idx] != 0) { + if (argv[idx][0] == '-') { + c = isvalidopt(argv[idx][1], options, &takesarg); + if (c == '?') + return c; + if (takesarg) { + idx++; + } + } + else { + tmp_argv[tmpidx] = argv[idx]; + tmpidx++; + } + idx++; + } + + // Reorder argv + for (idx=1; idx < argc; idx++) { + argv[idx] = tmp_argv[idx]; + } + + return 0; +} + +// +// px4_getopt +// +// returns: +// the valid option character +// '?' if any option is unknown +// -1 if no remaining options +// +// If the option takes an arg, myoptarg will be updated accordingly. +// After each call to px4_getopt, myoptind in incremented to the next +// unparsed arg index. +// Argv is changed to put all options and option args at the beginning, +// followed by non-options. +// +int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const char **myoptarg) +{ + char *p; + char c; + int takesarg; + + if (*myoptind == 1) + if (reorder(argc, argv, options) != 0) + return (int)'?'; + + p = argv[*myoptind]; + + if (*myoptarg == 0) + *myoptarg = argv[*myoptind]; + + if (p && options && myoptind && p[0] == '-') { + c = isvalidopt(p[1], options, &takesarg); + if (c == '?') + return (int)c; + *myoptind += 1; + if (takesarg) { + *myoptarg = argv[*myoptind]; + *myoptind += 1; + } + return (int)c; + } + return -1; +} + diff --git a/src/platforms/linux/drivers/accel/accel.cpp b/src/platforms/linux/drivers/accel/accel.cpp index 7e97d77b26..6efb4772cb 100644 --- a/src/platforms/linux/drivers/accel/accel.cpp +++ b/src/platforms/linux/drivers/accel/accel.cpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include @@ -496,7 +496,7 @@ ACCELSIM::init() _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); - if (_accel_topic < 0) { + if (_accel_topic == (orb_advert_t)(-1)) { warnx("ADVERT ERR"); } @@ -1069,7 +1069,12 @@ ACCELSIM::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + + // The first call to measure() is from init() and _accel_topic is not + // yet initialized + if (_accel_topic != (orb_advert_t)(-1)) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } } _accel_read++; @@ -1332,12 +1337,14 @@ accel_main(int argc, char *argv[]) int ch; enum Rotation rotation = ROTATION_NONE; int ret; + int myoptind = 1; + const char * myoptarg = NULL; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "R:")) != EOF) { + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': - rotation = (enum Rotation)atoi(optarg); + rotation = (enum Rotation)atoi(myoptarg); break; default: accel::usage(); @@ -1345,7 +1352,7 @@ accel_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + const char *verb = argv[myoptind]; /* * Start/load the driver. diff --git a/src/platforms/px4_getopt.h b/src/platforms/px4_getopt.h new file mode 100644 index 0000000000..7c85ef32d2 --- /dev/null +++ b/src/platforms/px4_getopt.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_getopt.h + * Thread safe version of getopt + */ + +#pragma once + +__BEGIN_DECLS + +int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const char **myoptarg); + +__END_DECLS +