Пример #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
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;
}
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
	calibrate_return result = calibrate_return_ok;

	*active_sensors = 0;

	accel_worker_data_t worker_data;

	worker_data.mavlink_log_pub = mavlink_log_pub;
	worker_data.done_count = 0;

	bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };

	// Initialize subs to error condition so we know which ones are open and which are not
	for (size_t i=0; i<max_accel_sens; i++) {
		worker_data.subs[i] = -1;
	}

	uint64_t timestamps[max_accel_sens];

	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
	const unsigned accel_count = orb_group_count(ORB_ID(sensor_accel));
	for (unsigned i = 0; i < accel_count; i++) {
		worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
		if (worker_data.subs[i] < 0) {
			result = calibrate_return_error;
			break;
		}

#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI)
		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
		struct accel_report	accel_report;
		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
		device_id[i] = accel_report.device_id;
#endif
		/* store initial timestamp - used to infer which sensors are active */
		struct accel_report arp = {};
		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
		timestamps[i] = arp.timestamp;

		if (device_id[i] != 0) {
			// Get priority
			int32_t prio;
			orb_priority(worker_data.subs[i], &prio);

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

	if (result == calibrate_return_ok) {
		int cancel_sub = calibrate_cancel_subscribe();
		result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
		calibrate_cancel_unsubscribe(cancel_sub);
	}

	/* close all subscriptions */
	for (unsigned i = 0; i < max_accel_sens; i++) {
		if (worker_data.subs[i] >= 0) {
			/* figure out which sensors were active */
			struct accel_report arp = {};
			(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
			if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
				(*active_sensors)++;
			}
			px4_close(worker_data.subs[i]);
		}
	}

	if (result == calibrate_return_ok) {
		/* calculate offsets and transform matrix */
		for (unsigned i = 0; i < (*active_sensors); i++) {
			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);

			if (result != calibrate_return_ok) {
				calibration_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error");
				break;
			}
		}
	}

	return result;
}
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
	calibrate_return result = calibrate_return_ok;

	*active_sensors = 0;

	accel_worker_data_t worker_data;

	worker_data.mavlink_log_pub = mavlink_log_pub;
	worker_data.done_count = 0;

	bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };

	// Initialise sub to sensor thermal compensation data
	worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));

	// Initialize subs to error condition so we know which ones are open and which are not
	for (size_t i=0; i<max_accel_sens; i++) {
		worker_data.subs[i] = -1;
	}

	uint64_t timestamps[max_accel_sens] = {};

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

	// Warn that we will not calibrate more than max_accels accelerometers
	if (orb_accel_count > max_accel_sens) {
		calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens);
	}

	for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {

		// Lock in to correct ORB instance
		bool found_cur_accel = false;
		for(unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
			worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);

			struct accel_report report = {};
			orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report);

#ifdef __PX4_NUTTX

			// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
			// and match it up with the one from the uORB subscription, because the
			// instance ordering of uORB and the order of the FDs may not be the same.

			if(report.device_id == device_id[cur_accel]) {
				// Device IDs match, correct ORB instance for this accel
				found_cur_accel = true;
				// store initial timestamp - used to infer which sensors are active
				timestamps[cur_accel] = report.timestamp;
			} else {
				orb_unsubscribe(worker_data.subs[cur_accel]);
			}

#else

			// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
			device_id[cur_accel] = report.device_id;
			found_cur_accel = true;

#endif
		}

		if(!found_cur_accel) {
			calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
			result = calibrate_return_error;
			break;
		}

		if (device_id[cur_accel] != 0) {
			// Get priority
			int32_t prio;
			orb_priority(worker_data.subs[cur_accel], &prio);

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

	if (result == calibrate_return_ok) {
		int cancel_sub = calibrate_cancel_subscribe();
		result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
		calibrate_cancel_unsubscribe(cancel_sub);
	}

	/* close all subscriptions */
	for (unsigned i = 0; i < max_accel_sens; i++) {
		if (worker_data.subs[i] >= 0) {
			/* figure out which sensors were active */
			struct accel_report arp = {};
			(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
			if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
				(*active_sensors)++;
			}
			px4_close(worker_data.subs[i]);
		}
	}
	orb_unsubscribe(worker_data.sensor_correction_sub);

	if (result == calibrate_return_ok) {
		/* calculate offsets and transform matrix */
		for (unsigned i = 0; i < (*active_sensors); i++) {
			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);

			if (result != calibrate_return_ok) {
				calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
				break;
			}
		}
	}

	return result;
}
Пример #5
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: As defined by configuration
	// start with a full mask, all six bits set
	uint32_t cal_mask = (1 << 6) - 1;
	param_get(param_find("CAL_MAG_SIDES"), &cal_mask);

	calibration_sides = 0;

	for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) /
				  sizeof(worker_data.side_data_collected[0])); i++) {

		if ((cal_mask & (1 << i)) > 0) {
			// mark as missing
			worker_data.side_data_collected[i] = false;
			calibration_sides++;

		} else {
			// mark as completed from the beginning
			worker_data.side_data_collected[i] = true;

			calibration_log_info(mavlink_log_pub,
					     "[cal] %s side done, rotate to a different side",
					     detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
			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] = nullptr;
		worker_data.y[cur_mag] = nullptr;
		worker_data.z[cur_mag] = nullptr;
		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] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) {
			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));

		// Warn that we will not calibrate more than max_mags magnetometers
		if (mag_count > max_mags) {
			calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", mag_count, max_mags);
		}

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

#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
			// 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];
	float diag_x[max_mags];
	float diag_y[max_mags];
	float diag_z[max_mags];
	float offdiag_x[max_mags];
	float offdiag_y[max_mags];
	float offdiag_z[max_mags];

	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
		sphere_x[cur_mag] = 0.0f;
		sphere_y[cur_mag] = 0.0f;
		sphere_z[cur_mag] = 0.0f;
		sphere_radius[cur_mag] = 0.2f;
		diag_x[cur_mag] = 1.0f;
		diag_y[cur_mag] = 1.0f;
		diag_z[cur_mag] = 1.0f;
		offdiag_x[cur_mag] = 0.0f;
		offdiag_y[cur_mag] = 0.0f;
		offdiag_z[cur_mag] = 0.0f;
	}

	// 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

				ellipsoid_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],
							    &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag],
							    &offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);

				result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
							    sphere_radius[cur_mag],
							    diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
							    offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
							    mavlink_log_pub, cur_mag);

				if (result == calibrate_return_error) {
					break;
				}
			}
		}
	}

	// 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) {

		for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
			if (device_ids[cur_mag] != 0) {
				struct mag_calibration_s mscale;
				mscale.x_scale = 1.0;
				mscale.y_scale = 1.0;
				mscale.z_scale = 1.0;

#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
				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) != PX4_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];
					mscale.x_scale = diag_x[cur_mag];
					mscale.y_scale = diag_y[cur_mag];
					mscale.z_scale = diag_z[cur_mag];

#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)

					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) {
						calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
						result = calibrate_return_error;
					}

#endif
				}

#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)

				// 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 |= (PX4_OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
					failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
					failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
					failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));

					// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
					failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
					failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
					failed |= (PX4_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);
					}
				}
			}
		}

		// Trigger a param set on the last step so the whole
		// system updates
		(void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary));
	}

	return result;
}