calibration_routines.h File Reference

Go to the source code of 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.

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

Detailed Description

Author

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

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);

calibration_log_emergency

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

Value:

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

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);

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

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

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

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

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

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

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