fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Save and check device id for acc and gyro calibration parameters.

Browse Source 
Fix config utility to work with all devices of each type.
Accel, gyro and mag devices correctly set their device_id devtype.
Combo devices (mpu6000 lsm303d) now correctly return their devtype.
config util shows device id for all sensor types.
Add, save during calibration and check during preflight ID parameters for accelerometer and gyro
This commit is contained in:
hauptmech 2015-01-28 15:45:00 +13:00 committed by Lorenz Meier
parent e9bcc0a262
commit 0b784c20c8
10 changed files with 156 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/drivers/drv_mag.h
View File

@ -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

18
src/drivers/drv_sensor.h
View File

@ -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
*

16
src/drivers/l3gd20/l3gd20.cpp
View File

@ -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]);

32
src/drivers/lsm303d/lsm303d.cpp
View File

@ -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

56
src/drivers/mpu6000/mpu6000.cpp
View File

@ -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);
}
}
/**

8
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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) {

7
src/modules/commander/gyro_calibration.cpp
View File

@ -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) {

15
src/modules/sensors/sensor_params.c
View File

@ -44,6 +44,13 @@
#include <nuttx/config.h>
#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
*
@ -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

17
src/systemcmds/config/config.c
View File

@ -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;
warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
param_get(param_find("SENS_GYRO_ID"), &(calibration_id));
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;
warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
param_get(param_find("SENS_ACC_ID"), &(calibration_id));
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);
}

38
src/systemcmds/preflight_check/preflight_check.c
View File

@ -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);