Пример #1
0
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
{
	calibrate_return result = calibrate_return_ok;

	mag_worker_data_t worker_data;
	
	worker_data.mavlink_fd = mavlink_fd;
	worker_data.done_count = 0;
	worker_data.calibration_points_perside = 40;
	worker_data.calibration_interval_perside_seconds = 20;
	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;

	// Collect: Right-side up, Left Side, Nose down
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
	
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		// Initialize to no subscription
		worker_data.sub_mag[cur_mag] = -1;
		
		// Initialize to no memory allocated
		worker_data.x[cur_mag] = NULL;
		worker_data.y[cur_mag] = NULL;
		worker_data.z[cur_mag] = NULL;
		worker_data.calibration_counter_total[cur_mag] = 0;
	}

	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
	
	char str[30];
	
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
			result = calibrate_return_error;
		}
	}

	
	// Setup subscriptions to mag sensors
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available
				worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
				if (worker_data.sub_mag[cur_mag] < 0) {
					mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
					result = calibrate_return_error;
					break;
				}
			}
		}
	}
	
	// Limit update rate to get equally spaced measurements over time (in ms)
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available
				unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
				
				//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
				orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
			}
		}
		
	}
    
	if (result == calibrate_return_ok) {
		int cancel_sub  = calibrate_cancel_subscribe();

		result = calibrate_from_orientation(mavlink_fd,                         // Mavlink fd to write output
						    cancel_sub,                         // Subscription to vehicle_command for cancel support
						    worker_data.side_data_collected,    // Sides to calibrate
						    mag_calibration_worker,             // Calibration worker
						    &worker_data,			// Opaque data for calibration worked
						    true);				// true: lenient still detection
		calibrate_cancel_unsubscribe(cancel_sub);
	}
	
	// Close subscriptions
	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
		if (worker_data.sub_mag[cur_mag] >= 0) {
			px4_close(worker_data.sub_mag[cur_mag]);
		}
	}
	
	// Calculate calibration values for each mag
	
	
	float sphere_x[max_mags];
	float sphere_y[max_mags];
	float sphere_z[max_mags];
	float sphere_radius[max_mags];
	
	// Sphere fit the data to get calibration values
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available and we should have values for it to calibrate
				
				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
							 worker_data.calibration_counter_total[cur_mag],
							 100, 0.0f,
							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
							 &sphere_radius[cur_mag]);
				
				if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
					result = calibrate_return_error;
				}
			}
		}
	}

	// Print uncalibrated data points
	if (result == calibrate_return_ok) {

		printf("RAW DATA:\n--------------------\n");
		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

			printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
				float x = worker_data.x[cur_mag][i];
				float y = worker_data.y[cur_mag][i];
				float z = worker_data.z[cur_mag][i];
				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
			}

			printf(">>>>>>>\n");
		}

		printf("CALIBRATED DATA:\n--------------------\n");
		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

			printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
				float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
				float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
				float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
			}

			printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
			printf(">>>>>>>\n");
		}
	}
	
	// Data points are no longer needed
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		free(worker_data.x[cur_mag]);
		free(worker_data.y[cur_mag]);
		free(worker_data.z[cur_mag]);
	}
	
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				int fd_mag = -1;
				struct mag_scale mscale;
				
				// Set new scale
				
				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
				fd_mag = px4_open(str, 0);
				if (fd_mag < 0) {
					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
					result = calibrate_return_error;
				}
				
				if (result == calibrate_return_ok) {
					if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
						mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
						result = calibrate_return_error;
					}
				}

				if (result == calibrate_return_ok) {
					mscale.x_offset = sphere_x[cur_mag];
					mscale.y_offset = sphere_y[cur_mag];
					mscale.z_offset = sphere_z[cur_mag];

					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
						result = calibrate_return_error;
					}
				}
				
				// Mag device no longer needed
				if (fd_mag >= 0) {
					px4_close(fd_mag);
				}

				if (result == calibrate_return_ok) {
					bool failed = false;
					
					/* set parameters */
					(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));

					if (failed) {
						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
						result = calibrate_return_error;
					} else {
						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
									     cur_mag,
									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
									     cur_mag,
									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
					}
				}
			}
		}
	}

	return result;
}
Пример #2
0
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
{
	/* 45 seconds */
	uint64_t calibration_interval = 25 * 1000 * 1000;

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

	float *x = NULL;
	float *y = NULL;
	float *z = NULL;

	char str[30];
	int res = OK;
	
	/* allocate memory */
	mavlink_and_console_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));

	if (x == NULL || y == NULL || z == NULL) {
		mavlink_and_console_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;
	}

	if (res == OK) {
		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);

		if (sub_mag < 0) {
			mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort");
			res = ERROR;
		}  else {
			struct mag_report mag;

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

			/* calibrate offsets */
			uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
			unsigned poll_errcount = 0;

			mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");

			calibration_counter = 0U;

			while (hrt_absolute_time() < calibration_deadline &&
			       calibration_counter < calibration_maxcount) {

				/* wait blocking for new data */
				struct pollfd fds[1];
				fds[0].fd = sub_mag;
				fds[0].events = POLLIN;

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

				if (poll_ret > 0) {
					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++;

					if (calibration_counter % (calibration_maxcount / 20) == 0) {
						mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
					}

				} else {
					poll_errcount++;
				}

				if (poll_errcount > 1000) {
					mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
					res = ERROR;
					break;
				}
			}

			close(sub_mag);
		}
	}

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

	if (res == OK && calibration_counter > (calibration_maxcount / 2)) {

		/* sphere fit */
		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);

		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
			mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
			res = ERROR;
		}
	}

	if (x != NULL) {
		free(x);
	}

	if (y != NULL) {
		free(y);
	}

	if (z != NULL) {
		free(z);
	}

	if (res == OK) {
		/* apply calibration and set parameters */
		struct mag_scale mscale;
		(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) {
			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
		}

		if (res == OK) {
			mscale.x_offset = sphere_x;
			mscale.y_offset = sphere_y;
			mscale.z_offset = sphere_z;

			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);

			if (res != OK) {
				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
			}
		}

		close(fd);

		if (res == OK) {

			bool failed = false;
			/* set parameters */
			(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 (failed) {
				res = ERROR;
				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
			}

			mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
		}

		mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
				 (double)mscale.y_offset, (double)mscale.z_offset);
		mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
				 (double)mscale.y_scale, (double)mscale.z_scale);
	}

	return res;
}
Пример #3
0
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
{
	calibrate_return result = calibrate_return_ok;

	mag_worker_data_t worker_data;

	worker_data.mavlink_log_pub = mavlink_log_pub;
	worker_data.done_count = 0;
	worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
	worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;

	// Collect: Right-side up, Left Side, Nose down
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;

	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
	usleep(100000);
	calibration_log_info(mavlink_log_pub,
		"[cal] %s side done, rotate to a different side",
		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
	usleep(100000);

	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		// Initialize to no subscription
		worker_data.sub_mag[cur_mag] = -1;

		// Initialize to no memory allocated
		worker_data.x[cur_mag] = NULL;
		worker_data.y[cur_mag] = NULL;
		worker_data.z[cur_mag] = NULL;
		worker_data.calibration_counter_total[cur_mag] = 0;
	}

	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;

	char str[30];

	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
			calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
			result = calibrate_return_error;
		}
	}


	// Setup subscriptions to mag sensors
	if (result == calibrate_return_ok) {

		// We should not try to subscribe if the topic doesn't actually exist and can be counted.
		const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));

		for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
			// Mag in this slot is available
			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);

#ifdef __PX4_QURT
			// For QURT respectively the driver framework, we need to get the device ID by copying one report.
			struct mag_report	mag_report;
			orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
			device_ids[cur_mag] = mag_report.device_id;
#endif
			if (worker_data.sub_mag[cur_mag] < 0) {
				calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
				result = calibrate_return_error;
				break;
			}

			if (device_ids[cur_mag] != 0) {
				// Get priority
				int32_t prio;
				orb_priority(worker_data.sub_mag[cur_mag], &prio);

				if (prio > device_prio_max) {
					device_prio_max = prio;
					device_id_primary = device_ids[cur_mag];
				}
			} else {
				calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
				result = calibrate_return_error;
				break;
			}
		}
	}

	// Limit update rate to get equally spaced measurements over time (in ms)
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available
				unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;

				//calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
				orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
			}
		}

	}

	if (result == calibrate_return_ok) {
		int cancel_sub  = calibrate_cancel_subscribe();

		result = calibrate_from_orientation(mavlink_log_pub,                    // uORB handle to write output
						    cancel_sub,                         // Subscription to vehicle_command for cancel support
						    worker_data.side_data_collected,    // Sides to calibrate
						    mag_calibration_worker,             // Calibration worker
						    &worker_data,			// Opaque data for calibration worked
						    true);				// true: lenient still detection
		calibrate_cancel_unsubscribe(cancel_sub);
	}

	// Close subscriptions
	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
		if (worker_data.sub_mag[cur_mag] >= 0) {
			px4_close(worker_data.sub_mag[cur_mag]);
		}
	}

	// Calculate calibration values for each mag


	float sphere_x[max_mags];
	float sphere_y[max_mags];
	float sphere_z[max_mags];
	float sphere_radius[max_mags];

	// Sphere fit the data to get calibration values
	if (result == calibrate_return_ok) {
		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				// Mag in this slot is available and we should have values for it to calibrate

				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
							 worker_data.calibration_counter_total[cur_mag],
							 100, 0.0f,
							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
							 &sphere_radius[cur_mag]);

				if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
					calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
					result = calibrate_return_error;
				}

				if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
					fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
					fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
					calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
					calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
						(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
					result = calibrate_return_ok;
				}
			}
		}
	}

	// Print uncalibrated data points
	if (result == calibrate_return_ok) {

		// DO NOT REMOVE! Critical validation data!

		// printf("RAW DATA:\n--------------------\n");
		// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

		// 	if (worker_data.calibration_counter_total[cur_mag] == 0) {
		// 		continue;
		// 	}

		// 	printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

		// 	for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
		// 		float x = worker_data.x[cur_mag][i];
		// 		float y = worker_data.y[cur_mag][i];
		// 		float z = worker_data.z[cur_mag][i];
		// 		printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
		// 	}

		// 	printf(">>>>>>>\n");
		// }

		// printf("CALIBRATED DATA:\n--------------------\n");
		// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {

		// 	if (worker_data.calibration_counter_total[cur_mag] == 0) {
		// 		continue;
		// 	}

		// 	printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);

		// 	for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
		// 		float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
		// 		float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
		// 		float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
		// 		printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
		// 	}

		// 	printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
		// 	printf(">>>>>>>\n");
		// }
	}

	// Data points are no longer needed
	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
		free(worker_data.x[cur_mag]);
		free(worker_data.y[cur_mag]);
		free(worker_data.z[cur_mag]);
	}

	if (result == calibrate_return_ok) {

		(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));

		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				struct mag_calibration_s mscale;
#ifndef __PX4_QURT
				int fd_mag = -1;

				// Set new scale
				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
				fd_mag = px4_open(str, 0);
				if (fd_mag < 0) {
					calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
					result = calibrate_return_error;
				}

				if (result == calibrate_return_ok) {
					if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
						calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
						result = calibrate_return_error;
					}
				}
#endif

				if (result == calibrate_return_ok) {
					mscale.x_offset = sphere_x[cur_mag];
					mscale.y_offset = sphere_y[cur_mag];
					mscale.z_offset = sphere_z[cur_mag];

#ifndef __PX4_QURT
					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
						calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
						result = calibrate_return_error;
					}
#endif
				}

#ifndef __PX4_QURT
				// Mag device no longer needed
				if (fd_mag >= 0) {
					px4_close(fd_mag);
				}
#endif

				if (result == calibrate_return_ok) {
					bool failed = false;

					/* set parameters */

					(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);

					// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
#endif

					if (failed) {
						calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
						result = calibrate_return_error;
					} else {
						calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
									     cur_mag,
									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
						calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
									     cur_mag,
									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
						usleep(200000);
					}
				}
			}
		}
	}

	return result;
}
Пример #4
0
int do_mag_calibration(int mavlink_fd)
{
	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
	mavlink_log_info(mavlink_fd, "don't move system");

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

	/* maximum 500 values */
	const unsigned int calibration_maxcount = 500;
	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);
	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);

		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");
			res = ERROR;
			return res;
		}
	} else {
		/* exit */
		return ERROR;
	}

	if (res == OK) {
		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
		struct mag_report mag;

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

		/* calibrate offsets */
		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
		unsigned poll_errcount = 0;

		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");

		calibration_counter = 0;

		while (hrt_absolute_time() < calibration_deadline &&
		       calibration_counter < calibration_maxcount) {

			/* wait blocking for new data */
			struct pollfd fds[1];
			fds[0].fd = sub_mag;
			fds[0].events = POLLIN;

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

			if (poll_ret > 0) {
				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++;

				if (calibration_counter % (calibration_maxcount / 20) == 0)
					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);

			} else {
				poll_errcount++;
			}

			if (poll_errcount > 1000) {
				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
				res = ERROR;
				break;
			}
		}

		close(sub_mag);
	}

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

	if (res == OK) {

		/* sphere fit */
		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);

		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
			mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
			res = ERROR;
		}
	}

	if (x != NULL)
		free(x);

	if (y != NULL)
		free(y);

	if (z != NULL)
		free(z);

	if (res == OK) {
		/* apply calibration and set parameters */
		struct mag_scale mscale;

		fd = open(MAG_DEVICE_PATH, 0);
		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);

		if (res != OK) {
			mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
		}

		if (res == OK) {
			mscale.x_offset = sphere_x;
			mscale.y_offset = sphere_y;
			mscale.z_offset = sphere_z;

			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);

			if (res != OK) {
				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
			}
		}

		close(fd);

		if (res == OK) {
			/* set parameters */
			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
				res = ERROR;

			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
				res = ERROR;

			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
				res = ERROR;

			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
				res = ERROR;

			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
				res = ERROR;

			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
				res = ERROR;

			if (res != OK) {
				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
			}

			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
		}

		if (res == OK) {
			/* auto-save to EEPROM */
			res = param_save_default();

			if (res != OK) {
				mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
			}
		}

		mavlink_log_info(mavlink_fd, "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, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
				 (double)mscale.y_scale, (double)mscale.z_scale);

		if (res == OK) {
			mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);

		} else {
			mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
		}
	}

	return res;
}