58 unsigned int size,
unsigned int max_iterations,
float delta,
float *sphere_x,
float *sphere_y,
float *sphere_z,
59 float *sphere_radius);
61 unsigned int size,
int max_iterations,
float delta,
float *offset_x,
float *offset_y,
float *offset_z,
62 float *sphere_radius,
float *diag_x,
float *diag_y,
float *diag_z,
float *offdiag_x,
float *offdiag_y,
64 int run_lm_sphere_fit(
const float x[],
const float y[],
const float z[],
float &_fitness,
float &_sphere_lambda,
65 unsigned int size,
float *offset_x,
float *offset_y,
float *offset_z,
66 float *sphere_radius,
float *diag_x,
float *diag_y,
float *diag_z,
float *offdiag_x,
float *offdiag_y,
68 int run_lm_ellipsoid_fit(
const float x[],
const float y[],
const float z[],
float &_fitness,
float &_sphere_lambda,
69 unsigned int size,
float *offset_x,
float *offset_y,
float *offset_z,
70 float *sphere_radius,
float *diag_x,
float *diag_y,
float *diag_z,
float *offdiag_x,
float *offdiag_y,
96 bool lenient_still_detection);
120 bool lenient_still_detection);
140 #define calibration_log_info(_pub, _text, ...) \ 142 mavlink_and_console_log_info(_pub, _text, ##__VA_ARGS__); \ 146 #define calibration_log_critical(_pub, _text, ...) \ 148 mavlink_log_critical(_pub, _text, ##__VA_ARGS__); \ 152 #define calibration_log_emergency(_pub, _text, ...) \ 154 mavlink_log_emergency(_pub, _text, ##__VA_ARGS__); \
bool inverse4x4(float m[], float invOut[])
static orb_advert_t * mavlink_log_pub
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.
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.
detect_orientation_return
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.
void calibrate_cancel_unsubscribe(int cancel_sub)
Called to cancel the subscription to the cancel command.
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)
static const unsigned max_accel_sens
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)
const char * detect_orientation_str(enum detect_orientation_return orientation)
Returns the human readable string representation of the orientation.
calibrate_return(* calibration_from_orientation_worker_t)(detect_orientation_return orientation, int cancel_sub, void *worker_data)
Opaque worker data.
bool mat_inverse(float *A, float *inv, uint8_t n)
bool inv(const SquareMatrix< Type, M > &A, SquareMatrix< Type, M > &inv)
inverse based on LU factorization with partial pivotting
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
Used to periodically check for a cancel command.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int calibrate_cancel_subscribe(void)
Called at the beginning of calibration in order to subscribe to the cancel command.
static const unsigned detect_orientation_side_count
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)