sbgEComBinaryLogEkf.h

Go to the documentation of this file.

#ifndef __SBG_ECOM_BINARY_LOG_EKF_H__
#define __SBG_ECOM_BINARY_LOG_EKF_H__

#include <sbgCommon.h>
#include <streamBuffer/sbgStreamBuffer.h>

//----------------------------------------------------------------------//
//- Solution status definitions                                        -//
//----------------------------------------------------------------------//

#define SBG_ECOM_SOLUTION_MODE_SHIFT            (0u)                                    
#define SBG_ECOM_SOLUTION_MODE_MASK                     (0x0000000Fu)                   
#define SBG_ECOM_SOL_ATTITUDE_VALID                     (0x00000001u << 4)              
#define SBG_ECOM_SOL_HEADING_VALID                      (0x00000001u << 5)              
#define SBG_ECOM_SOL_VELOCITY_VALID                     (0x00000001u << 6)              
#define SBG_ECOM_SOL_POSITION_VALID                     (0x00000001u << 7)              
#define SBG_ECOM_SOL_VERT_REF_USED                      (0x00000001u << 8)              
#define SBG_ECOM_SOL_MAG_REF_USED                       (0x00000001u << 9)              
#define SBG_ECOM_SOL_GPS1_VEL_USED                      (0x00000001u << 10)             
#define SBG_ECOM_SOL_GPS1_POS_USED                      (0x00000001u << 11)             
#define SBG_ECOM_SOL_GPS1_HDT_USED                      (0x00000001u << 13)             
#define SBG_ECOM_SOL_GPS2_VEL_USED                      (0x00000001u << 14)             
#define SBG_ECOM_SOL_GPS2_POS_USED                      (0x00000001u << 15)             
#define SBG_ECOM_SOL_GPS2_HDT_USED                      (0x00000001u << 17)             
#define SBG_ECOM_SOL_ODO_USED                           (0x00000001u << 18)             
#define SBG_ECOM_SOL_DVL_BT_USED                        (0x00000001u << 19)             
#define SBG_ECOM_SOL_DVL_WT_USED                        (0x00000001u << 20)             
#define SBG_ECOM_SOL_USER_POS_USED                      (0x00000001u << 21)             
#define SBG_ECOM_SOL_USER_VEL_USED                      (0x00000001u << 22)             
#define SBG_ECOM_SOL_USER_HEADING_USED          (0x00000001u << 23)             
#define SBG_ECOM_SOL_USBL_USED                          (0x00000001u << 24)             
#define SBG_ECOM_SOL_AIR_DATA_USED                      (0x00000001u << 25)             
#define SBG_ECOM_SOL_ZUPT_USED                          (0x00000001u << 26)             
#define SBG_ECOM_SOL_ALIGN_VALID                        (0x00000001u << 27)             
#define SBG_ECOM_SOL_DEPTH_USED                         (0x00000001u << 28)             
typedef enum _SbgEComSolutionMode
{
        SBG_ECOM_SOL_MODE_UNINITIALIZED                 = 0,                                    
        SBG_ECOM_SOL_MODE_VERTICAL_GYRO                 = 1,                                    
        SBG_ECOM_SOL_MODE_AHRS                                  = 2,                                    
        SBG_ECOM_SOL_MODE_NAV_VELOCITY                  = 3,                                    
        SBG_ECOM_SOL_MODE_NAV_POSITION                  = 4                                             
} SbgEComSolutionMode;

//----------------------------------------------------------------------//
//- Solution status helpers methods                                    -//
//----------------------------------------------------------------------//

SBG_INLINE SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status)
{
        return (SbgEComSolutionMode)((status >> SBG_ECOM_SOLUTION_MODE_SHIFT) & SBG_ECOM_SOLUTION_MODE_MASK);
}

SBG_INLINE uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint32_t masks)
{
        //
        // Create the combined status field
        //
        return  ((((uint32_t)solutionMode)&SBG_ECOM_SOLUTION_MODE_MASK) << SBG_ECOM_SOLUTION_MODE_SHIFT) | masks;
}


//----------------------------------------------------------------------//
//- Log structure definitions                                          -//
//----------------------------------------------------------------------//

typedef struct _SbgLogEkfEulerData
{
        uint32_t        timeStamp;                              
        float   euler[3];                               
        float   eulerStdDev[3];                 
        uint32_t        status;                                 
} SbgLogEkfEulerData;

typedef struct _SbgLogEkfQuatData
{
        uint32_t        timeStamp;                              
        float   quaternion[4];                  
        float   eulerStdDev[3];                 
        uint32_t        status;                                 
} SbgLogEkfQuatData;

typedef struct _SbgLogEkfNavData
{
        uint32_t        timeStamp;                              
        float   velocity[3];                    
        float   velocityStdDev[3];              
        double  position[3];                    
        float   undulation;                             
        float   positionStdDev[3];              
        uint32_t        status;                                 
} SbgLogEkfNavData;

//----------------------------------------------------------------------//
//- Operations                                                         -//
//----------------------------------------------------------------------//

SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData);

SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pOutputStream, const SbgLogEkfEulerData *pInputData);

SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pInputStream, SbgLogEkfQuatData *pOutputData);

SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pOutputStream, const SbgLogEkfQuatData *pInputData);

SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pInputStream, SbgLogEkfNavData *pOutputData);

SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pOutputStream, const SbgLogEkfNavData *pInputData);

#endif