forked from Archive/PX4-Autopilot
Added minimum set of IO MAVLink text messages, report critical errors such as in-air restarts
This commit is contained in:
Lorenz Meier
parent
ebac51cad8
commit
e8e52afcc4
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue