Implementation of accelerometer calibration. More...

#include "accelerometer_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <fcntl.h>
#include <math.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <lib/ecl/geo/geo.h>
#include <conversion/rotation.h>
#include <parameters/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/Subscription.hpp>

Include dependency graph for accelerometer_calibration.cpp:

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

Functions
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	read_accelerometer_avg (int sensor_correction_sub, int(&subs)[max_accel_sens], float(&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)

int	mat_invert3 (float src[3][3], float dst[3][3])

calibrate_return	calculate_calibration_values (unsigned sensor, float(&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float(&accel_T)[max_accel_sens][3][3], float(&accel_offs)[max_accel_sens][3], float g)

int	do_accel_calibration (orb_advert_t *mavlink_log_pub)

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

int	do_level_calibration (orb_advert_t *mavlink_log_pub)

Variables
static const char *	sensor_name = "accel"

static int32_t	device_id [max_accel_sens]

static int	device_prio_max = 0

static int32_t	device_id_primary = 0

Detailed Description

Implementation of accelerometer calibration.

Transform acceleration vector to true orientation, scale and offset

===== Model ===== accel_corr = accel_T * (accel_raw - accel_offs)

accel_corr[3] - fully corrected acceleration vector in body frame accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform accel_raw[3] - raw acceleration vector accel_offs[3] - acceleration offset vector

===== Calibration =====

Reference vectors accel_corr_ref[6][3] = [ g 0 0 ] // nose up | -g 0 0 | // nose down | 0 g 0 | // left side down | 0 -g 0 | // right side down | 0 0 g | // on back [ 0 0 -g ] // level accel_raw_ref[6][3]

accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5

6 reference vectors * 3 axes = 18 equations 9 (accel_T) + 3 (accel_offs) = 12 unknown constants

Find accel_offs

accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2

Find accel_T

9 unknown constants need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations

accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2

Solve separate system for each row of accel_T:

accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2

A * x = b

x = [ accel_T[0][i] ] | accel_T[1][i] | [ accel_T[2][i] ]

b = [ accel_corr_ref[0][i] ] // One measurement per side is enough | accel_corr_ref[2][i] | [ accel_corr_ref[4][i] ]

a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2

Matrix A is common for all three systems: A = [ a[0][0] a[0][1] a[0][2] ] | a[2][0] a[2][1] a[2][2] | [ a[4][0] a[4][1] a[4][2] ]

x = A^-1 * b

accel_T = A^-1 * g g = 9.80665

===== Rotation =====

Calibrating using model: accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)

Actual correction: accel_corr = rot * accel_T * (accel_raw - accel_offs)

Known: accel_T_r, accel_offs_r, rot Unknown: accel_T, accel_offs

Solution: accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) => accel_T = rot^-1 * accel_T_r * rot => accel_offs = rot^-1 * accel_offs_r

Author: Anton Babushkin anton.nosp@m..bab.nosp@m.ushki.nosp@m.n@me.nosp@m..com

Definition in file accelerometer_calibration.cpp.

Function Documentation

◆ accel_calibration_worker()

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

static

Definition at line 457 of file accelerometer_calibration.cpp.

References accel_worker_data_t::accel_ref, CAL_QGC_PROGRESS_MSG, calibrate_return_ok, calibration_log_info, detect_orientation_str(), accel_worker_data_t::done_count, accel_worker_data_t::mavlink_log_pub, read_accelerometer_avg(), accel_worker_data_t::sensor_correction_sub, and accel_worker_data_t::subs.

Referenced by do_accel_calibration_measurements().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculate_calibration_values()

calibrate_return calculate_calibration_values	(	unsigned	sensor,
		float(&)	accel_ref[max_accel_sens][detect_orientation_side_count][3],
		float(&)	accel_T[max_accel_sens][3][3],
		float(&)	accel_offs[max_accel_sens][3],
		float	g
	)

Definition at line 756 of file accelerometer_calibration.cpp.

References calibrate_return_error, calibrate_return_ok, and mat_invert3().

Referenced by do_accel_calibration_measurements().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ do_accel_calibration()

int do_accel_calibration ( orb_advert_t * mavlink_log_pub )

< sensor thermal corrections

Definition at line 177 of file accelerometer_calibration.cpp.

References ACCEL_BASE_DEVICE_PATH, ACCELIOCSSCALE, CAL_ERROR_APPLY_CAL_MSG, CAL_ERROR_RESET_CAL_MSG, CAL_ERROR_SENSOR_MSG, CAL_ERROR_SET_PARAMS_MSG, CAL_QGC_DONE_MSG, CAL_QGC_FAILED_MSG, CAL_QGC_STARTED_MSG, calibrate_return_cancelled, calibrate_return_ok, calibration_log_critical, calibration_log_info, device_id, device_id_primary, do_accel_calibration_measurements(), fd, get_rot_matrix(), max_accel_sens, ORB_ID, param_find(), param_get(), param_notify_changes(), param_set_no_notification(), px4_close(), px4_ioctl(), px4_open(), sensor_name, matrix::Matrix< Type, M, N >::transpose(), accel_calibration_s::x_offset, accel_calibration_s::x_scale, accel_calibration_s::y_offset, accel_calibration_s::y_scale, accel_calibration_s::z_offset, and accel_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:

◆ do_accel_calibration_measurements()

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
	)

Definition at line 480 of file accelerometer_calibration.cpp.

References accel_calibration_worker(), accel_worker_data_t::accel_ref, calculate_calibration_values(), calibrate_cancel_subscribe(), calibrate_cancel_unsubscribe(), calibrate_from_orientation(), calibrate_return_error, calibrate_return_ok, calibration_log_critical, CONSTANTS_ONE_G, detect_orientation_side_count, sensor_accel_s::device_id, device_id, device_id_primary, device_prio_max, accel_worker_data_t::done_count, mavlink_log_pub, accel_worker_data_t::mavlink_log_pub, max_accel_sens, orb_copy(), orb_group_count(), ORB_ID, orb_priority(), orb_subscribe(), orb_subscribe_multi(), orb_unsubscribe(), px4_close(), accel_worker_data_t::sensor_correction_sub, accel_worker_data_t::subs, and sensor_accel_s::timestamp.

Referenced by do_accel_calibration().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ do_level_calibration()

int do_level_calibration ( orb_advert_t * mavlink_log_pub )

Definition at line 794 of file accelerometer_calibration.cpp.

References CAL_QGC_DONE_MSG, CAL_QGC_FAILED_MSG, CAL_QGC_PROGRESS_MSG, CAL_QGC_STARTED_MSG, calibration_log_critical, calibration_log_info, counter, f(), FLT_EPSILON, hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), orb_copy(), ORB_ID, orb_subscribe(), param_find(), param_get(), param_notify_changes(), param_set_no_notification(), matrix::Euler< Type >::phi(), px4_poll(), vehicle_attitude_s::q, start(), and matrix::Euler< Type >::theta().

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:

◆ mat_invert3()

int mat_invert3	(	float	src[3][3],
		float	dst[3][3]
	)

Definition at line 733 of file accelerometer_calibration.cpp.

References FLT_EPSILON.

Referenced by calculate_calibration_values().

Here is the caller graph for this function:

◆ read_accelerometer_avg()

calibrate_return read_accelerometer_avg	(	int	sensor_correction_sub,
		int(&)	subs[max_accel_sens],
		float(&)	accel_avg[max_accel_sens][detect_orientation_side_count][3],
		unsigned	orient,
		unsigned	samples_num
	)