diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 8 | ||||
-rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 99 |
2 files changed, 96 insertions, 11 deletions
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f21ac6e4c..9698a0f2f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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++; } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 42256be61..7b1c9679e 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -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[]) 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[]) 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[]) 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[]) 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, ¶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("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: |