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

Commander: Implement calibration routines for multi-sensor setups

Browse Source
This commit is contained in:
Lorenz Meier
2015-02-03 13:47:46 +01:00
parent ac155b0fac
commit 807cf7bd16
6 changed files with 372 additions and 227 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+112 -93
View File
@@ -63,12 +63,78 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id);
int do_mag_calibration(int mavlink_fd)
{
int32_t device_id;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
const unsigned max_mags = 3;
int32_t device_id[max_mags];
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
sleep(1);
struct mag_scale mscale_null[max_mags] = {
{
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
}
} ;
int res = ERROR;
char str[30];
for (unsigned s = 0; s < max_mags; s++) {
mavlink_log_info(mavlink_fd, "Magnetometer #%u", s);
sleep(3);
/* erase old calibration */
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
int fd = open(str, O_RDONLY);
if (fd < 0) {
continue;
}
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
/* ensure all scale fields are initialized tha same as the first struct */
(void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0]));
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* calibrate range */
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
mavlink_log_info(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
}
}
close(fd);
if (res == OK) {
res = calibrate_instance(mavlink_fd, s, device_id[s]);
}
}
return res;
}
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
{
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
@@ -76,80 +142,42 @@ int do_mag_calibration(int mavlink_fd)
const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* erase old calibration */
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* calibrate range */
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
}
}
close(fd);
float *x = NULL;
float *y = NULL;
float *z = NULL;
if (res == OK) {
/* allocate memory */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
char str[30];
int res = ERROR;
/* allocate memory */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
/* clean up */
if (x != NULL) {
free(x);
}
if (y != NULL) {
free(y);
}
if (z != NULL) {
free(z);
}
res = ERROR;
return res;
/* clean up */
if (x != NULL) {
free(x);
}
} else {
/* exit */
return ERROR;
if (y != NULL) {
free(y);
}
if (z != NULL) {
free(z);
}
res = ERROR;
return res;
}
if (res == OK) {
int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);
if (sub_mag < 0) {
mavlink_log_critical(mavlink_fd, "No mag found, abort");
@@ -239,8 +267,8 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* apply calibration and set parameters */
struct mag_scale mscale;
fd = open(MAG_DEVICE_PATH, 0);
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
if (res != OK) {
@@ -262,35 +290,26 @@ int do_mag_calibration(int mavlink_fd)
close(fd);
if (res == OK) {
bool failed = false;
/* set parameters */
if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
res = ERROR;
}
if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
(void)sprintf(str, "CAL_MAG%u_ID", s);
failed |= (OK != param_set(param_find(str), &(device_id)));
(void)sprintf(str, "CAL_MAG%u_XOFF", s);
failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", s);
failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", s);
failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
(void)sprintf(str, "CAL_MAG%u_XSCALE", s);
failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", s);
failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));
if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
if (failed) {
res = ERROR;
}
if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
res = ERROR;
}
if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
res = ERROR;
}
if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
res = ERROR;
}
if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
res = ERROR;
}
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
}