sensor_fusion.h

Go to the documentation of this file.

/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
// Sensor fusion requires a fairly extensive set of data structures, which are
// defined in this file.  The top level structure is shown near the bottom.  The
// size of this structure (SensorFusionGlobals) varies dramatically as a function
// of which fusion variations have been selected in build.h.
 
#ifndef SENSOR_FUSION_H
#define SENSOR_FUSION_H
 
#include <Arduino.h>
 
#ifdef __cplusplus
extern "C" {
#endif
 
// Standard includes
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
 
#include "board.h"                      // Hardware-specific details (e.g. particular sensor ICs)
#include "build.h"                      // This is where the build parameters are defined
#include "driver_sensors_types.h"       // Typedefs for the sensor hardware
#include "magnetic.h"                   // Magnetic calibration functions/structures
#include "matrix.h"                     // Matrix math
#include "orientation.h"                // Functions for manipulating orientations
#include "precisionAccelerometer.h"     // Accel calibration functions/structures
 
typedef enum quaternion {
    Q3,         
    Q3M,        
    Q3G,        
    Q6MA,       
    Q6AG,       
    Q9          
} quaternion_type;
 
#define CHX 0   
#define CHY 1   
#define CHZ 2   
 
// booleans
#ifndef true
#define true 1  
#endif
#ifndef false
#define false 0 
#endif
 
//#define B0 (1 << 0)
//#define B1 (1 << 1)
//#define B2 (1 << 2)
//#define B3 (1 << 3)
 
#ifndef PI
    #define PI 3.141592654F             
#endif
#define PIOVER2 1.570796327F            
#define FPIOVER180 0.01745329251994F            
#define F180OVERPI 57.2957795130823F            
#define F180OVERPISQ 3282.8063500117F           
#define ONETHIRD 0.33333333F            
#define ONESIXTH 0.166666667F           
#define ONESIXTEENTH 0.0625F            
#define ONEOVER12 0.083333333F          
#define ONEOVER48 0.02083333333F        
#define ONEOVER120 0.0083333333F        
#define ONEOVER3840 0.0002604166667F            
#define ONEOVERSQRT2 0.707106781F       
#define SQRT15OVER4  0.968245837F       
#define GTOMSEC2 9.80665            
 
// Placeholder structures (redefined later, but needed now for pointer definitions)
struct SensorFusionGlobals;                     
struct StatusSubsystem;                         
struct PhysicalSensor;                          
struct ControlSubsystem;                        
 
typedef enum {                                  
    OFF,                                    
    INITIALIZING,                           
    LOWPOWER,                               
    NORMAL,                                 
  RECEIVING_WIRED,                        
  RECEIVING_WIRELESS,                     
    HARD_FAULT,                             
  SOFT_FAULT                              
} fusion_status_t;
 
// declare typedefs for function prototypes that need to be installed
typedef int8_t (initializeSensor_t) (
    struct PhysicalSensor *sensor,
    struct SensorFusionGlobals *sfg
) ;
typedef int8_t (readSensor_t) (
    struct PhysicalSensor *sensor,
    struct SensorFusionGlobals *sfg
) ;
typedef int8_t (readSensors_t) (
    struct SensorFusionGlobals *sfg,
    uint8_t read_loop_counter
) ;
typedef int8_t (installSensor_t) (
    struct SensorFusionGlobals *sfg,    
    struct PhysicalSensor *sensor,      
    uint16_t addr,                      
    uint16_t schedule,                  
    registerDeviceInfo_t *busInfo,      
    initializeSensor_t *initialize,     
    readSensor_t *read                  
);
 
typedef void   (initializeFusionEngine_t)   (struct SensorFusionGlobals *sfg, int pin_i2c_sda1, int pin_i2c_scl);
typedef void   (runFusion_t)            (struct SensorFusionGlobals *sfg);
typedef void   (clearFIFOs_t)           (struct SensorFusionGlobals *sfg);
typedef void   (conditionSensorReadings_t)  (struct SensorFusionGlobals *sfg);
typedef void   (applyPerturbation_t)        (struct SensorFusionGlobals *sfg) ;
typedef void   (setStatus_t)            (struct SensorFusionGlobals *sfg, fusion_status_t status);
typedef fusion_status_t  (getStatus_t)          (struct SensorFusionGlobals *sfg);
typedef void   (updateStatus_t)         (struct SensorFusionGlobals *sfg);
typedef void   (ssSetStatus_t)          (struct StatusSubsystem *pStatus, fusion_status_t status);
typedef fusion_status_t   (ssGetStatus_t)           (struct StatusSubsystem *pStatus);
typedef void   (ssUpdateStatus_t)       (struct StatusSubsystem *pStatus);
 
struct PhysicalSensor {
        registerDeviceInfo_t deviceInfo;        
        registerDeviceInfo_t *busInfo;          
    uint16_t addr;              
        uint16_t isInitialized;                 
    struct PhysicalSensor *next;        
        uint8_t schedule;                      
    initializeSensor_t *initialize;     
    readSensor_t *read;         
};
 
// Now start "standard" sensor fusion structure definitions
 
struct PressureSensor
{
    uint8_t iWhoAmI;                
  bool  isEnabled;            
    int32_t iH;             
    int32_t iP;             
    float fH;               
    float fT;               
    float fmPerCount;               
    float fCPerCount;               
    int16_t iT;             
};
 
struct AccelSensor
{
    uint8_t iWhoAmI;            
    bool  isEnabled;                        
    uint8_t iFIFOCount;         
    uint16_t iFIFOExceeded;                 
    int16_t iGsFIFO[ACCEL_FIFO_SIZE][3];    
        // End of common fields which can be referenced via FifoSensor union type
    float fGs[3];                   
    float fGc[3];               
    float fgPerCount;           
    float fCountsPerg;          
    int16_t iGs[3];             
    int16_t iGc[3];             
    int16_t iCountsPerg;            
};
 
struct MagSensor
{
    uint8_t iWhoAmI;            
        bool  isEnabled;                        
    uint8_t iFIFOCount;         
        uint16_t iFIFOExceeded;                 
    int16_t iBsFIFO[MAG_FIFO_SIZE][3];  
        // End of common fields which can be referenced via FifoSensor union type
    float fBs[3];               
    float fBc[3];               
    float fuTPerCount;          
    float fCountsPeruT;         
    int16_t iBs[3];             
    int16_t iBc[3];             
    int16_t iCountsPeruT;           
};
 
struct TempSensor
{
    float temperatureC;         // temperature in Celsius
};
 
struct GyroSensor
{
    uint8_t iWhoAmI;            
        bool  isEnabled;                        
    uint8_t iFIFOCount;         
        uint16_t iFIFOExceeded;                 
    int16_t iYsFIFO[GYRO_FIFO_SIZE][3]; 
        // End of common fields which can be referenced via FifoSensor union type
    float fYs[3];               
    float fDegPerSecPerCount;       
    int16_t iCountsPerDegPerSec;        
    int16_t iYs[3];             
};
 
union FifoSensor  {
    struct GyroSensor Gyro;
    struct MagSensor  Mag;
    struct AccelSensor Accel;
};
 
struct SV_1DOF_P_BASIC
{
    float fLPH;             
    float fLPT;             
    float fdeltat;              
    float flpf;             
    int32_t systick;            
    int8_t resetflag;           
};
 
struct SV_3DOF_G_BASIC
{
    // start: elements common to all motion state vectors
    float fLPPhi;               
    float fLPThe;               
    float fLPPsi;               
    float fLPRho;               
    float fLPChi;               
    float fLPR[3][3];           
    Quaternion fLPq;            
    float fLPRVec[3];           
    float fOmega[3];            
    int32_t systick;            
    // end: elements common to all motion state vectors
    float fR[3][3];             
    Quaternion fq;              
    float fdeltat;              
    float flpf;             
    int8_t resetflag;           
};
 
struct SV_3DOF_B_BASIC
{
    // start: elements common to all motion state vectors
    float fLPPhi;                   
    float fLPThe;               
    float fLPPsi;               
    float fLPRho;               
    float fLPChi;               
    float fLPR[3][3];           
    Quaternion fLPq;            
    float fLPRVec[3];           
    float fOmega[3];            
    int32_t systick;            
    // end: elements common to all motion state vectors
    float fR[3][3];             
    Quaternion fq;              
    float fdeltat;              
    float flpf;             
    int8_t resetflag;           
};
 
struct SV_3DOF_Y_BASIC
{
    // start: elements common to all motion state vectors
    float fPhi;             
    float fThe;             
    float fPsi;             
    float fRho;             
    float fChi;             
    float fR[3][3];             
    Quaternion fq;              
    float fRVec[3];             
    float fOmega[3];            
    int32_t systick;            
    // end: elements common to all motion state vectors
    float fdeltat;              
    int8_t resetflag;           
};
 
struct SV_6DOF_GB_BASIC
{
    // start: elements common to all motion state vectors
    float fLPPhi;               
    float fLPThe;               
    float fLPPsi;               
    float fLPRho;               
    float fLPChi;               
    float fLPR[3][3];           
    Quaternion fLPq;            
    float fLPRVec[3];           
    float fOmega[3];            
    int32_t systick;            
    // end: elements common to all motion state vectors
    float fR[3][3];             
    Quaternion fq;              
    float fDelta;               
    float fLPDelta;             
    float fdeltat;              
    float flpf;             
    int8_t resetflag;           
};
 
struct SV_6DOF_GY_KALMAN
{
    // start: elements common to all motion state vectors
    float fPhiPl;               
    float fThePl;               
    float fPsiPl;               
    float fRhoPl;               
    float fChiPl;               
    float fRPl[3][3];           
    Quaternion fqPl;            
    float fRVecPl[3];           
    float fOmega[3];            
    int32_t systick;            
    // end: elements common to all motion state vectors
    float fQw6x6[6][6];         
    float fK6x3[6][3];          
    float fQwCT6x3[6][3];           
    float fQv;              
    float fZErr[3];             
    float fqgErrPl[3];          
    float fbPl[3];              
    float fbErrPl[3];           
    float fAccGl[3];            
    float fdeltat;              
    float fAlphaOver2;          
    float fAlphaSqOver4;                
    float fAlphaSqQvYQwbOver12;     
    float fAlphaQwbOver6;           
    float fQwbOver3;            
    float fMaxGyroOffsetChange;     
    int8_t resetflag;           
};
 
struct SV_9DOF_GBY_KALMAN
{
    // start: elements common to all motion state vectors
    float fPhiPl;               
    float fThePl;               
    float fPsiPl;               
    float fRhoPl;               
    float fChiPl;               
    float fRPl[3][3];           
    Quaternion fqPl;            
    float fRVecPl[3];           
    float fOmega[3];            
    int32_t systick;            
    // end: elements common to all motion state vectors
    float fQw9x9[9][9];         
    float fK9x6[9][6];          
    float fQwCT9x6[9][6];           
    float fZErr[6];             
    float fQv6x1[6];            
    float fDeltaPl;             
    float fsinDeltaPl;          
    float fcosDeltaPl;          
    float fqgErrPl[3];          
    float fqmErrPl[3];          
    float fbPl[3];              
    float fbErrPl[3];           
    float fAccGl[3];            
    float fVelGl[3];            
    float fDisGl[3];            
    float fdeltat;              
    float fgdeltat;             
    float fAlphaOver2;          
    float fAlphaSqOver4;            
    float fAlphaSqQvYQwbOver12;     
    float fAlphaQwbOver6;           
    float fQwbOver3;            
    float fMaxGyroOffsetChange;     
    int8_t iFirstAccelMagLock;      
    int8_t resetflag;           
};
 
struct SV_COMMON {
    float fPhi;             
    float fThe;             
    float fPsi;             
    float fRho;             
    float fChi;             
    float fRM[3][3];            
    Quaternion fq;                  
    float fRVec[3];                 
    float fOmega[3];            
    int32_t systick;            
};
 
typedef struct SV_COMMON *SV_ptr;
 
typedef struct SensorFusionGlobals
{
    // Subsystem Pointers
    struct ControlSubsystem *pControlSubsystem;
    struct StatusSubsystem *pStatusSubsystem;
        uint32_t iFlags;                        
    struct PhysicalSensor *pSensors;                
    volatile uint8_t iPerturbation;         
    // Book-keeping variables
    int32_t loopcounter;            
    int32_t systick_I2C;            
    int32_t systick_Spare;          
#if     F_1DOF_P_BASIC
    struct PressureSensor   Pressure;       
#endif
#if     F_USING_ACCEL
    struct AccelSensor  Accel;                  
    AccelCalibration AccelCal;              
    AccelBuffer AccelBuffer;                
#endif
#if     F_USING_MAG
    struct MagSensor    Mag;                    
    struct MagCalibration MagCal;                  
    struct MagBuffer MagBuffer;                    
#endif
#if     F_USING_GYRO
    struct GyroSensor   Gyro;                   
#endif
    struct TempSensor Temp;                 //temperature storage
 
#if     F_1DOF_P_BASIC
    struct SV_1DOF_P_BASIC SV_1DOF_P_BASIC;        
#endif
#if     F_3DOF_G_BASIC
    struct SV_3DOF_G_BASIC SV_3DOF_G_BASIC;        
#endif
#if     F_3DOF_B_BASIC
    struct SV_3DOF_B_BASIC SV_3DOF_B_BASIC;        
#endif
#if     F_3DOF_Y_BASIC
    struct SV_3DOF_Y_BASIC SV_3DOF_Y_BASIC;        
#endif
#if     F_6DOF_GB_BASIC            // 6DOF accel and mag eCompass: (accel + mag)
    struct SV_6DOF_GB_BASIC SV_6DOF_GB_BASIC;      
#endif
#if     F_6DOF_GY_KALMAN
    struct SV_6DOF_GY_KALMAN SV_6DOF_GY_KALMAN;    
#endif
#if     F_9DOF_GBY_KALMAN
    struct SV_9DOF_GBY_KALMAN SV_9DOF_GBY_KALMAN;  
#endif
    installSensor_t     *installSensor;         
    initializeFusionEngine_t *initializeFusionEngine ;  
    applyPerturbation_t     *applyPerturbation ;    
    readSensors_t       *readSensors;       
    runFusion_t     *runFusion;     
        conditionSensorReadings_t *conditionSensorReadings;  
        clearFIFOs_t            *clearFIFOs;            
    setStatus_t     *setStatus;     
    setStatus_t     *queueStatus;           
    updateStatus_t      *updateStatus;      
    updateStatus_t      *testStatus;        
  getStatus_t   *getStatus; 
 
} SensorFusionGlobals;
 
// The following functions are defined in sensor_fusion.c
void initSensorFusionGlobals(
    SensorFusionGlobals *sfg,                           
    struct StatusSubsystem *pStatusSubsystem,           
    struct ControlSubsystem *pControlSubsystem          
);
installSensor_t installSensor;
initializeFusionEngine_t initializeFusionEngine ;
void conditionSensorReadings(
    SensorFusionGlobals *sfg                            
);
void clearFIFOs(
    SensorFusionGlobals *sfg                            
);
runFusion_t runFusion;
readSensors_t readSensors;
void zeroArray(
    struct StatusSubsystem *pStatus,                    
    void* data,                                         
    uint16_t size,                                      
    uint16_t numElements,                               
    uint8_t check                                       
);
void conditionSample(
    int16_t sample[3]                                   
);
 
void addToFifo(
    union FifoSensor *sensor,                                 
    uint16_t maxFifoSize,                               
    int16_t sample[3]                                   
);
 
// The following functions are defined in hal_axis_remap.c
// Please note that these are board-dependent - they account for 
//various orientations of sensor ICs on the sensor PCB.
 
void ApplyAccelHAL(
    struct AccelSensor *Accel                                  
);
void ApplyMagHAL(
    struct MagSensor *Mag                                     
);
void ApplyGyroHAL(
    struct GyroSensor *Gyro                                    
);
applyPerturbation_t ApplyPerturbation;
 
#ifdef __cplusplus
}
#endif
 
#endif // SENSOR_FUSION_H