APMCompassCal.h 6.68 KB
Newer Older
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
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

#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];

57 58
    static const char*      rgCompassParams[3][4];

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 169 170 171 172 173 174 175 176
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;
177
    float               _rgSavedCompassOffsets[3][3];
178 179 180 181
};

#endif