PX4 Firmware
PX4 Autopilot Software http://px4.io
calibration_routines.cpp File Reference

Calibration routines implementations. More...

#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 <math.h>
#include <float.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <lib/ecl/geo/geo.h>
#include <string.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_tone_alarm.h>
#include "calibration_routines.h"
#include "calibration_messages.h"
#include "commander_helper.h"
Include dependency graph for calibration_routines.cpp:

Go to the source code of this file.

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)
 
enum detect_orientation_return detect_orientation (orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position)
 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_position)
 Perform calibration sequence which require a rest orientation detection prior to calibration. More...
 
int calibrate_cancel_subscribe ()
 Called at the beginning of calibration in order to subscribe to the cancel command. More...
 
void calibrate_cancel_unsubscribe (int cmd_sub)
 Called to cancel the subscription to the cancel command. More...
 
static void calibrate_answer_command (orb_advert_t *mavlink_log_pub, struct vehicle_command_s &cmd, unsigned result)
 
bool calibrate_cancel_check (orb_advert_t *mavlink_log_pub, int cancel_sub)
 Used to periodically check for a cancel command. More...
 

Detailed Description

Calibration routines implementations.

Author
Lorenz Meier lm@in.nosp@m.f.et.nosp@m.hz.ch

Definition in file calibration_routines.cpp.

Function Documentation

◆ calibrate_answer_command()

static void calibrate_answer_command ( orb_advert_t mavlink_log_pub,
struct vehicle_command_s cmd,
unsigned  result 
)
static

Definition at line 822 of file calibration_routines.cpp.

References vehicle_command_s::command, mavlink_log_critical, tune_negative(), and tune_positive().

Referenced by calibrate_cancel_check().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ 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_pubuORB 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_subCancel 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_pubuORB handle to write output to
cancel_subCancel subscription from calibration_cancel_subscribe
side_data_collectedSides for which data still needs calibration
calibration_workerWorker routine which performs the actual calibration
worker_dataOpaque 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_pubuORB handle to write output to
cancel_subCancel subscription from calibration_cancel_subscribe
accel_subOrb 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
orientationOrientation 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:

◆ 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
xpoint coordinates on the X axis
ypoint coordinates on the Y axis
zpoint coordinates on the Z axis
sizenumber of points
max_iterationsabort if maximum number of iterations have been reached. If unsure, set to 100.
deltaabort if error is below delta. If unsure, set to 0 to run max_iterations times.
sphere_xcoordinate of the sphere center on the X axis
sphere_ycoordinate of the sphere center on the Y axis
sphere_zcoordinate of the sphere center on the Z axis
sphere_radiussphere 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: