diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-07 20:51:33 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-07 20:51:33 +0100 |
commit | e8e52afcc426491f5959e6f879f26c9211a88d4c (patch) | |
tree | fe763c133488fb0070fe57c22250c6f28250f9ea | |
parent | ebac51cad8e144b64938e6726e26bdc23aaf45e5 (diff) | |
download | px4-firmware-e8e52afcc426491f5959e6f879f26c9211a88d4c.tar.gz px4-firmware-e8e52afcc426491f5959e6f879f26c9211a88d4c.tar.bz2 px4-firmware-e8e52afcc426491f5959e6f879f26c9211a88d4c.zip |
Added minimum set of IO MAVLink text messages, report critical errors such as in-air restarts
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 19 | ||||
-rw-r--r-- | apps/mavlink/mavlink_log.h | 12 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 8 |
3 files changed, 35 insertions, 4 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 882ca9fed..f9ffa6bcd 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -82,6 +82,7 @@ #include <uORB/topics/parameter_update.h> #include <px4io/protocol.h> +#include <mavlink/mavlink_log.h> #include "uploader.h" #include <debug.h> @@ -112,6 +113,8 @@ private: volatile int _task; ///< worker task volatile bool _task_should_exit; + int _mavlink_fd; + perf_counter_t _perf_update; /* cached IO state */ @@ -286,6 +289,7 @@ PX4IO::PX4IO() : _update_interval(0), _task(-1), _task_should_exit(false), + _mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), @@ -302,6 +306,9 @@ PX4IO::PX4IO() : /* we need this potentially before it could be set in task_main */ g_dev = this; + /* open MAVLink text channel */ + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _debug_enabled = true; } @@ -355,6 +362,7 @@ PX4IO::init() (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -381,6 +389,8 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO * remains untouched (so manual override is still available). @@ -470,6 +480,7 @@ PX4IO::init() ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); return ret; } @@ -491,6 +502,8 @@ PX4IO::init() return -errno; } + mavlink_log_info(_mavlink_fd, "[IO] init ok"); + return OK; } @@ -1165,9 +1178,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { debug("mixer upload OK"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; } else { debug("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } /* load must have failed for some reason */ @@ -1484,7 +1499,7 @@ test(void) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); - if (ret != sizeof(servos)) + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { @@ -1567,7 +1582,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (argc > 2 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { if (argc > 2 + 1) { #warning implement this } else { diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 62e6f7ca0..233a76cb3 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -63,7 +63,11 @@ * @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 /** * Send a mavlink critical message. @@ -71,7 +75,11 @@ * @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 /** * Send a mavlink info message. @@ -79,7 +87,11 @@ * @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 struct mavlink_logmessage { char text[51]; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 589264661..0c4838523 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -215,12 +215,16 @@ user_start(int argc, char *argv[]) /* post debug state at ~1Hz */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", + + struct mallinfo minfo = mallinfo(); + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets); + (unsigned)i2c_loop_resets, + (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } } |