zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Better preflight check, catches wrong RC configs. Needs rework of mavlink text message API to VARARGs

sbg
Lorenz Meier 12 years ago
parent
0165034e49
commit
88ba97816d
2 changed files with 96 additions and 11 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      src/modules/px4iofirmware/registers.c
  2. 99
      src/systemcmds/preflight_check/preflight_check.c

8
src/modules/px4iofirmware/registers.c
Unescape View File

@ -412,18 +412,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) @@ -412,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++;
}

99
src/systemcmds/preflight_check/preflight_check.c
Unescape View File

@ -45,6 +45,7 @@ @@ -45,6 +45,7 @@
#include <errno.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@ -98,7 +99,7 @@ int preflight_check_main(int argc, char *argv[]) @@ -98,7 +99,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 +112,7 @@ int preflight_check_main(int argc, char *argv[]) @@ -111,7 +112,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 +125,7 @@ int preflight_check_main(int argc, char *argv[]) @@ -124,7 +125,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 +135,98 @@ int preflight_check_main(int argc, char *argv[]) @@ -134,6 +135,98 @@ 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;
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("ERROR: %d config error(s) for RC channel %d.", count, (channel + 1));
usleep(100000);
rc_ok = false;
}
}
/* require RC ok to keep system_ok */
system_ok &= rc_ok;
system_eval:

Write Preview
Loading…
Cancel
Save