OrientationSensor.h

/*--------------------------------------------------------------------------------------------
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
{
    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         
    };

    enum MagnetometerState
    {
          e_no_compass          
        , e_compass_valid       
        , e_compass_invalid     
        , e_calculating_bias    
    };

    enum AccelerometerState
    {
          e_no_accelerometer        
        , e_accelerometer_valid     
        , e_accelerometer_invalid   
    };

    enum LevelingState
    {
          e_not_used            
        , e_level_invalid       
        , e_level_warning       
        , e_level_valid         
    };
    class StationModel;         
    class CalibrationData;      
    class Calibration;          
    class IMUCalibrator;        
    class DesignObjectLookup;   

    class HARDWARE_API OrientationSensor : public Device
    {
        Q_OBJECT
    public:
        OrientationSensor(const Model& model,  LogPtr log, QObject* parent = nullptr);
        [[nodiscard]] virtual Time getGyroAlignmentTime() 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_magnetometer_dir; };
        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 requestGyroAlignment(bool reference_previous) { UNUSED(reference_previous); 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 setLevelingLimit(Angle<fltp08> limit);
        virtual void setUseGyro(bool use_gyro) { m_using_gyro = use_gyro; };
        virtual void clearLevelingLimit();
        virtual void setLevelingRange(Angle<fltp08> limit);
        virtual void clearLevelingRange();
        virtual LevelingState levelingState() const;
        Angle<fltp08> levelingLimit() const { return m_leveling_limit; }
        Angle<fltp08> levelingRange() const { return m_leveling_range; }
        virtual void setDriftRate(const Vector<3, Angle<fltp08>>& drift_rate);
        virtual void setCalibrator(IMUCalibrator* imu_calibrator);
        virtual bool usedForLeveling() const { return IsValid(m_leveling_limit) || IsValid(m_leveling_range); }
        [[nodiscard]] Angle<fltp08> magneticHeading() const;
#if NDEVR_CALIBRATION
        virtual Calibration createAlignmentObject(DesignObjectLookup* lookup, bool reference_previous);
#endif
        virtual CalibrationData* 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 alignmentID() const { return m_alignment_id; };
    protected:
        [[nodiscard]] virtual Vector<3, Angle<fltp08>> lastOrientationAngle() const;
    signals:
        void driftRateUpdatedSignal();          
        void magnetometerCalibrationUpdatedSignal();    
        void deploymentModeOptionsUpdatedSignal();  
        void calibrationChangedSignal();        
    protected:
        Ray<3, fltp08> m_raw_magnetometer_vector = Constant<Ray<3, fltp08>>::Invalid;   
        Ray<3, fltp08> m_raw_acceleration_vector = Constant<Ray<3, fltp08>>::Invalid;   
        Ray<3, fltp08> m_magnetometer_dir = Constant<Ray<3, fltp08>>::Invalid;          
        Ray<3, fltp08> m_acceleration_dir = 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;        
        Time m_angle_drift_start_time;                      
        Time m_alignment_time;                              
        fltp04 m_calibration_percent;                       
        Angle<fltp08> m_roll_lock;                          
        Angle<fltp08> m_yaw_lock;                           
        Angle<fltp08> m_leveling_limit = Constant<Angle<fltp08>>::Invalid;      
        Angle<fltp08> m_leveling_range = Constant<Angle<fltp08>>::Invalid;      
        Angle<fltp08> m_roll_lock_reference_yaw;            
        Angle<fltp08> m_magnetic_azimuth = Constant<Angle<fltp08>>::Invalid;    
        IMUCalibrator* m_imu_calibrator = nullptr;          
        UUID m_alignment_id;                                
        GyroState m_gyro_state;                             
        MagnetometerState m_magnetometer_state;             
        AccelerometerState m_accelerometer_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;          
        bool m_is_magnet_field_acceptable = false;          
    };
}