Magnetometer calibration routine. More...

#include "mag_calibration.h"
#include "commander_helper.h"
#include "calibration_routines.h"
#include "calibration_messages.h"
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_tone_alarm.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
#include <uORB/topics/sensor_combined.h>

Include dependency graph for mag_calibration.cpp:

Classes
struct	mag_worker_data_t
	Data passed to calibration worker routine. More...

Functions
calibrate_return	mag_calibrate_all (orb_advert_t *mavlink_log_pub)

int	do_mag_calibration (orb_advert_t *mavlink_log_pub)

static bool	reject_sample (float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)

static unsigned	progress_percentage (mag_worker_data_t *worker_data)

static calibrate_return	check_calibration_result (float offset_x, float offset_y, float offset_z, float sphere_radius, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, orb_advert_t *mavlink_log_pub, size_t cur_mag)

static calibrate_return	mag_calibration_worker (detect_orientation_return orientation, int cancel_sub, void *data)

Variables
static const char *	sensor_name = "mag"

static constexpr unsigned	max_mags = 4

static constexpr float	mag_sphere_radius = 0.2f

static unsigned int	calibration_sides = 6
	The total number of sides. More...

static constexpr unsigned int	calibration_total_points = 240
	The total points per magnetometer. More...

static constexpr unsigned int	calibraton_duration_seconds = 42
	The total duration the routine is allowed to take. More...

static constexpr float	MAG_MAX_OFFSET_LEN
	The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions. More...

int32_t	device_ids [max_mags]

bool	internal [max_mags]

int	device_prio_max = 0

int32_t	device_id_primary = 0

static unsigned	_last_mag_progress = 0

Detailed Description

Magnetometer calibration routine.

Definition in file mag_calibration.cpp.

Function Documentation

◆ check_calibration_result()

static calibrate_return check_calibration_result	(	float	offset_x,
		float	offset_y,
		float	offset_z,
		float	sphere_radius,
		float	diag_x,
		float	diag_y,
		float	diag_z,
		float	offdiag_x,
		float	offdiag_y,
		float	offdiag_z,
		orb_advert_t *	mavlink_log_pub,
		size_t	cur_mag
	)

static

Definition at line 296 of file mag_calibration.cpp.

References calibrate_return_error, calibrate_return_ok, calibration_log_critical, calibration_log_emergency, f(), and MAG_MAX_OFFSET_LEN.

Referenced by mag_calibrate_all().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ do_mag_calibration()

int do_mag_calibration ( orb_advert_t * mavlink_log_pub )

Definition at line 99 of file mag_calibration.cpp.

References _last_mag_progress, CAL_ERROR_RESET_CAL_MSG, CAL_QGC_DONE_MSG, CAL_QGC_FAILED_MSG, CAL_QGC_PROGRESS_MSG, CAL_QGC_STARTED_MSG, calibrate_return_cancelled, calibrate_return_ok, calibration_log_critical, calibration_log_info, device_ids, fd, MAG_BASE_DEVICE_PATH, mag_calibrate_all(), MAGIOCCALIBRATE, MAGIOCGEXTERNAL, MAGIOCSSCALE, max_mags, param_find(), param_notify_changes(), param_set_no_notification(), px4_close(), px4_ioctl(), px4_open(), sensor_name, mag_calibration_s::x_offset, mag_calibration_s::x_scale, mag_calibration_s::y_offset, mag_calibration_s::y_scale, mag_calibration_s::z_offset, and mag_calibration_s::z_scale.

Referenced by commander_low_prio_loop(), and commander_main().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mag_calibrate_all()

calibrate_return mag_calibrate_all ( orb_advert_t * mavlink_log_pub )

Definition at line 529 of file mag_calibration.cpp.

References CAL_ERROR_APPLY_CAL_MSG, CAL_ERROR_SET_PARAMS_MSG, calibrate_cancel_subscribe(), calibrate_cancel_unsubscribe(), calibrate_from_orientation(), calibrate_return_error, calibrate_return_ok, mag_worker_data_t::calibration_counter_total, mag_worker_data_t::calibration_interval_perside_seconds, mag_worker_data_t::calibration_interval_perside_useconds, calibration_log_critical, calibration_log_info, mag_worker_data_t::calibration_points_perside, calibration_sides, calibration_total_points, calibraton_duration_seconds, check_calibration_result(), detect_orientation_str(), device_id_primary, device_ids, device_prio_max, mag_worker_data_t::done_count, ellipsoid_fit_least_squares(), MAG_BASE_DEVICE_PATH, mag_calibration_worker(), mag_report, MAGIOCGSCALE, MAGIOCSSCALE, mavlink_log_pub, mag_worker_data_t::mavlink_log_pub, max_mags, orb_copy(), orb_group_count(), ORB_ID, orb_priority(), orb_set_interval(), orb_subscribe_multi(), orb_unsubscribe(), param_find(), param_get(), param_set(), param_set_no_notification(), px4_close(), px4_ioctl(), px4_open(), mag_worker_data_t::side_data_collected, mag_worker_data_t::sub_mag, mag_worker_data_t::x, mag_calibration_s::x_offset, mag_calibration_s::x_scale, mag_worker_data_t::y, mag_calibration_s::y_offset, mag_calibration_s::y_scale, mag_worker_data_t::z, mag_calibration_s::z_offset, and mag_calibration_s::z_scale.

Referenced by do_mag_calibration().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mag_calibration_worker()

static calibrate_return mag_calibration_worker	(	detect_orientation_return	orientation,
		int	cancel_sub,
		void *	data
	)

static

Definition at line 347 of file mag_calibration.cpp.

Referenced by mag_calibrate_all().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ progress_percentage()

static unsigned progress_percentage ( mag_worker_data_t * worker_data )

static

Definition at line 289 of file mag_calibration.cpp.

References calibration_sides, and mag_worker_data_t::done_count.

Referenced by mag_calibration_worker().

Here is the caller graph for this function:

◆ reject_sample()

static bool reject_sample	(	float	sx,
		float	sy,
		float	sz,
		float	x[],
		float	y[],
		float	z[],
		unsigned	count,
		unsigned	max_count
	)

static

Definition at line 270 of file mag_calibration.cpp.

References f(), and mag_sphere_radius.

Referenced by mag_calibration_worker().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ _last_mag_progress

unsigned _last_mag_progress = 0

static

Definition at line 79 of file mag_calibration.cpp.

Referenced by do_mag_calibration(), and mag_calibration_worker().

◆ calibration_sides

unsigned int calibration_sides = 6

static

The total number of sides.

Definition at line 68 of file mag_calibration.cpp.

Referenced by mag_calibrate_all(), mag_calibration_worker(), and progress_percentage().

◆ calibration_total_points

constexpr unsigned int calibration_total_points = 240

static

The total points per magnetometer.

Definition at line 69 of file mag_calibration.cpp.

Referenced by mag_calibrate_all().

◆ calibraton_duration_seconds

constexpr unsigned int calibraton_duration_seconds = 42

static

The total duration the routine is allowed to take.

Definition at line 70 of file mag_calibration.cpp.

Referenced by mag_calibrate_all().

◆ device_id_primary

int32_t device_id_primary = 0

Definition at line 78 of file mag_calibration.cpp.

Referenced by do_gyro_calibration(), and mag_calibrate_all().

◆ device_ids

int32_t device_ids[max_mags]

Definition at line 75 of file mag_calibration.cpp.

Referenced by do_mag_calibration(), mag_calibrate_all(), and mag_calibration_worker().

◆ device_prio_max

int device_prio_max = 0

Definition at line 77 of file mag_calibration.cpp.

Referenced by do_gyro_calibration(), and mag_calibrate_all().

◆ internal

bool internal[max_mags]

Definition at line 76 of file mag_calibration.cpp.

◆ MAG_MAX_OFFSET_LEN

constexpr float MAG_MAX_OFFSET_LEN

static

Initial value:

=

1.3f

The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions.

Definition at line 72 of file mag_calibration.cpp.

Referenced by check_calibration_result().

◆ mag_sphere_radius

constexpr float mag_sphere_radius = 0.2f

static

Definition at line 67 of file mag_calibration.cpp.

Referenced by reject_sample().

◆ max_mags

constexpr unsigned max_mags = 4

static

Definition at line 66 of file mag_calibration.cpp.

Referenced by do_mag_calibration(), mag_calibrate_all(), and mag_calibration_worker().

◆ sensor_name

const char* sensor_name = "mag"

static

Definition at line 65 of file mag_calibration.cpp.

Referenced by do_mag_calibration().

Classes

Functions

Variables

Detailed Description

Function Documentation

◆ check_calibration_result()

◆ do_mag_calibration()

◆ mag_calibrate_all()

◆ mag_calibration_worker()

◆ progress_percentage()

◆ reject_sample()

Variable Documentation

◆ _last_mag_progress

◆ calibration_sides

◆ calibration_total_points

◆ calibraton_duration_seconds

◆ device_id_primary

◆ device_ids

◆ device_prio_max

◆ internal

◆ MAG_MAX_OFFSET_LEN

◆ mag_sphere_radius

◆ max_mags

◆ sensor_name