forked from Archive/PX4-Autopilot
ist8310: remove calibration code
- it does nothing useful - increases boot time by 2 seconds on pixhawk 4 due to a poll timeout: IST8310 on I2C bus 3 at 0x0e (bus: 100 KHz, max: 400 KHz) WARN [ist8310] ERROR: TIMEOUT 2
This commit is contained in:
Beat Küng
Daniel Agar
parent
225a3a0cba
commit
b399698306
|
|
@ -20,7 +20,7 @@ then
|
|||
fi
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
|
|
|
|||
|
|
@ -14,14 +14,14 @@ bmi088 -A -R 10 start
|
|||
bmi088 -G -R 10 start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
lis3mdl -X start
|
||||
|
||||
# Possible internal compass
|
||||
ist8310 -C -b 5 start
|
||||
ist8310 -b 5 start
|
||||
|
||||
# Baro on internal SPI
|
||||
ms5611 -s start
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ fi
|
|||
bmp280 start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@ mpu9250 -s -R 14 start
|
|||
# Possible external compasses
|
||||
hmc5883 -X start
|
||||
|
||||
ist8310 -C -b 1 -R 4 start
|
||||
ist8310 -b 1 -R 4 start
|
||||
|
||||
ll40ls start i2c -a
|
||||
|
||||
|
|
|
|||
|
|
@ -21,8 +21,8 @@ bmi088 -A -R 4 start
|
|||
bmi088 -G -R 4 start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
|
||||
|
|
|
|||
|
|
@ -21,8 +21,8 @@ bmi088 -G -R 10 start
|
|||
dps310 -s start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
lis3mdl -X start
|
||||
|
|
|
|||
|
|
@ -8,7 +8,6 @@ adc start
|
|||
# External I2C bus
|
||||
hmc5883 -C -T -X start
|
||||
lis3mdl -X start
|
||||
ist8310 -C start
|
||||
|
||||
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
|
||||
mpu6000 -R 2 -T 20608 start
|
||||
|
|
@ -20,8 +19,8 @@ mpu6000 -R 2 -T 20602 start
|
|||
mpu9250 -R 2 start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
lis3mdl -X start
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@ adc start
|
|||
# External I2C bus
|
||||
hmc5883 -C -T -X start
|
||||
lis3mdl -X start
|
||||
ist8310 -C start
|
||||
ist8310 start
|
||||
qmc5883 -X start
|
||||
|
||||
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@ adc start
|
|||
# Possible external compasses
|
||||
hmc5883 -C -X start
|
||||
lis3mdl -X start
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -b 1 start
|
||||
qmc5883 -X start
|
||||
|
||||
# Internal Mag I2C bus roll 180, yaw 90
|
||||
|
|
|
|||
|
|
@ -30,8 +30,8 @@ bmi055 -A -R 10 start
|
|||
bmi055 -G -R 10 start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
lis3mdl -X start
|
||||
|
|
@ -44,7 +44,7 @@ then
|
|||
fi
|
||||
|
||||
# Possible internal compass
|
||||
ist8310 -C -b 5 start
|
||||
ist8310 -b 5 start
|
||||
|
||||
# Baro on internal SPI
|
||||
ms5611 -s start
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@ adc start
|
|||
# External I2C bus
|
||||
hmc5883 -C -T -X start
|
||||
lis3mdl -X start
|
||||
ist8310 -C start
|
||||
ist8310 start
|
||||
|
||||
# Internal I2C bus
|
||||
hmc5883 -C -T -I -R 4 start
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@ adc start
|
|||
# External I2C bus
|
||||
hmc5883 -C -T -X start
|
||||
lis3mdl -X start
|
||||
ist8310 -C start
|
||||
ist8310 start
|
||||
qmc5883 -X start
|
||||
|
||||
# Internal I2C bus
|
||||
|
|
|
|||
|
|
@ -18,8 +18,8 @@ bmi055 -A -R 10 start
|
|||
bmi055 -G -R 10 start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
lis3mdl -X start
|
||||
|
|
@ -32,7 +32,7 @@ then
|
|||
fi
|
||||
|
||||
# Possible internal compass
|
||||
ist8310 -C -b 5 start
|
||||
ist8310 -b 5 start
|
||||
|
||||
# Baro on internal SPI
|
||||
ms5611 -s start
|
||||
|
|
|
|||
|
|
@ -22,8 +22,8 @@ bmi088 -A -R 12 start
|
|||
bmi088 -G -R 12 start
|
||||
|
||||
# Possible external compasses
|
||||
ist8310 -C -b 1 start
|
||||
ist8310 -C -b 2 start
|
||||
ist8310 -b 1 start
|
||||
ist8310 -b 2 start
|
||||
hmc5883 -C -T -X start
|
||||
qmc5883 -X start
|
||||
|
||||
|
|
|
|||
|
|
@ -222,9 +222,6 @@ private:
|
|||
perf_counter_t _range_errors;
|
||||
perf_counter_t _conf_errors;
|
||||
|
||||
/* status reporting */
|
||||
bool _sensor_ok{false}; /**< sensor was found and reports ok */
|
||||
bool _calibrated{false}; /**< the calibration is valid */
|
||||
enum Rotation _rotation;
|
||||
|
||||
sensor_mag_s _last_report{}; /**< used for info() */
|
||||
|
|
@ -250,17 +247,6 @@ private:
|
|||
*/
|
||||
int reset();
|
||||
|
||||
/**
|
||||
* Perform the on-sensor scale calibration routine.
|
||||
*
|
||||
* @note The sensor will continue to provide measurements, these
|
||||
* will however reflect the uncalibrated sensor state until
|
||||
* the calibration routine has been completed.
|
||||
*
|
||||
* @param enable set to 1 to enable self-test strap, 0 to disable
|
||||
*/
|
||||
int calibrate(struct file *filp, unsigned enable);
|
||||
|
||||
/**
|
||||
* check the sensor configuration.
|
||||
*
|
||||
|
|
@ -343,20 +329,6 @@ private:
|
|||
*/
|
||||
float meas_to_float(uint8_t in[2]);
|
||||
|
||||
/**
|
||||
* Check the current scale calibration
|
||||
*
|
||||
* @return 0 if scale calibration is ok, 1 else
|
||||
*/
|
||||
int check_scale();
|
||||
|
||||
/**
|
||||
* Check the current offset calibration
|
||||
*
|
||||
* @return 0 if offset calibration is ok, 1 else
|
||||
*/
|
||||
int check_offset();
|
||||
|
||||
/**
|
||||
* Place the device in self test mode
|
||||
*
|
||||
|
|
@ -440,8 +412,6 @@ IST8310::init()
|
|||
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but not calibrated */
|
||||
_sensor_ok = true;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -639,9 +609,6 @@ IST8310::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
|
||||
return 0;
|
||||
|
||||
case MAGIOCCALIBRATE:
|
||||
return calibrate(filp, arg);
|
||||
|
||||
case MAGIOCGEXTERNAL:
|
||||
return external();
|
||||
|
||||
|
|
@ -894,158 +861,6 @@ out:
|
|||
return ret;
|
||||
}
|
||||
|
||||
int IST8310::calibrate(struct file *filp, unsigned enable)
|
||||
{
|
||||
sensor_mag_s report {};
|
||||
ssize_t sz;
|
||||
int ret = 1;
|
||||
float total_x = 0.0f;
|
||||
float total_y = 0.0f;
|
||||
float total_z = 0.0f;
|
||||
|
||||
// XXX do something smarter here
|
||||
int fd = (int)enable;
|
||||
|
||||
struct mag_calibration_s mscale_previous;
|
||||
|
||||
struct mag_calibration_s mscale_null;
|
||||
mscale_null.x_offset = 0.0f;
|
||||
mscale_null.x_scale = 1.0f;
|
||||
mscale_null.y_offset = 0.0f;
|
||||
mscale_null.y_scale = 1.0f;
|
||||
mscale_null.z_offset = 0.0f;
|
||||
mscale_null.z_scale = 1.0f;
|
||||
|
||||
float sum_in_test[3] = {0.0f, 0.0f, 0.0f};
|
||||
float sum_in_normal[3] = {0.0f, 0.0f, 0.0f};
|
||||
float *sum = &sum_in_normal[0];
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
|
||||
PX4_WARN("FAILED: MAGIOCGSCALE 1");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||
PX4_WARN("FAILED: MAGIOCSSCALE 1");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* start the sensor polling at 50 Hz */
|
||||
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
|
||||
PX4_WARN("FAILED: SENSORIOCSPOLLRATE 50Hz");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
||||
// discard 10 samples to let the sensor settle
|
||||
/* read the sensor 50 times */
|
||||
|
||||
for (uint8_t p = 0; p < 2; p++) {
|
||||
|
||||
if (p == 1) {
|
||||
|
||||
/* start the Self test */
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
|
||||
PX4_WARN("FAILED: MAGIOCEXSTRAP 1");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
||||
sum = &sum_in_test[0];
|
||||
}
|
||||
|
||||
|
||||
for (uint8_t i = 0; i < 30; i++) {
|
||||
|
||||
|
||||
struct pollfd fds;
|
||||
|
||||
/* wait for data to be ready */
|
||||
fds.fd = fd;
|
||||
fds.events = POLLIN;
|
||||
ret = ::poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1) {
|
||||
PX4_WARN("ERROR: TIMEOUT 2");
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
|
||||
sz = ::read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
PX4_WARN("ERROR: READ 2");
|
||||
ret = -EIO;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (i > 10) {
|
||||
sum[0] += report.x_raw;
|
||||
sum[1] += report.y_raw;
|
||||
sum[2] += report.z_raw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
total_x = fabsf(sum_in_test[0] - sum_in_normal[0]);
|
||||
total_y = fabsf(sum_in_test[1] - sum_in_normal[1]);
|
||||
total_z = fabsf(sum_in_test[2] - sum_in_normal[2]);
|
||||
|
||||
ret = ((total_x + total_y + total_z) < (float)0.000001);
|
||||
|
||||
out:
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
|
||||
PX4_WARN("FAILED: MAGIOCSSCALE 2");
|
||||
}
|
||||
|
||||
/* set back to normal mode */
|
||||
|
||||
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
|
||||
PX4_WARN("FAILED: MAGIOCEXSTRAP 0");
|
||||
}
|
||||
|
||||
if (ret == OK) {
|
||||
if (check_scale()) {
|
||||
/* failed */
|
||||
PX4_WARN("FAILED: SCALE");
|
||||
ret = PX4_ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("FAILED: CALIBRATION SCALE %d, %d, %d", (int)total_x, (int)total_y, (int)total_z);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int IST8310::check_scale()
|
||||
{
|
||||
return OK;
|
||||
}
|
||||
|
||||
int IST8310::check_offset()
|
||||
{
|
||||
bool offset_valid;
|
||||
|
||||
if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
|
||||
(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
|
||||
(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
|
||||
/* offset is zero */
|
||||
offset_valid = false;
|
||||
|
||||
} else {
|
||||
offset_valid = true;
|
||||
}
|
||||
|
||||
/* return 0 if calibrated, 1 else */
|
||||
return !offset_valid;
|
||||
}
|
||||
|
||||
int
|
||||
IST8310::set_selftest(unsigned enable)
|
||||
{
|
||||
|
|
@ -1151,7 +966,6 @@ struct ist8310_bus_option &find_bus(enum IST8310_BUS busid);
|
|||
void test(enum IST8310_BUS busid);
|
||||
void reset(enum IST8310_BUS busid);
|
||||
int info(enum IST8310_BUS busid);
|
||||
int calibrate(enum IST8310_BUS busid);
|
||||
void usage();
|
||||
|
||||
/**
|
||||
|
|
@ -1299,51 +1113,6 @@ test(enum IST8310_BUS busid)
|
|||
}
|
||||
|
||||
|
||||
/**
|
||||
* Automatic scale calibration.
|
||||
*
|
||||
* Basic idea:
|
||||
*
|
||||
* output = (ext field +- 1.1 Ga self-test) * scale factor
|
||||
*
|
||||
* and consequently:
|
||||
*
|
||||
* 1.1 Ga = (excited - normal) * scale factor
|
||||
* scale factor = (excited - normal) / 1.1 Ga
|
||||
*
|
||||
* sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
|
||||
* sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
|
||||
*
|
||||
* By subtracting the non-excited measurement the pure 1.1 Ga reading
|
||||
* can be extracted and the sensitivity of all axes can be matched.
|
||||
*
|
||||
* SELF TEST OPERATION
|
||||
* To check the IST8310L for proper operation, a self test feature in incorporated
|
||||
* in which the sensor will change the polarity on all 3 axis. The values with and
|
||||
* with and without selftest on shoult be compared and if the absolete value are equal
|
||||
* the IC is functional.
|
||||
*/
|
||||
int calibrate(enum IST8310_BUS busid)
|
||||
{
|
||||
int ret;
|
||||
struct ist8310_bus_option &bus = find_bus(busid);
|
||||
const char *path = bus.devpath;
|
||||
|
||||
int fd = open(path, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "%s open failed (try 'ist8310 start' if the driver is not running", path);
|
||||
}
|
||||
|
||||
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
|
||||
PX4_WARN("failed to enable sensor calibration mode");
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the driver.
|
||||
*/
|
||||
|
|
@ -1389,10 +1158,9 @@ info(enum IST8310_BUS busid)
|
|||
void
|
||||
usage()
|
||||
{
|
||||
PX4_INFO("missing command: try 'start', 'info', 'test', 'reset', 'calibrate'");
|
||||
PX4_INFO("missing command: try 'start', 'info', 'test', 'reset'");
|
||||
PX4_INFO("options:");
|
||||
PX4_INFO(" -R rotation");
|
||||
PX4_INFO(" -C calibrate on start");
|
||||
PX4_INFO(" -a 12C Address (0x%02x)", IST8310_BUS_I2C_ADDR);
|
||||
PX4_INFO(" -b 12C bus (%d-%d)", IST8310_BUS_I2C_EXTERNAL, IST8310_BUS_I2C_INTERNAL);
|
||||
}
|
||||
|
|
@ -1406,12 +1174,11 @@ ist8310_main(int argc, char *argv[])
|
|||
int i2c_addr = IST8310_BUS_I2C_ADDR; /* 7bit */
|
||||
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
bool calibrate = false;
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "R:Ca:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
while ((ch = px4_getopt(argc, argv, "R:a:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
rotation = (enum Rotation)atoi(myoptarg);
|
||||
|
|
@ -1425,10 +1192,6 @@ ist8310_main(int argc, char *argv[])
|
|||
i2c_busid = (IST8310_BUS)strtol(myoptarg, NULL, 0);
|
||||
break;
|
||||
|
||||
case 'C':
|
||||
calibrate = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
ist8310::usage();
|
||||
exit(0);
|
||||
|
|
@ -1447,16 +1210,6 @@ ist8310_main(int argc, char *argv[])
|
|||
*/
|
||||
if (!strcmp(verb, "start")) {
|
||||
ist8310::start(i2c_busid, i2c_addr, rotation);
|
||||
|
||||
if (i2c_busid == IST8310_BUS_ALL) {
|
||||
PX4_ERR("calibration only feasible against one bus");
|
||||
return 1;
|
||||
|
||||
} else if (calibrate && (ist8310::calibrate(i2c_busid) != 0)) {
|
||||
PX4_ERR("calibration failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -1481,20 +1234,6 @@ ist8310_main(int argc, char *argv[])
|
|||
ist8310::info(i2c_busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Autocalibrate the scaling
|
||||
*/
|
||||
if (!strcmp(verb, "calibrate")) {
|
||||
if (ist8310::calibrate(i2c_busid) == 0) {
|
||||
PX4_INFO("calibration successful");
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
PX4_ERR("calibration failed");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
ist8310::usage();
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue