1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef APMCompassCal_H
#define APMCompassCal_H
#include <QObject>
#include <QThread>
#include <QVector3D>
#include "QGCLoggingCategory.h"
#include "QGCMAVLink.h"
#include "Vehicle.h"
Q_DECLARE_LOGGING_CATEGORY(APMCompassCalLog)
class CalWorkerThread : public QThread
{
Q_OBJECT
public:
CalWorkerThread(Vehicle* vehicle, QObject* parent = NULL);
// Cancel currently in progress calibration
void cancel(void) { _cancel = true; }
// Overrides from QThread
void run(void) Q_DECL_FINAL;
static const unsigned max_mags = 3;
bool rgCompassAvailable[max_mags];
QMutex lastScaledImuMutex;
mavlink_raw_imu_t lastRawImu;
mavlink_scaled_imu_t rgLastScaledImu[max_mags];
static const char* rgCompassParams[3][4];
signals:
void vehicleTextMessage(int vehicleId, int compId, int severity, QString text);
private:
void _emitVehicleTextMessage(const QString& message);
// The routines below are based on the PX4 flight stack compass cal routines. Hence
// the PX4 Flight Stack coding style to maintain some level of code movement.
static const float mag_sphere_radius;
static const unsigned int calibration_sides; ///< The total number of sides
static const unsigned int calibration_total_points; ///< The total points per magnetometer
static const unsigned int calibraton_duration_seconds; ///< The total duration the routine is allowed to take
// The order of these cannot change since the calibration calculations depend on them in this order
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
};
static const unsigned detect_orientation_side_count = 6;
// Data passed to calibration worker routine
typedef struct {
unsigned done_count;
unsigned int calibration_points_perside;
unsigned int calibration_interval_perside_seconds;
uint64_t calibration_interval_perside_useconds;
unsigned int calibration_counter_total[max_mags];
bool side_data_collected[detect_orientation_side_count];
float* x[max_mags];
float* y[max_mags];
float* z[max_mags];
} mag_worker_data_t;
enum calibrate_return {
calibrate_return_ok,
calibrate_return_error,
calibrate_return_cancelled
};
/**
* Least-squares fit of a sphere to a set of points.
*
* Fits a sphere to a set of points on the sphere surface.
*
* @param x point coordinates on the X axis
* @param y point coordinates on the Y axis
* @param z point coordinates on the Z axis
* @param size number of points
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
* @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
* @param sphere_x coordinate of the sphere center on the X axis
* @param sphere_y coordinate of the sphere center on the Y axis
* @param sphere_z coordinate of the sphere center on the Z axis
* @param sphere_radius sphere radius
*
* @return 0 on success, 1 on failure
*/
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);
/// Wait for vehicle to become still and detect it's orientation
/// @return Returns detect_orientation_return according to orientation of still vehicle
enum detect_orientation_return detect_orientation(void);
/// Returns the human readable string representation of the orientation
/// @param orientation Orientation to return string for, "error" if buffer is too small
const char* detect_orientation_str(enum detect_orientation_return orientation);
/// Perform calibration sequence which require a rest orientation detection prior to calibration.
/// @return OK: Calibration succeeded, ERROR: Calibration failed
calibrate_return calibrate_from_orientation(
bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
void* worker_data); ///< Opaque data passed to worker routine
calibrate_return calibrate(void);
calibrate_return mag_calibration_worker(detect_orientation_return orientation, void* data);
unsigned progress_percentage(mag_worker_data_t* worker_data);
Vehicle* _vehicle;
bool _cancel;
};
// Used to calibrate APM Stack compass by simulating PX4 Flight Stack firmware compass cal
// on the ground station side of things.
class APMCompassCal : public QObject
{
Q_OBJECT
public:
APMCompassCal(void);
~APMCompassCal();
void setVehicle(Vehicle* vehicle);
void startCalibration(void);
void cancelCalibration(void);
signals:
void vehicleTextMessage(int vehicleId, int compId, int severity, QString text);
private slots:
void _handleMavlinkRawImu(mavlink_message_t message);
void _handleMavlinkScaledImu2(mavlink_message_t message);
void _handleMavlinkScaledImu3(mavlink_message_t message);
private:
void _setSensorTransmissionSpeed(bool fast);
void _stopCalibration(void);
void _emitVehicleTextMessage(const QString& message);
Vehicle* _vehicle;
CalWorkerThread* _calWorkerThread;
float _rgSavedCompassOffsets[3][3];
};
#endif