fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 16:24:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #277 from PX4/preflight_check

Browse Source 
Preflight check improvements
This commit is contained in:
Lorenz Meier 2013-05-21 01:27:22 -07:00
commit 7392ad9992
7 changed files with 158 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/px4io/px4io.cpp
View File

@ -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),

45
src/include/mavlink/mavlink_log.h
View File

@ -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

29
src/modules/mavlink/mavlink_log.c
View File

@ -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]);
}

3
src/modules/px4iofirmware/controls.c
View File

@ -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;
}
/*

1
src/modules/px4iofirmware/mixer.cpp
View File

@ -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;

9
src/modules/px4iofirmware/registers.c
View File

@ -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++;
}

101
src/systemcmds/preflight_check/preflight_check.c
View File

@ -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, &param_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, &param_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, &param_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, &param_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, &param_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: