PX4 Firmware
PX4 Autopilot Software http://px4.io
calibration_routines.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /// @file calibration_routines.h
35 /// @author Don Gagne <don@thegagnes.com>
36 
37 #pragma once
38 
39 /**
40  * Least-squares fit of a sphere to a set of points.
41  *
42  * Fits a sphere to a set of points on the sphere surface.
43  *
44  * @param x point coordinates on the X axis
45  * @param y point coordinates on the Y axis
46  * @param z point coordinates on the Z axis
47  * @param size number of points
48  * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
49  * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
50  * @param sphere_x coordinate of the sphere center on the X axis
51  * @param sphere_y coordinate of the sphere center on the Y axis
52  * @param sphere_z coordinate of the sphere center on the Z axis
53  * @param sphere_radius sphere radius
54  *
55  * @return 0 on success, 1 on failure
56  */
57 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
58  unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
59  float *sphere_radius);
60 int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
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,
63  float *offdiag_z);
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,
67  float *offdiag_z);
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,
71  float *offdiag_z);
72 bool inverse4x4(float m[], float invOut[]);
73 bool mat_inverse(float *A, float *inv, uint8_t n);
74 
75 // FIXME: Change the name
76 static const unsigned max_accel_sens = 3;
77 
78 // The order of these cannot change since the calibration calculations depend on them in this order
87 };
88 static const unsigned detect_orientation_side_count = 6;
89 
90 /// Wait for vehicle to become still and detect it's orientation
91 /// @return Returns detect_orientation_return according to orientation when vehicle
92 /// and ready for measurements
93 enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
94  int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
95  int accel_sub, ///< Orb subcription to accel sensor
96  bool lenient_still_detection); ///< true: Use more lenient still position detection
97 
98 /// Returns the human readable string representation of the orientation
99 /// @param orientation Orientation to return string for, "error" if buffer is too small
100 const char *detect_orientation_str(enum detect_orientation_return orientation);
101 
106 };
107 
109  orientation, ///< Orientation which was detected
110  int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
111  void *worker_data); ///< Opaque worker data
112 
113 /// Perform calibration sequence which require a rest orientation detection prior to calibration.
114 /// @return OK: Calibration succeeded, ERROR: Calibration failed
115 calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
116  int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
117  bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
118  calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration
119  void *worker_data, ///< Opaque data passed to worker routine
120  bool lenient_still_detection); ///< true: Use more lenient still position detection
121 
122 /// Called at the beginning of calibration in order to subscribe to the cancel command
123 /// @return Handle to vehicle_command subscription
125 
126 /// Called to cancel the subscription to the cancel command
127 /// @param cancel_sub Cancel subcription from calibration_cancel_subscribe
128 void calibrate_cancel_unsubscribe(int cancel_sub);
129 
130 /// Used to periodically check for a cancel command
131 bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
132  int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe
133 
134 
135 // TODO FIXME: below are workarounds for QGC. The issue is that sometimes
136 // a mavlink log message is overwritten by the following one. A workaround
137 // is to wait for some time after publishing each message and hope that it
138 // gets sent out in the meantime.
139 
140 #define calibration_log_info(_pub, _text, ...) \
141  do { \
142  mavlink_and_console_log_info(_pub, _text, ##__VA_ARGS__); \
143  px4_usleep(10000); \
144  } while(0);
145 
146 #define calibration_log_critical(_pub, _text, ...) \
147  do { \
148  mavlink_log_critical(_pub, _text, ##__VA_ARGS__); \
149  px4_usleep(10000); \
150  } while(0);
151 
152 #define calibration_log_emergency(_pub, _text, ...) \
153  do { \
154  mavlink_log_emergency(_pub, _text, ##__VA_ARGS__); \
155  px4_usleep(10000); \
156  } while(0);
bool inverse4x4(float m[], float invOut[])
Definition: matrix_alg.cpp:260
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&#39;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)
Definition: matrix_alg.cpp:215
bool inv(const SquareMatrix< Type, M > &A, SquareMatrix< Type, M > &inv)
inverse based on LU factorization with partial pivotting
calibrate_return
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.
Definition: uORB.h:134
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)