/*****************************************************************************
Copyright 1993 MBARI                                                      
******************************************************************************
Summary  : Kalman filter routines header file
Filename : kalman.h
Author   : Michael B. Matthews                                                
Project  : New ROV                                                        
Version  : Version 1.0                                                    
Created  : February 12, 1996                                                     
Modified :                                                                
Archived :                                                                
Notes    : 
*****************************************************************************/

#ifndef kalman_h
#define kalman_h

typedef struct {
  DM_Item compassRadians;		/* compass heading measurement        */
  DM_Item gyroRadians;			/* gyro heading measurement           */
  DM_Item gyroRateRadians;		/* gyro heading rate measurement      */
  DM_Item compassDataValid;		/* compass data valid flag            */
  DM_Item gyroDataValid;		/* gyro data valid flag               */
  DM_Item Heading;			/* filtered heading                   */
  DM_Item Pitch;			/* filtered pitch                     */
  DM_Item Roll;				/* filtered roll                      */
  DM_Item Heading_Rate;			/* filtered heading rate              */
  DM_Item Pitch_Rate;			/* filtered pitch rate                */
  DM_Item Roll_Rate;			/* filtered roll rate                 */
  DM_Item Acceleration_X;		/* acceleration x-axis                */
  DM_Item Acceleration_Y;		/* acceleration y-axis                */
  DM_Item Acceleration_Z;		/* acceleration z-axis                */
  DM_Item Heading_Rate_Bias;		/* heading rate bias                  */
  DM_Item Heading_Rate_Bias_2D;		/* heading rate bias 2nd order        */
  DM_Item Gyro_Drift;	         	/* gyro drift                         */
  DM_Item Gyro_Drift_2D;	      	/* gyro drift 2nd order               */
  DM_Item Status; 			/* status process flag		      */
} kalmanDMItems;


/* sensors group id bits */
typedef struct {
  DWord compassBit;              	/* compass bit                        */
  DWord gyroBit;                   	/* gyro bit                           */
} sensorBits;


/* buffer pointer structure for logger */
typedef struct {
  MBool status;
  MBool state;
  Flt32 compass;
  Flt32 gyro;
  Int32 count;
  MotionPak_Input_Buffer input_buffer;
  MotionPak_Output_Buffer* output_buffer;
  MotionPak_Output_Buffer* write_buffer;
  MotionPak_Output_Buffer* temp_buffer;
  MotionPak_Output_Buffer output_buffer1;
  MotionPak_Output_Buffer output_buffer2;
  MotionPak_Output output;
  FIR_Filter filter;
} MotionPak_Buffers;


/* Kalman status commands */
enum {	  START, 
	  STOP, 
	  STOP_LOGGING, 
	  START_LOGGING, 
	  HEADING_RESET, 
	  ATTITUDE_RESET, 
	  NOP };

enum { BUFFER_OK, BUFFER_FULL, BUFFER_READY, BUFFER_BUSY };
 
/* function prototypes */
#ifdef __STDC__
Errno initKalmanFilter(kalmanDMItems *kalmanDmi, DM_Group *sensorGroup,
                       sensorBits *sensorId);
#endif

STATUS Kalman_Filter(Void);
STATUS Kalman_Filter_Offline(char*);
int Logger_MotionPak_Write_Buffer(MotionPak_Buffers*, FILE*, SEM_ID);

#endif