mag_calibration.cpp 8.09 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
/****************************************************************************
 *
 *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
35
 * @file mag_calibration.cpp
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
 * Magnetometer calibration routine
 */

#include "mag_calibration.h"
#include "commander_helper.h"
#include "calibration_routines.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>

56
57
58
59
60
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
61

62
int do_mag_calibration(int mavlink_fd)
63
{
Lorenz Meier's avatar
Lorenz Meier committed
64
	mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
	struct mag_report mag;

	/* 45 seconds */
	uint64_t calibration_interval = 45 * 1000 * 1000;

	/* maximum 2000 values */
	const unsigned int calibration_maxcount = 500;
	unsigned int calibration_counter = 0;

	/* limit update rate to get equally spaced measurements over time (in ms) */
	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);

	int fd = open(MAG_DEVICE_PATH, O_RDONLY);

	/* erase old calibration */
	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 scale / offsets for mag");
		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
	}

	/* calibrate range */
	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
		warnx("failed to calibrate scale");
	}

	close(fd);

103
104
	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");

105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
	/* calibrate offsets */

	// uint64_t calibration_start = hrt_absolute_time();

	uint64_t axis_deadline = hrt_absolute_time();
	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;

	const char axislabels[3] = { 'X', 'Y', 'Z'};
	int axis_index = -1;

	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);

	if (x == NULL || y == NULL || z == NULL) {
		warnx("mag cal failed: out of memory");
		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
		warnx("x:%p y:%p z:%p\n", x, y, z);
123
		return ERROR;
124
125
	}

Lorenz Meier's avatar
Lorenz Meier committed
126
127
128
129
	mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");

	unsigned poll_errcount = 0;

130
131
132
133
	while (hrt_absolute_time() < calibration_deadline &&
	       calibration_counter < calibration_maxcount) {

		/* wait blocking for new data */
134
135
136
		struct pollfd fds[1];
		fds[0].fd = sub_mag;
		fds[0].events = POLLIN;
137
138
139
140
141
142
143

		/* user guidance */
		if (hrt_absolute_time() >= axis_deadline &&
		    axis_index < 3) {

			axis_index++;

Lorenz Meier's avatar
Lorenz Meier committed
144
			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
145
146
147
148
149
150
151
152
153
154
155
			tune_neutral();

			axis_deadline += calibration_interval / 3;
		}

		if (!(axis_index < 3)) {
			break;
		}

		int poll_ret = poll(fds, 1, 1000);

Lorenz Meier's avatar
Lorenz Meier committed
156
		if (poll_ret > 0) {
157
158
159
160
161
162
163
			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);

			x[calibration_counter] = mag.x;
			y[calibration_counter] = mag.y;
			z[calibration_counter] = mag.z;

			calibration_counter++;
164
165
166
			if (calibration_counter % (calibration_maxcount / 20) == 0)
				mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);

167

Lorenz Meier's avatar
Lorenz Meier committed
168
169
170
171
172
173
		} else {
			poll_errcount++;
		}

		if (poll_errcount > 1000) {
			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
174
175
176
177
			close(sub_mag);
			free(x);
			free(y);
			free(z);
Lorenz Meier's avatar
Lorenz Meier committed
178
			return ERROR;
179
		}
Lorenz Meier's avatar
Lorenz Meier committed
180
181


182
183
184
185
186
187
188
	}

	float sphere_x;
	float sphere_y;
	float sphere_z;
	float sphere_radius;

189
	mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
190
	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
191
	mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240

	free(x);
	free(y);
	free(z);

	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {

		fd = open(MAG_DEVICE_PATH, 0);

		struct mag_scale mscale;

		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
			warn("WARNING: failed to get scale / offsets for mag");

		mscale.x_offset = sphere_x;
		mscale.y_offset = sphere_y;
		mscale.z_offset = sphere_z;

		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
			warn("WARNING: failed to set scale / offsets for mag");

		close(fd);

		/* announce and set new offset */

		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
			warnx("Setting X mag offset failed!\n");
		}

		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
			warnx("Setting Y mag offset failed!\n");
		}

		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
			warnx("Setting Z mag offset failed!\n");
		}

		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
			warnx("Setting X mag scale failed!\n");
		}

		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
			warnx("Setting Y mag scale failed!\n");
		}

		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
			warnx("Setting Z mag scale failed!\n");
		}

241
242
		mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");

243
244
245
246
247
248
		/* auto-save to EEPROM */
		int save_ret = param_save_default();

		if (save_ret != 0) {
			warn("WARNING: auto-save of params to storage failed");
			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
249
			close(sub_mag);
250
			return ERROR;
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
		}

		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);

		char buf[52];
		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
			(double)mscale.y_offset, (double)mscale.z_offset);
		mavlink_log_info(mavlink_fd, buf);

		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
			(double)mscale.y_scale, (double)mscale.z_scale);
		mavlink_log_info(mavlink_fd, buf);

Lorenz Meier's avatar
Lorenz Meier committed
266
		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
267
		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
268

269
		close(sub_mag);
270
		return OK;
271
272
273
274
		/* third beep by cal end routine */

	} else {
		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
275
		close(sub_mag);
276
		return ERROR;
277
	}
278
}