diff options
Diffstat (limited to 'src/systemcmds')
| -rw-r--r-- | src/systemcmds/config/config.c | 17 | ||||
| -rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 38 |
2 files changed, 42 insertions, 13 deletions
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 9f13edb18..f54342f06 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -81,7 +81,7 @@ config_main(int argc, char *argv[]) do_device(argc - 1, argv + 1); } } - + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } @@ -192,8 +192,12 @@ do_gyro(int argc, char *argv[]) int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, GYROIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; + + param_get(param_find("SENS_GYRO_ID"), &(calibration_id)); - warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range); + warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); close(fd); } @@ -267,9 +271,10 @@ do_mag(int argc, char *argv[]) int range = ioctl(fd, MAGIOCGRANGE, 0); int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; + param_get(param_find("SENS_MAG_ID"), &(calibration_id)); - warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); + warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); close(fd); } @@ -341,8 +346,12 @@ do_accel(int argc, char *argv[]) int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; + + param_get(param_find("SENS_ACC_ID"), &(calibration_id)); - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); + warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index bbd90b961..3e1f76714 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,7 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; - int32_t mag_devid,mag_calibration_devid; + int32_t devid, calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -98,9 +98,9 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } - mag_devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid)); - if (mag_devid != mag_calibration_devid){ + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(calibration_devid)); + if (devid != calibration_devid){ warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); system_ok = false; @@ -108,7 +108,7 @@ int preflight_check_main(int argc, char *argv[]) } ret = ioctl(fd, MAGIOCSELFTEST, 0); - + if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); @@ -120,8 +120,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_ACC_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, ACCELIOCSELFTEST, 0); - + if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); @@ -156,8 +166,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(GYRO_DEVICE_PATH, 0); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_GYRO_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("gyro calibration is for a different device - calibrate gyro first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, GYROIOCSELFTEST, 0); - + if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); @@ -183,10 +203,10 @@ int preflight_check_main(int argc, char *argv[]) system_ok &= rc_ok; - + system_eval: - + if (system_ok) { /* all good, exit silently */ exit(0); |