diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-21 01:27:22 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-21 01:27:22 -0700 |
commit | 7392ad999232a6902c8accb90dbbd7290ddac6ee (patch) | |
tree | b0cc83a57eb039f454576628153b72809eff536c | |
parent | bc3eca5df3a980cfcc0cc6f03926e1c9f9c24a39 (diff) | |
parent | 5d9512eb799c86fc674ee9d610f12273f2e97e62 (diff) | |
download | px4-firmware-7392ad999232a6902c8accb90dbbd7290ddac6ee.tar.gz px4-firmware-7392ad999232a6902c8accb90dbbd7290ddac6ee.tar.bz2 px4-firmware-7392ad999232a6902c8accb90dbbd7290ddac6ee.zip |
Merge pull request #277 from PX4/preflight_check
Preflight check improvements
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 3 | ||||
-rw-r--r-- | src/include/mavlink/mavlink_log.h | 45 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_log.c | 29 | ||||
-rw-r--r-- | src/modules/px4iofirmware/controls.c | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 1 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 9 | ||||
-rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 101 |
7 files changed, 158 insertions, 33 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a40142792..8b2fae12b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1273,6 +1273,9 @@ PX4IO::print_status() ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); + /* now clear alarms */ + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); + printf("vbatt %u ibatt %u vbatt scale %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 233a76cb3..a28ff3a68 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -47,27 +47,42 @@ */ #include <sys/ioctl.h> -/* +/** * The mavlink log device node; must be opened before messages * can be logged. */ #define MAVLINK_LOG_DEVICE "/dev/mavlink" +/** + * The maximum string length supported. + */ +#define MAVLINK_LOG_MAXLEN 50 #define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1) #define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2) #define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3) +#ifdef __cplusplus +extern "C" { +#endif +__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); +#ifdef __cplusplus +} +#endif + +/* + * The va_args implementation here is not beautiful, but obviously we run into the same issues + * the GCC devs saw, and are using their solution: + * + * http://gcc.gnu.org/onlinedocs/cpp/Variadic-Macros.html + */ + /** * Send a mavlink emergency message. * * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#ifdef __cplusplus -#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); -#else -#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); -#endif +#define mavlink_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); /** * Send a mavlink critical message. @@ -75,11 +90,7 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#ifdef __cplusplus -#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); -#else -#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); -#endif +#define mavlink_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); /** * Send a mavlink info message. @@ -87,14 +98,10 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#ifdef __cplusplus -#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); -#else -#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); -#endif +#define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); struct mavlink_logmessage { - char text[51]; + char text[MAVLINK_LOG_MAXLEN + 1]; unsigned char severity; }; @@ -116,5 +123,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); +void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...); + #endif diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c index fa974dc0b..d9416a08b 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/mavlink/mavlink_log.c @@ -41,6 +41,7 @@ #include <string.h> #include <stdlib.h> +#include <stdarg.h> #include <mavlink/mavlink_log.h> @@ -87,3 +88,31 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa return 1; } } + +void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + int end = (lb->start + lb->count) % lb->size; + lb->elems[end].severity = severity; + vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap); + va_end(ap); + + /* increase count */ + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } +} + +__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + char text[MAVLINK_LOG_MAXLEN + 1]; + vsnprintf(text, sizeof(text), fmt, ap); + va_end(ap); + ioctl(_fd, severity, (unsigned long)&text[0]); +} diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index dc36f6c93..3cf9ca149 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -217,9 +217,8 @@ controls_tick() { if (assigned_channels == 0) { rc_input_lost = true; } else { - /* set RC OK flag and clear RC lost alarm */ + /* set RC OK flag */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; } /* diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 5ada1b220..1234e2eea 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -100,7 +100,6 @@ mixer_tick(void) } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; } source = MIX_FAILSAFE; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 61049c8b6..9698a0f2f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -203,7 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; break; @@ -413,18 +412,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { count++; } - /* assert deadzone is sane */ if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - // The following check isn't needed as constraint checks in controls.c will catch this. - //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - // count++; - //} - //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - // count++; - //} if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { count++; } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 42256be61..4bcce18fb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -41,10 +41,12 @@ #include <unistd.h> #include <stdlib.h> #include <stdio.h> +#include <string.h> #include <fcntl.h> #include <errno.h> #include <systemlib/err.h> +#include <systemlib/param/param.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -98,7 +100,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); system_ok = false; goto system_eval; } @@ -111,7 +113,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("accel self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); system_ok = false; goto system_eval; } @@ -124,7 +126,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("gyro self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); system_ok = false; goto system_eval; } @@ -134,6 +136,99 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(BARO_DEVICE_PATH, 0); + /* ---- RC CALIBRATION ---- */ + + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + float param_min, param_max, param_trim, param_rev, param_dz; + + bool rc_ok = true; + char nbuf[20]; + + for (int i = 0; i < 12; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* XXX needs inspection of all the _MAP params */ + // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + rc_ok = false; + } + } + + /* require RC ok to keep system_ok */ + system_ok &= rc_ok; + + system_eval: |