diff options
| -rw-r--r-- | src/drivers/drv_mag.h | 6 | ||||
| -rw-r--r-- | src/drivers/drv_sensor.h | 18 | ||||
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 16 | ||||
| -rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 32 | ||||
| -rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 56 | ||||
| -rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 8 | ||||
| -rw-r--r-- | src/modules/commander/gyro_calibration.cpp | 7 | ||||
| -rw-r--r-- | src/modules/sensors/sensor_params.c | 15 | ||||
| -rw-r--r-- | src/systemcmds/config/config.c | 17 | ||||
| -rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 38 |
10 files changed, 156 insertions, 57 deletions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index d8fe1ae7a..193c816e0 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -41,7 +41,6 @@ #include <stdint.h> #include <sys/ioctl.h> -#include "drv_device.h" #include "drv_sensor.h" #include "drv_orb_dev.h" @@ -83,11 +82,6 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag); -/* - * mag device types, for _device_id - */ -#define DRV_MAG_DEVTYPE_HMC5883 1 -#define DRV_MAG_DEVTYPE_LSM303D 2 /* * ioctl() definitions diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 5e4598de8..467dace08 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -43,6 +43,24 @@ #include <stdint.h> #include <sys/ioctl.h> +#include "drv_device.h" + +/** + * Sensor type definitions. + * + * Used to create a unique device id for redundant and combo sensors + */ + +#define DRV_MAG_DEVTYPE_HMC5883 0x01 +#define DRV_MAG_DEVTYPE_LSM303D 0x02 +#define DRV_ACC_DEVTYPE_LSM303D 0x11 +#define DRV_ACC_DEVTYPE_BMA180 0x12 +#define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_GYR_DEVTYPE_MPU6000 0x21 +#define DRV_GYR_DEVTYPE_L3GD20 0x22 +#define DRV_RNG_DEVTYPE_MB12XX 0x31 +#define DRV_RNG_DEVTYPE_LL40LS 0x32 + /* * ioctl() definitions * diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index bd1bd9f86..f583bced4 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -220,7 +220,7 @@ private: struct hrt_call _call; unsigned _call_interval; - + RingBuffer *_reports; struct gyro_scale _gyro_scale; @@ -424,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati // enable debug() calls _debug_enabled = true; + _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20; + // default scale factors _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; @@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -867,7 +869,7 @@ L3GD20::reset() disable_i2c(); /* set default configuration */ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ @@ -911,7 +913,7 @@ L3GD20::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -974,7 +976,7 @@ L3GD20::measure() we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort */ - perf_count(_errors); + perf_count(_errors); return; } #endif @@ -994,7 +996,7 @@ L3GD20::measure() */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_bad_registers); - + switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: @@ -1072,7 +1074,7 @@ L3GD20::print_info() for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) { uint8_t v = read_reg(_checked_registers[i]); if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", + ::printf("reg %02x:%02x should be %02x\n", (unsigned)_checked_registers[i], (unsigned)v, (unsigned)_checked_values[i]); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ff7068936..2a0bf522b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -301,7 +301,7 @@ private: enum Rotation _rotation; - // values used to + // values used to float _last_accel[3]; uint8_t _constant_accel_count; @@ -569,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _constant_accel_count(0), _checked_next(0) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + // enable debug() calls _debug_enabled = true; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D; + + /* Prime _mag with parents devid. */ + _mag->_device_id.devid = _device_id.devid; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + + // default scale factors _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; @@ -660,6 +667,7 @@ LSM303D::init() warnx("ADVERT ERR"); } + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ @@ -698,7 +706,7 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ @@ -732,7 +740,7 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - + if (success) { _checked_values[0] = WHO_I_AM; return OK; @@ -1005,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_mag_interval; - + case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) @@ -1410,7 +1418,7 @@ LSM303D::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1534,7 +1542,7 @@ LSM303D::measure() perf_count(_bad_values); _constant_accel_count = 0; } - + _last_accel[0] = x_in_new; _last_accel[1] = y_in_new; _last_accel[2] = z_in_new; @@ -1652,7 +1660,7 @@ LSM303D::print_info() for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) { uint8_t v = read_reg(_checked_registers[i]); if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", + ::printf("reg %02x:%02x should be %02x\n", (unsigned)_checked_registers[i], (unsigned)v, (unsigned)_checked_values[i]); @@ -1769,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) int LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) { - return _parent->mag_ioctl(filp, cmd, arg); + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->mag_ioctl(filp, cmd, arg); + } } void diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e4e982490..e322e8b3a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -446,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_INT_ENABLE, MPUREG_INT_PIN_CFG }; - + /** * Helper class implementing the gyro driver node. @@ -523,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev // disable debug() calls _debug_enabled = false; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000; + // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -609,6 +615,7 @@ MPU6000::init() _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; + /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); /* if probe/setup failed, bail now */ @@ -668,7 +675,7 @@ int MPU6000::reset() // for it to come out of sleep write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); - + // Disable I2C bus (recommended on datasheet) write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); @@ -726,7 +733,7 @@ int MPU6000::reset() case MPU6000_REV_D9: case MPU6000_REV_D10: // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug + // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); @@ -804,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - /* + /* choose next highest filter frequency available */ if (frequency_hz == 0) { @@ -906,7 +913,7 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* + /* * Maximum deviation of 20 degrees, according to * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf * Section 6.1, initial ZRO tolerance @@ -967,7 +974,7 @@ MPU6000::factory_self_test() // gyro self test has to be done at 250DPS write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - struct MPUReport mpu_report; + struct MPUReport mpu_report; float accel_baseline[3]; float gyro_baseline[3]; float accel[3]; @@ -997,10 +1004,10 @@ MPU6000::factory_self_test() } #if 1 - write_reg(MPUREG_GYRO_CONFIG, - BITS_FS_250DPS | - BITS_GYRO_ST_X | - BITS_GYRO_ST_Y | + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | BITS_GYRO_ST_Z); // accel 8g, self-test enabled all axes @@ -1070,7 +1077,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } for (uint8_t i=0; i<3; i++) { float diff = gyro[i]-gyro_baseline[i]; float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; @@ -1085,7 +1092,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); @@ -1232,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_accel_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -1521,13 +1528,13 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1640,7 +1647,7 @@ MPU6000::measure() _register_wait--; return; } - + /* * Swap axes and negate y @@ -1701,7 +1708,7 @@ MPU6000::measure() float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - + arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); @@ -1722,7 +1729,7 @@ MPU6000::measure() float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - + grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); @@ -1776,7 +1783,7 @@ MPU6000::print_info() for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) { uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED); if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", + ::printf("reg %02x:%02x should be %02x\n", (unsigned)_checked_registers[i], (unsigned)v, (unsigned)_checked_values[i]); @@ -1848,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen) int MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { - return _parent->gyro_ioctl(filp, cmd, arg); + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } } /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d4cd97be6..13ab966ab 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { int fd; + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); @@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ fd = open(ACCEL_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } + + if (param_set(param_find("SENS_ACC_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2be0e881e..8410297ef 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -62,6 +62,7 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "HOLD STILL"); @@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); @@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } + if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bf5708e0b..67b7feef7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -45,6 +45,13 @@ #include <systemlib/param/param.h> /** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); + +/** * Gyro X-axis offset * * @min -10.0 @@ -153,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_ACC_ID, 0); /** * Accelerometer X-axis offset @@ -270,7 +283,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** * PX4Flow board rotation * - * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * This parameter defines the rotation of the PX4FLOW board relative to the platform. * Zero rotation is defined as Y on flow board pointing towards front of vehicle * Possible values are: * 0 = No rotation 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); |