diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-07-22 14:58:40 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-07-22 14:58:40 +0400 |
commit | 57837eb0b949d07941c7c70dd9e574851eb87a66 (patch) | |
tree | c97f4289810bd86eb1a99a66708a707458c97b54 /src | |
parent | bd50092dd29a83dd5044fc3027e20be91b3275e5 (diff) | |
parent | dc5bcdda761e5f8f4f7f26a600f02df007ab1df6 (diff) | |
download | px4-firmware-57837eb0b949d07941c7c70dd9e574851eb87a66.tar.gz px4-firmware-57837eb0b949d07941c7c70dd9e574851eb87a66.tar.bz2 px4-firmware-57837eb0b949d07941c7c70dd9e574851eb87a66.zip |
Merge commit 'dc5bcdda761e5f8f4f7f26a600f02df007ab1df6' into seatbelt_multirotor
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/boards/px4fmu/px4fmu_init.c | 2 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 3 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 3 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611.cpp | 3 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/kalman_main.cpp | 3 | ||||
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 6 | ||||
-rwxr-xr-x | src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp | 6 | ||||
-rw-r--r-- | src/modules/commander/accelerometer_calibration.c | 17 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink.c | 47 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_log.c | 11 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 21 | ||||
-rw-r--r-- | src/modules/systemlib/module.mk | 3 | ||||
-rw-r--r-- | src/modules/systemlib/system_params.c | 47 | ||||
-rw-r--r-- | src/systemcmds/param/param.c | 73 | ||||
-rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 6 |
15 files changed, 196 insertions, 55 deletions
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 212a92cfa..36af2298c 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void) /* initial LED state */ drv_led_start(); led_off(LED_AMBER); - led_on(LED_BLUE); + led_off(LED_BLUE); /* Configure SPI-based devices */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 59e90d86c..ac3bdc132 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1221,7 +1221,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver, attempt expansion bus first */ g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 220842536..8d9054a38 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1063,7 +1063,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 59ab5936e..c13b65d5a 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -969,7 +969,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MS5611(MS5611_BUS); diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 4befdc879..372b2d162 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -121,12 +121,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("is running\n"); + exit(0); } else { warnx("not started\n"); + exit(1); } - exit(0); } usage("unrecognized command"); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d8b40ac3b..1eff60e88 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_ekf app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_ekf app not started\n"); + warnx("not started"); + exit(1); } exit(0); diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3ca50fb39..107c2dfb1 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_so3_comp app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_so3_comp app not started\n"); + warnx("not started"); + exit(1); } exit(0); diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 48a36ac26..6a304896a 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -268,8 +268,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ float ema_len = 0.2f; - /* set "still" threshold to 0.1 m/s^2 */ - float still_thr2 = pow(0.1f, 2); + /* set "still" threshold to 0.25 m/s^2 */ + float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ @@ -283,6 +283,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { hrt_abstime t = t_start; hrt_abstime t_prev = t_start; hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); @@ -327,12 +330,14 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "ERROR: poll failure"); - return -3; + poll_errcount++; } if (t > t_timeout) { - mavlink_log_info(mavlink_fd, "ERROR: timeout"); + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); return -1; } } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c1c4b175..919d01561 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -420,12 +420,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d\n", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -433,37 +433,35 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf int termios_state; *is_usb = false; - /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } + } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - } else { - *is_usb = true; + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; } return uart; @@ -751,8 +749,7 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); + tcsetattr(uart, TCSANOW, &uart_config_original); thread_running = false; diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c index d9416a08b..192195856 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/mavlink/mavlink_log.c @@ -41,16 +41,20 @@ #include <string.h> #include <stdlib.h> +#include <stdio.h> #include <stdarg.h> #include <mavlink/mavlink_log.h> +static FILE* text_recorder_fd = NULL; + void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); + text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) @@ -82,6 +86,13 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; + + if (text_recorder_fd) { + fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); + fputc("\n", text_recorder_fd); + fsync(text_recorder_fd); + } + return 0; } else { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ae5a55109..5722d2fdf 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -139,14 +139,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ -#if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ /** * Gather and publish PPM input data. */ void ppm_poll(); -#endif /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -393,13 +391,11 @@ namespace sensors #endif static const int ERROR = -1; -Sensors *g_sensors; +Sensors *g_sensors = nullptr; } Sensors::Sensors() : -#ifdef CONFIG_HRT_PPM _ppm_last_valid(0), -#endif _fd_adc(-1), _last_adc(0), @@ -1135,16 +1131,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } -#if CONFIG_HRT_PPM void Sensors::ppm_poll() { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - bool rc_updated; - orb_check(_rc_sub, &rc_updated); + struct pollfd fds[1]; + fds[0].fd = _rc_sub; + fds[0].events = POLLIN; + /* check non-blocking for new data */ + int poll_ret = poll(fds, 1, 0); - if (rc_updated) { + if (poll_ret > 0) { struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1332,7 +1330,6 @@ Sensors::ppm_poll() } } -#endif void Sensors::task_main_trampoline(int argc, char *argv[]) @@ -1445,10 +1442,8 @@ Sensors::task_main() if (_publishing) orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); -#ifdef CONFIG_HRT_PPM /* Look for new r/c input data */ ppm_poll(); -#endif perf_end(_loop_perf); } @@ -1488,7 +1483,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(1, "sensors task already running"); + errx(0, "sensors task already running"); sensors::g_sensors = new Sensors; diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fd0289c9a..b470c1227 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -47,4 +47,5 @@ SRCS = err.c \ pid/pid.c \ geo/geo.c \ systemlib.c \ - airspeed.c + airspeed.c \ + system_params.c diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c new file mode 100644 index 000000000..75be090f8 --- /dev/null +++ b/src/modules/systemlib/system_params.c @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. 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 system_params.c + * + * System wide parameters + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +// Auto-start script with index #n +PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); + +// Automatically configure default values +PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 60e61d07b..40a9297a7 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,6 +63,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val); +static void do_compare(const char* name, const char* val); int param_main(int argc, char *argv[]) @@ -117,9 +118,17 @@ param_main(int argc, char *argv[]) errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); } } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 4) { + do_compare(argv[2], argv[3]); + } else { + errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); + } + } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); } static void @@ -295,3 +304,65 @@ do_set(const char* name, const char* val) exit(0); } + +static void +do_compare(const char* name, const char* val) +{ + int32_t i; + float f; + param_t param = param_find(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found */ + errx(1, "Error: Parameter %s not found.", name); + } + + /* + * Set parameter if type is known and conversion from string to value turns out fine + */ + + int ret = 1; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + + /* convert string */ + char* end; + int j = strtol(val,&end,10); + if (i == j) { + printf(" %d: ", i); + ret = 0; + } + + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + + /* convert string */ + char* end; + float g = strtod(val, &end); + if (fabsf(f - g) < 1e-7f) { + printf(" %4.4f: ", (double)f); + ret = 0; + } + } + + break; + + default: + errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param)); + } + + if (ret == 0) { + printf("%c %s: equal\n", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + } + + exit(ret); +} diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 7752ffe67..d1dd85d47 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -135,6 +135,7 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(BARO_DEVICE_PATH, 0); + close(fd); /* ---- RC CALIBRATION ---- */ @@ -251,6 +252,11 @@ system_eval: int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); + if (leds < 0) { + close(buzzer); + errx(1, "failed to open leds, aborting"); + } + /* flip blue led into alternating amber */ led_off(leds, LED_BLUE); led_off(leds, LED_AMBER); |