diff options
author | Julian Oes <julian@oes.ch> | 2013-06-12 12:24:52 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-06-12 12:24:52 +0200 |
commit | 7f90ebf537f226bc974a9d6023b67a9b32dccfe3 (patch) | |
tree | 7664a08157148ab0429aae7cb1a02575c6ef5c5b /src/systemcmds | |
parent | f5c157e74df12a0cb36b7d27cdad9828d96cc534 (diff) | |
parent | 42ce3112ad645e53788463180c350279b243b02e (diff) | |
download | px4-firmware-7f90ebf537f226bc974a9d6023b67a9b32dccfe3.tar.gz px4-firmware-7f90ebf537f226bc974a9d6023b67a9b32dccfe3.tar.bz2 px4-firmware-7f90ebf537f226bc974a9d6023b67a9b32dccfe3.zip |
Merge remote-tracking branch 'upstream/master' into new_state_machine
Conflicts:
src/examples/fixedwing_control/main.c
Diffstat (limited to 'src/systemcmds')
-rw-r--r-- | src/systemcmds/mixer/mixer.c | 4 | ||||
-rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 101 | ||||
-rw-r--r-- | src/systemcmds/pwm/pwm.c | 6 | ||||
-rw-r--r-- | src/systemcmds/tests/.context | 0 |
4 files changed, 106 insertions, 5 deletions
diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.c index 55c4f0836..e642ed067 100644 --- a/src/systemcmds/mixer/mixer.c +++ b/src/systemcmds/mixer/mixer.c @@ -88,8 +88,8 @@ load(const char *devname, const char *fname) { int dev; FILE *fp; - char line[80]; - char buf[512]; + char line[120]; + char buf[2048]; /* open the device */ if ((dev = open(devname, 0)) < 0) diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 42256be61..4bcce18fb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -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, ¶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(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: diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index ff733df52..e150b5a74 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -185,12 +185,18 @@ pwm_main(int argc, char *argv[]) const char *arg = argv[0]; argv++; if (!strcmp(arg, "arm")) { + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + /* tell IO that the system is armed (it will output values if safety is off) */ ret = ioctl(fd, PWM_SERVO_ARM, 0); if (ret != OK) err(1, "PWM_SERVO_ARM"); continue; } if (!strcmp(arg, "disarm")) { + /* disarm, but do not revoke the SET_ARM_OK flag */ ret = ioctl(fd, PWM_SERVO_DISARM, 0); if (ret != OK) err(1, "PWM_SERVO_DISARM"); diff --git a/src/systemcmds/tests/.context b/src/systemcmds/tests/.context deleted file mode 100644 index e69de29bb..000000000 --- a/src/systemcmds/tests/.context +++ /dev/null |