This graph shows which files directly or indirectly include this file:

Macros
#define	calibration_log_info(_pub, _text, ...)

#define	calibration_log_critical(_pub, _text, ...)

#define	calibration_log_emergency(_pub, _text, ...)

Typedefs
typedef calibrate_return(*	calibration_from_orientation_worker_t) (detect_orientation_return orientation, int cancel_sub, void *worker_data)
	Opaque worker data. More...

Enumerations
enum	detect_orientation_return { DETECT_ORIENTATION_TAIL_DOWN, DETECT_ORIENTATION_NOSE_DOWN, DETECT_ORIENTATION_LEFT, DETECT_ORIENTATION_RIGHT, DETECT_ORIENTATION_UPSIDE_DOWN, DETECT_ORIENTATION_RIGHTSIDE_UP, DETECT_ORIENTATION_ERROR }

enum	calibrate_return { calibrate_return_ok, calibrate_return_error, calibrate_return_cancelled }

Functions
int	sphere_fit_least_squares (const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float sphere_x, float sphere_y, float sphere_z, float sphere_radius)
	Least-squares fit of a sphere to a set of points. More...

int	ellipsoid_fit_least_squares (const float x[], const float y[], const float z[], unsigned int size, int max_iterations, float delta, 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)

int	run_lm_sphere_fit (const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, unsigned int size, 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)

int	run_lm_ellipsoid_fit (const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, unsigned int size, 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)

bool	inverse4x4 (float m[], float invOut[])

bool	mat_inverse (float A, float inv, uint8_t n)

enum detect_orientation_return	detect_orientation (orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_detection)
	Wait for vehicle to become still and detect it's orientation. More...

const char *	detect_orientation_str (enum detect_orientation_return orientation)
	Returns the human readable string representation of the orientation. More...

calibrate_return	calibrate_from_orientation (orb_advert_t mavlink_log_pub, int cancel_sub, bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, void worker_data, bool lenient_still_detection)
	Perform calibration sequence which require a rest orientation detection prior to calibration. More...

int	calibrate_cancel_subscribe (void)
	Called at the beginning of calibration in order to subscribe to the cancel command. More...

void	calibrate_cancel_unsubscribe (int cancel_sub)
	Called to cancel the subscription to the cancel command. More...

bool	calibrate_cancel_check (orb_advert_t *mavlink_log_pub, int cancel_sub)
	Used to periodically check for a cancel command. More...

Variables
static const unsigned	max_accel_sens = 3

static const unsigned	detect_orientation_side_count = 6

Detailed Description

Author: Don Gagne don@t.nosp@m.hega.nosp@m.gnes..nosp@m.com

Definition in file calibration_routines.h.

Macro Definition Documentation

◆ calibration_log_critical

#define calibration_log_critical	(	_pub,
		_text,
		...
	)

Value:

do { \
        mavlink_log_critical(_pub, _text, ##__VA_ARGS__); \
        px4_usleep(10000); \
    } while(0);

Definition at line 146 of file calibration_routines.h.

Referenced by calibrate_from_orientation(), check_calibration_result(), detect_orientation(), do_accel_calibration(), do_accel_calibration_measurements(), do_airspeed_calibration(), do_esc_calibration(), do_gyro_calibration(), do_level_calibration(), do_mag_calibration(), feedback_calibration_failed(), gyro_calibration_worker(), mag_calibrate_all(), and mag_calibration_worker().

◆ calibration_log_emergency

#define calibration_log_emergency	(	_pub,
		_text,
		...
	)

Value:

do { \
        mavlink_log_emergency(_pub, _text, ##__VA_ARGS__); \
        px4_usleep(10000); \
    } while(0);

Definition at line 152 of file calibration_routines.h.

Referenced by check_calibration_result().

◆ calibration_log_info

#define calibration_log_info	(	_pub,
		_text,
		...
	)

Value:

do { \
        mavlink_and_console_log_info(_pub, _text, ##__VA_ARGS__); \
        px4_usleep(10000); \
    } while(0);

Definition at line 140 of file calibration_routines.h.

Referenced by accel_calibration_worker(), calibrate_from_orientation(), detect_orientation(), do_accel_calibration(), do_airspeed_calibration(), do_esc_calibration(), do_gyro_calibration(), do_level_calibration(), do_mag_calibration(), gyro_calibration_worker(), mag_calibrate_all(), and mag_calibration_worker().

Typedef Documentation

◆ calibration_from_orientation_worker_t

typedef calibrate_return(* calibration_from_orientation_worker_t) (detect_orientation_return orientation, int cancel_sub, void *worker_data)

Opaque worker data.

Definition at line 108 of file calibration_routines.h.

Enumeration Type Documentation

◆ calibrate_return

enum calibrate_return

Enumerator
calibrate_return_ok
calibrate_return_error
calibrate_return_cancelled

Definition at line 102 of file calibration_routines.h.

◆ detect_orientation_return

enum detect_orientation_return

Enumerator
DETECT_ORIENTATION_TAIL_DOWN
DETECT_ORIENTATION_NOSE_DOWN
DETECT_ORIENTATION_LEFT
DETECT_ORIENTATION_RIGHT
DETECT_ORIENTATION_UPSIDE_DOWN
DETECT_ORIENTATION_RIGHTSIDE_UP
DETECT_ORIENTATION_ERROR

Definition at line 79 of file calibration_routines.h.

Function Documentation

◆ calibrate_cancel_check()

bool calibrate_cancel_check	(	orb_advert_t *	mavlink_log_pub,
		int	cancel_sub
	)

Used to periodically check for a cancel command.

Cancel subcription fromcalibration_cancel_subscribe

Parameters

mavlink_log_pub uORB handle to write output to

Definition at line 839 of file calibration_routines.cpp.

References CAL_QGC_CANCELLED_MSG, calibrate_answer_command(), vehicle_command_s::command, vehicle_command_s::from_external, mavlink_log_critical, orb_copy(), ORB_ID, vehicle_command_s::param1, vehicle_command_s::param2, vehicle_command_s::param3, vehicle_command_s::param4, vehicle_command_s::param5, vehicle_command_s::param6, and px4_poll().

Referenced by calibrate_from_orientation(), do_airspeed_calibration(), gyro_calibration_worker(), and mag_calibration_worker().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calibrate_cancel_subscribe()

int calibrate_cancel_subscribe ( void )

Called at the beginning of calibration in order to subscribe to the cancel command.

Returns: Handle to vehicle_command subscription

Definition at line 800 of file calibration_routines.cpp.

References orb_check(), orb_copy(), ORB_ID, and orb_subscribe().

Referenced by do_accel_calibration_measurements(), do_airspeed_calibration(), do_gyro_calibration(), and mag_calibrate_all().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calibrate_cancel_unsubscribe()

void calibrate_cancel_unsubscribe ( int cancel_sub )

Called to cancel the subscription to the cancel command.

Parameters

cancel_sub Cancel subcription from calibration_cancel_subscribe

Definition at line 817 of file calibration_routines.cpp.

References orb_unsubscribe().

Referenced by do_accel_calibration_measurements(), do_airspeed_calibration(), do_gyro_calibration(), and mag_calibrate_all().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calibrate_from_orientation()

calibrate_return calibrate_from_orientation	(	orb_advert_t *	mavlink_log_pub,
		int	cancel_sub,
		bool	side_data_collected[detect_orientation_side_count],
		calibration_from_orientation_worker_t	calibration_worker,
		void *	worker_data,
		bool	lenient_still_detection
	)

Perform calibration sequence which require a rest orientation detection prior to calibration.

Returns: OK: Calibration succeeded, ERROR: Calibration failed true: Use more lenient still position detection

Parameters

mavlink_log_pub	uORB handle to write output to
cancel_sub	Cancel subscription from calibration_cancel_subscribe
side_data_collected	Sides for which data still needs calibration
calibration_worker	Worker routine which performs the actual calibration
worker_data	Opaque data passed to worker routine

Definition at line 683 of file calibration_routines.cpp.

References CAL_QGC_FAILED_MSG, CAL_QGC_ORIENTATION_DETECTED_MSG, CAL_QGC_SIDE_DONE_MSG, calibrate_cancel_check(), calibrate_return_cancelled, calibrate_return_error, calibrate_return_ok, calibration_log_critical, calibration_log_info, detect_orientation(), DETECT_ORIENTATION_ERROR, detect_orientation_side_count, detect_orientation_str(), ORB_ID, orb_subscribe(), px4_close(), rgbled_set_color_and_mode(), set_tune(), TONE_NOTIFY_NEGATIVE_TUNE, and TONE_NOTIFY_NEUTRAL_TUNE.

Referenced by do_accel_calibration_measurements(), and mag_calibrate_all().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ detect_orientation()

enum detect_orientation_return detect_orientation	(	orb_advert_t *	mavlink_log_pub,
		int	cancel_sub,
		int	accel_sub,
		bool	lenient_still_detection
	)

Wait for vehicle to become still and detect it's orientation.

Returns: Returns detect_orientation_return according to orientation when vehicle and ready for measurements true: Use more lenient still position detection

Parameters

mavlink_log_pub	uORB handle to write output to
cancel_sub	Cancel subscription from calibration_cancel_subscribe
accel_sub	Orb subcription to accel sensor

Definition at line 524 of file calibration_routines.cpp.

References sensor_combined_s::accelerometer_m_s2, CAL_ERROR_SENSOR_MSG, calibration_log_critical, calibration_log_info, CONSTANTS_ONE_G, DETECT_ORIENTATION_ERROR, DETECT_ORIENTATION_LEFT, DETECT_ORIENTATION_NOSE_DOWN, DETECT_ORIENTATION_RIGHT, DETECT_ORIENTATION_RIGHTSIDE_UP, DETECT_ORIENTATION_TAIL_DOWN, DETECT_ORIENTATION_UPSIDE_DOWN, dt, f(), hrt_absolute_time(), hrt_abstime, orb_copy(), ORB_ID, and px4_poll().

Referenced by calibrate_from_orientation().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ detect_orientation_str()

const char* detect_orientation_str ( enum detect_orientation_return orientation )

Returns the human readable string representation of the orientation.

Parameters

orientation Orientation to return string for, "error" if buffer is too small

Definition at line 668 of file calibration_routines.cpp.

Referenced by accel_calibration_worker(), calibrate_from_orientation(), mag_calibrate_all(), and mag_calibration_worker().

Here is the caller graph for this function:

◆ ellipsoid_fit_least_squares()

int ellipsoid_fit_least_squares	(	const float	x[],
		const float	y[],
		const float	z[],
		unsigned int	size,
		int	max_iterations,
		float	delta,
		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
	)

Definition at line 240 of file calibration_routines.cpp.

References run_lm_ellipsoid_fit(), and run_lm_sphere_fit().

Referenced by mag_calibrate_all().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ inverse4x4()

bool inverse4x4	(	float	m[],
		float	invOut[]
	)

Definition at line 260 of file matrix_alg.cpp.

References f(), and matrix::inv().

Here is the call graph for this function:

◆ mat_inverse()

bool mat_inverse	(	float *	A,
		float *	inv,
		uint8_t	n
	)

Definition at line 215 of file matrix_alg.cpp.

References mat_back_sub(), mat_forward_sub(), mat_LU_decompose(), mat_mul(), and P.

Referenced by run_lm_ellipsoid_fit().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_lm_ellipsoid_fit()

int run_lm_ellipsoid_fit	(	const float	x[],
		const float	y[],
		const float	z[],
		float &	_fitness,
		float &	_sphere_lambda,
		unsigned int	size,
		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
	)

Definition at line 388 of file calibration_routines.cpp.

References mat_inverse().

Referenced by ellipsoid_fit_least_squares().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_lm_sphere_fit()

int run_lm_sphere_fit	(	const float	x[],
		const float	y[],
		const float	z[],
		float &	_fitness,
		float &	_sphere_lambda,
		unsigned int	size,
		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
	)

Definition at line 264 of file calibration_routines.cpp.

References matrix::SquareMatrix< Type, M >::I().

Referenced by ellipsoid_fit_least_squares().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sphere_fit_least_squares()

int sphere_fit_least_squares	(	const float	x[],
		const float	y[],
		const float	z[],
		unsigned int	size,
		unsigned int	max_iterations,
		float	delta,
		float *	sphere_x,
		float *	sphere_y,
		float *	sphere_z,
		float *	sphere_radius
	)

Least-squares fit of a sphere to a set of points.

Fits a sphere to a set of points on the sphere surface.

Parameters

x	point coordinates on the X axis
y	point coordinates on the Y axis
z	point coordinates on the Z axis
size	number of points
max_iterations	abort if maximum number of iterations have been reached. If unsure, set to 100.
delta	abort if error is below delta. If unsure, set to 0 to run max_iterations times.
sphere_x	coordinate of the sphere center on the X axis
sphere_y	coordinate of the sphere center on the Y axis
sphere_z	coordinate of the sphere center on the Z axis
sphere_radius	sphere radius

Returns: 0 on success, 1 on failure

Definition at line 65 of file calibration_routines.cpp.

References f(), and FLT_EPSILON.

Here is the call graph for this function:

Variable Documentation

◆ detect_orientation_side_count

const unsigned detect_orientation_side_count = 6

static

Definition at line 88 of file calibration_routines.h.

Referenced by calibrate_from_orientation(), and do_accel_calibration_measurements().

◆ max_accel_sens

const unsigned max_accel_sens = 3

static

Definition at line 76 of file calibration_routines.h.

Referenced by do_accel_calibration(), do_accel_calibration_measurements(), and read_accelerometer_avg().

Macros

Typedefs

Enumerations

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ calibration_log_critical

◆ calibration_log_emergency

◆ calibration_log_info

Typedef Documentation

◆ calibration_from_orientation_worker_t

Enumeration Type Documentation

◆ calibrate_return

◆ detect_orientation_return

Function Documentation

◆ calibrate_cancel_check()

◆ calibrate_cancel_subscribe()

◆ calibrate_cancel_unsubscribe()

◆ calibrate_from_orientation()

◆ detect_orientation()

◆ detect_orientation_str()

◆ ellipsoid_fit_least_squares()

◆ inverse4x4()

◆ mat_inverse()

◆ run_lm_ellipsoid_fit()

◆ run_lm_sphere_fit()

◆ sphere_fit_least_squares()

Variable Documentation

◆ detect_orientation_side_count

◆ max_accel_sens