OrientationSensor.h

Go to the documentation of this file.

/*--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants
Licensee a limited, non-exclusive, non-transferable, royalty-free license
(without the right to sublicense) to use the API solely for the purpose of
Licensee's internal development efforts to develop applications for which
the API was provided.
 
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Hardware
File: OrientationSensor
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/Device.h>
#include <NDEVR/Matrix.h>
#include <QObject>
namespace NDEVR
{
    /**--------------------------------------------------------------------------------------------------
    \brief Used by OrientationSensor to display the current state of any available gyro
    \ingroup Hardware Magnetometer
    **/
    enum GyroState
    {
        e_no_gyro
        , e_gyro_invalid
        , e_gyro_valid
        , e_initializing
        , e_disabled
        , e_stabalizing_temp
        , e_calculating_drift
        , e_validating_drift
        , e_needs_reset
    };
 
    /**--------------------------------------------------------------------------------------------------
    \brief Used by OrientationSensor to display the current state of any available magnetometer
    \ingroup Hardware Magnetometer
    **/
    enum MagnetometerState
    {
        e_no_compass
        , e_compass_valid
        , e_compass_invalid
        , e_calculating_bias
    };
    /**--------------------------------------------------------------------------------------------------
    \brief Reports the state of an accelerometer within a OrientationSensor
    **/
    enum AccelerometerState
    {
        e_no_accelerometer
        , e_accelerometer_valid
        , e_accelerometer_invalid
    };
    class StationModel;
    class Calibration;
    class IMUCalibrator;
    class DesignObjectLookup;
    class MagneticCalibrationData;
 
    /**--------------------------------------------------------------------------------------------------
    \brief Used to report the orientation of a Hardware object within 3D space
    \ingroup Hardware Magnetometer Orientation Gyro Accelerometer
    **/
    class HARDWARE_API OrientationSensor : public Device
    {
        Q_OBJECT
    public:
        OrientationSensor(const Model& model, ProgressInfo* log, QObject* parent = nullptr);
        [[nodiscard]] virtual Time getGyroCalibrationTime() const;
        [[nodiscard]] virtual MagnetometerState magnetometerState() const { return m_magnetometer_state; };
        [[nodiscard]] virtual GyroState gyroState() const;
        [[nodiscard]] virtual Matrix<fltp08> lastOrientation() const;
        [[nodiscard]] static Matrix<fltp08, 3, 3> calculateDeviation(TimeSpan time, const Vector<4, fltp08>& start, const Vector<4, fltp08>& end);
        [[nodiscard]] static Angle<fltp08> calculateNorthFacing(const Matrix<fltp08, 3, 3>& offset_mat);
        [[nodiscard]] static Matrix<fltp08, 3, 3> offsetCalibration(TimeSpan time_since_calibration, const Matrix<fltp08, 3, 3>& current_orientation, const Matrix<fltp08, 3, 3>& dev_matrix);
        [[nodiscard]] virtual bool usingAccelerometer() const { return m_using_accelerometer; }
        [[nodiscard]] virtual bool usingCompass() const { return m_using_compass; }
        [[nodiscard]] virtual bool usingGyro() const { return m_using_gyro; }
        [[nodiscard]] virtual bool isGyroUpdating() const { return m_is_gyro_updating; }
        [[nodiscard]] virtual bool hasLockedYaw() const;
        [[nodiscard]] virtual bool hasLockedRoll() const;
        [[nodiscard]] const Vector<3, Angle<fltp08>>& driftRate() const { return m_angle_drift_rate; }
        [[nodiscard]] Vector<3, Angle<fltp08>> driftValue() const;
        [[nodiscard]] Vector<3, Angle<fltp08>> userSpecifiedOffsets() const { return m_user_specified_offsets; };
        [[nodiscard]] Vector<3, Angle<fltp08>> deviceOffsets() const { return m_device_specified_offsets; };
        [[nodiscard]] virtual bool isLocked() const { return m_is_locked; }
        [[nodiscard]] virtual TranslatedString calibrationMessage() const;
        void checkForSetMagneticDeclination();
        void setUserSpecifiedOffsets(const Vector<3, Angle<fltp08>>& user_specified_offsets) { m_user_specified_offsets = user_specified_offsets; }
        void setDeviceOffsets(Vector<3, Angle<fltp08>> device_offsets) { m_device_specified_offsets = device_offsets; }
        virtual bool requestAccelerationBiasLimit(fltp04 bias_limit) { UNUSED(bias_limit); return false; }
        virtual bool requestUsingAccelerometer(bool using_accelerometer);
        virtual Ray<3, fltp08> calibratedMagnetometerVector() const { return m_calibrated_magnetomer_vector; };
        virtual Ray<3, fltp08> rawMagnetometerVector() const { return m_raw_magnetometer_vector; };
        virtual Angle<fltp08> compassAngle() const { return Constant<Angle<fltp08>>::Invalid; };
        virtual bool requestUsingCompass(bool using_compass);
        virtual bool requestUsingGyro(bool using_gyro, bool reset_state);
        virtual bool requestGyroCalibration() { return false; }
        virtual void lockRoll(bool lock_roll, Angle<fltp08> roll = Constant<Angle<fltp08>>::Invalid, Angle<fltp08> yaw = Constant<Angle<fltp08>>::Invalid);
        virtual void lockYaw(bool lock_yaw, Angle<fltp08> lock_heading = Constant<Angle<fltp08>>::Invalid);
        virtual bool supportsFixed() const { return true; }
        virtual bool supportsModes() const { return false; }
        virtual bool supportsRodded() const { return true; }
        virtual void setDriftRate(const Vector<3, Angle<fltp08>>& drift_rate);
        virtual void setCalibrator(IMUCalibrator* imu_calibrator);
#if NDEVR_CALIBRATION
        virtual Calibration createCalibrationObject(DesignObjectLookup* lookup);
#endif
        virtual MagneticCalibrationData* magnetometerCalibrationObject() { return nullptr; }
        virtual bool requestMagnetometerCalibration(bool) { return false; }
        virtual bool requestApplyMagnetometerCalibration() { return false; }
        virtual bool needsMagnetometerCalibration() const { return false; }
        virtual bool hasCalibratedDrift() const { return m_has_calibrated_drift; }
        Vector<3, Angle<fltp08>> gravityAccelerationToAngle(const Vector<3, fltp08>& acceleration_vector);
        Vector<3, Angle<fltp08>> quaternionToAngle(const Vector<4, fltp08>& quaternion) const;
        void setFromGravityAcceleration(const Vector<3, fltp08>& acceleration_vector);
        virtual void setFromQuaternion(const Vector<4, fltp08>& quaternion);
        void setCalibrationOffset(const Matrix<fltp08>& offset);
        void createCalibrationOffset(const Matrix<fltp08>& map_from, const Matrix<fltp08>& map_to);
        void setFromAngles(const Vector<3, Angle<fltp08>>& angles);
        fltp04 calibrationPercent() const;
        virtual void clearCurrentDrift();
        IMUCalibrator* calibrator() const { return m_imu_calibrator; }
        [[nodiscard]] virtual Ray<3, fltp08> referenceDirection() const { return Ray<3, fltp08>(1.0, 0.0, 0.0); }
        [[nodiscard]] virtual UUID calibrationID() const { return m_calibration_id; };
    protected:
        [[nodiscard]] virtual Vector<3, Angle<fltp08>> lastOrientationAngle() const;
    signals:
        void driftRateUpdatedSignal();
        void magnetometerCalibrationUpdatedSignal();
        void deploymentModeOptionsUpdatedSignal();
        void calibrationChangedSignal();
    protected:
        Ray<3, fltp08> m_calibrated_magnetomer_vector = Constant<Ray<3, fltp08>>::Invalid;
        Ray<3, fltp08> m_raw_magnetometer_vector = Constant<Ray<3, fltp08>>::Invalid;
        Vector<3, Angle<fltp08>> m_tilt_angle;
        Vector<3, Angle<fltp08>> m_device_specified_offsets;
        Vector<3, Angle<fltp08>> m_user_specified_offsets;
        Matrix<fltp08> m_calibration_offset;
        Vector<3, Angle<fltp08>> m_angle_drift_rate;//per minute
        Time m_angle_drift_start_time;
        Time m_calibration_time;//The time when the current calibration was created
        fltp04 m_calibration_percent;
        Angle<fltp08> m_roll_lock;
        Angle<fltp08> m_yaw_lock;
        Angle<fltp08> m_roll_lock_reference_yaw;
        IMUCalibrator* m_imu_calibrator = nullptr;
        UUID m_calibration_id;
        GyroState m_gyro_state;
        MagnetometerState m_magnetometer_state;
        AccelerometerState m_acceleromter_state;
        bool m_using_gyro = false;
        bool m_using_compass = false;
        bool m_using_accelerometer = false;
        bool m_is_gyro_updating = false;
        bool m_is_locked = false;
        bool m_has_calibrated_drift = false;
        bool m_gyro_calibration_requested = false;
    };
}