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

Calibration WIP, not compiling

Browse Source
This commit is contained in:
Lorenz Meier 2012-10-17 08:02:50 +02:00
parent 32e586d4b7
commit 97726fa679
5 changed files with 192 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
apps/commander/commander.c
View File

@ -299,6 +299,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
int fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale_null = {
0.0f,
1.0f,
@ -396,6 +399,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
* to shift the center to zero
*
* offset = max - ((max - min) / 2.0f)
* max - max/2 + min/2
* max/2 + min/2
*
* which reduces to
*
@ -422,6 +427,18 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mag_offset[0]))) {
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mag_offset[1]))) {
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mag_offset[2]))) {
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
}
fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale = {
mag_offset[0],

3
apps/drivers/drv_mag.h
View File

@ -105,4 +105,7 @@ ORB_DECLARE(sensor_mag);
/** perform self-calibration, update scale factors to canonical units */
#define MAGIOCCALIBRATE _MAGIOC(5)
/** excite strap */
#define MAGIOCEXSTRAP _MAGIOC(6)
#endif /* _DRV_MAG_H */

362
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -186,6 +186,18 @@ private:
*/
int calibrate(unsigned enable);
/**
* 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 positive strap, -1 to enable
* negative strap, 0 to set to normal mode
*/
int set_excitement(unsigned enable);
/**
* Set the sensor range.
*
@ -593,6 +605,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCCALIBRATE:
return calibrate(arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@ -799,6 +814,139 @@ out:
}
int HMC5883::calibrate(unsigned enable)
{
struct mag_report report;
ssize_t sz;
int ret;
int fd = (int)enable;
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
err(1, "immediate read failed");
warnx("starting mag scale calibration");
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
errx(1, "failed to set queue depth");
/* start the sensor polling at 50 Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50))
errx(1, "failed to set 2Hz poll rate");
/* Set to 2.5 Gauss */
if (OK != ioctl(fd, MAGIOCSRANGE, 2)) {
warnx("failed to set 2.5 Ga range");
}
if (OK != ioctl(fd, MAGIOCPOSEX, 1)) {
warnx("failed to enable sensor calibration mode");
}
struct mag_scale mscale_previous = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to get scale / offsets for mag");
}
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
}
float avg_excited[3];
unsigned i;
/* read the sensor 10x and report each value */
for (i = 0; i < 500; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
errx(1, "timed out waiting for sensor data");
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
} else {
avg_excited[0] += report.x;
avg_excited[1] += report.y;
avg_excited[2] += report.z;
}
// warnx("periodic read %u", i);
// warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
// warnx("time: %lld", report.timestamp);
}
avg_excited[0] /= i;
avg_excited[1] /= i;
avg_excited[2] /= i;
warnx("periodic excited reads %u", i);
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
/* Set to 1.1 Gauss and end calibration */
ret = ioctl(fd, MAGIOCNONEX, 0);
ret = ioctl(fd, MAGIOCSRANGE, 1);
float scaling[3];
/* calculate axis scaling */
scaling[0] = 1.16f / avg_excited[0];
/* second axis inverted */
scaling[1] = 1.16f / -avg_excited[1];
scaling[2] = 1.08f / avg_excited[2];
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
}
if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) {
warnx("failed to disable sensor calibration mode");
}
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
}
}
int HMC5883::set_excitement(unsigned enable)
{
int ret;
/* arm the excitement strap */
@ -806,8 +954,10 @@ int HMC5883::calibrate(unsigned enable)
ret = read_reg(ADDR_CONF_A, conf_reg);
if (OK != ret)
perf_count(_comms_errors);
if (enable) {
if (enable < 0) {
conf_reg |= 0x01;
} else if (enable > 0) {
conf_reg |= 0x02;
} else {
conf_reg &= ~0x03;
}
@ -1020,7 +1170,6 @@ test()
*/
int calibrate()
{
struct mag_report report;
ssize_t sz;
int ret;
@ -1029,216 +1178,11 @@ int calibrate()
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
err(1, "immediate read failed");
warnx("single read");
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
errx(1, "failed to set queue depth");
/* start the sensor polling at 10 Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10))
errx(1, "failed to set 2Hz poll rate");
/* Set to 2.5 Gauss */
if (OK != ioctl(fd, MAGIOCSRANGE, 2)) {
warnx("failed to set 2.5 Ga range");
}
if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) {
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
warnx("failed to enable sensor calibration mode");
}
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
}
float avg_excited[3];
unsigned i;
/* read the sensor 10x and report each value */
for (i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
errx(1, "timed out waiting for sensor data");
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
} else {
avg_excited[0] += report.x;
avg_excited[1] += report.y;
avg_excited[2] += report.z;
}
warnx("periodic read %u", i);
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
}
// warnx("starting calibration");
// struct mag_report report;
// ssize_t sz;
// int ret;
// int fd = open(MAG_DEVICE_PATH, O_RDONLY);
// if (fd < 0)
// err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
// /* do a simple demand read */
// sz = read(fd, &report, sizeof(report));
// if (sz != sizeof(report))
// err(1, "immediate read failed");
// warnx("single read");
// warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
// warnx("time: %lld", report.timestamp);
// /* get scaling, set to zero */
// struct mag_scale mscale_previous;
// if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
// warn("WARNING: failed to get scale / offsets for mag");
// }
// struct mag_scale mscale_null = {
// 0.0f,
// 1.0f,
// 0.0f,
// 1.0f,
// 0.0f,
// 1.0f,
// };
// if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
// warn("WARNING: failed to set null scale / offsets for mag");
// }
// warnx("sensor ready");
// float avg_excited[3] = {0.0f, 0.0f, 0.0f};
// if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) {
// warnx("failed to enable sensor calibration mode");
// }
// /* Set to 2.5 Gauss */
// if (OK != ioctl(fd, MAGIOCSRANGE, 2)) {
// warnx("failed to set 2.5 Ga range");
// }
// /* set the queue depth to 10 */
// if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
// warnx("failed to set queue depth");
// return 1;
// } else {
// warnx("set queue depth");
// }
// /* start the sensor polling at 100Hz */
// if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 100)) {
// warnx("failed to set 100 Hz poll rate");
// return 1;
// } else {
// warnx("set 100 Hz poll rate");
// }
// int i;
// for (i = 0; i < 10; i++) {
// struct pollfd fds;
// (void) ioctl(fd, MAGIOCCALIBRATE, 1);
// /* wait for data to be ready */
// fds.fd = fd;
// fds.events = POLLIN;
// ret = poll(&fds, 1, 2000);
// if (ret != 1) {
// warnx("timed out waiting for sensor data");
// return 1;
// }
// /* now go get it */
// sz = read(fd, &report, sizeof(report));
// if (sz != sizeof(report)) {
// warn("periodic read failed");
// return 1;
// } else {
// avg_excited[0] += report.x;
// avg_excited[1] += report.y;
// avg_excited[2] += report.z;
// }
// warnx("excited read %u", i);
// warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
// warnx("time: %lld", report.timestamp);
// }
avg_excited[0] /= i;
avg_excited[1] /= i;
avg_excited[2] /= i;
warnx("periodic excited reads %u", i);
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
/* Set to 1.1 Gauss and end calibration */
ret = ioctl(fd, MAGIOCCALIBRATE, 0);
ret = ioctl(fd, MAGIOCSRANGE, 1);
float scaling[3];
/* calculate axis scaling */
scaling[0] = 1.16f / avg_excited[0];
/* second axis inverted */
scaling[1] = 1.16f / -avg_excited[1];
scaling[2] = 1.08f / avg_excited[2];
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
}
if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) {
warnx("failed to disable sensor calibration mode");
}
/* set scaling in device */
// mscale_previous.x_scale = scaling[0];
// mscale_previous.y_scale = scaling[1];
// mscale_previous.z_scale = scaling[2];
// if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
// warn("WARNING: failed to set new scale / offsets for mag");
// }
close(fd);
errx(0, "PASS");
}

4
apps/sensors/sensor_params.c
View File

@ -52,6 +52,10 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);

19
apps/sensors/sensors.cpp
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,6 +34,7 @@
/**
* @file sensors.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Sensor readout process.
*/
@ -178,6 +179,7 @@ private:
float gyro_offset[3];
float mag_offset[3];
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
@ -209,6 +211,7 @@ private:
param_t accel_offset[3];
param_t accel_scale[3];
param_t mag_offset[3];
param_t mag_scale[3];
param_t rc_map_roll;
param_t rc_map_pitch;
@ -414,6 +417,10 @@ Sensors::Sensors() :
_parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF");
_parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF");
_parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE");
_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* fetch initial parameter values */
@ -537,6 +544,10 @@ Sensors::parameters_update()
param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0]));
param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1]));
param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2]));
/* mag scaling */
param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_scale[0]));
param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_scale[1]));
param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_scale[2]));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@ -798,11 +809,11 @@ Sensors::parameter_update_poll(bool forced)
fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale = {
_parameters.mag_offset[0],
1.0f,
_parameters.mag_scale[0],
_parameters.mag_offset[1],
1.0f,
_parameters.mag_scale[1],
_parameters.mag_offset[2],
1.0f,
_parameters.mag_scale[2],
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to set scale / offsets for mag");