accelerometer_calibration.cpp File Reference

Implementation of accelerometer calibration. More...

#include "accelerometer_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_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>

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)
int	do_level_calibration (orb_advert_t *mavlink_log_pub)

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

Function Documentation

do_accel_calibration()

int do_accel_calibration

(

orb_advert_t *

mavlink_log_pub

)

< sensor thermal corrections

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
	)

< sensor thermal corrections