/*****************************************************************************/ /* Copyright 1994 MBARI */ /*****************************************************************************/ /* Summary : IP16ADC Greenspring IP module information for MVME162 CPU brd */ /* Filename : motionpak.h */ /* Author : Michael B. Matthews */ /* Project : Kalman Filter */ /* Version : Version 1.0 */ /* Created : 8.12.94 */ /* Modified : */ /* Archived : */ /*****************************************************************************/ #ifndef motionpak_h #define motionpak_h /* Control/Status Register Definitions */ #define CHSL0 0x0001 /* bit 0 [W] - input MUX channel select bit 0 */ #define CHSL1 0x0002 /* bit 1 [W] - input MUX channel select bit 1 */ #define CHSL2 0x0004 /* bit 2 [W] - input MUX channel select bit 2 */ #define INSL0 0x0008 /* bit 3 [W] - odd input MUX select bit */ #define INSL1 0x0010 /* bit 4 [R/W] - even input MUX select bit */ #define UNITY_HALF 0x0020 /* bit 5 [R/W] - gain select bit */ #define BP_UP 0x0040 /* bit 6 [R/W] - bipolar/unipolar select bit */ #define ADCH1_2 0x0080 /* bit 7 [R/W] - ADC input polarity select bit */ #define NO_FREERUN 0x0100 /* bit 8 [R/W] - continuous capture enable bit */ #define CRS_FIN 0x0200 /* bit 9 [R/W] - course/fine select bit */ #define DATINTRQ 0x0400 /* bit 10 [R] - data available intrpt status bit */ #define CHGINTRQ 0x0800 /* bit 11 [R] - change window interrupt status bit*/ #define SDL 0x1000 /* bit 12 [R] - serial data latch signal bit */ #define SWRESET_ 0x2000 /* bit 13 [W] - reset ADC, internal calibration */ #define STDBY 0x2000 /* bit 13 [R] - calibration cycle status bit */ #define DATINTEN 0x4000 /* bit 14 [R/W] - data interrupt enable bit */ #define CHGINTEN 0x8000 /* bit 15 [R/W] - change window intrpt enable bit */ /* mode definitions */ #define UNITY_GAIN 0x0020 /* bit 5 [R/W] - gain select bit */ #define HALF_GAIN 0x0000 #define BIPOLAR 0x0040 /* bit 6 [R/W] - bipolar/unipolar select bit */ #define UNIPOLAR 0x0000 #define ADCH1 0x0080 /* bit 7 [R/W] - selects AGND input */ #define ADCH2 0x0000 /* selects MUX inputs */ #define ODD_INPUT 0x0008 #define EVEN_INPUT 0x0010 #define DIFF_INPUT 0x0018 #define VREF_INPUT 0x0001 #define AGND_INPUT 0x0000 #define NO_FREERUN 0x0100 /* bit 8 [R/W] - continuous capture enable bit */ #define FREERUN 0x0000 #define COURSE 0x0200 /* bit 9 [R/W] - course/fine select bit */ #define FINE 0x0000 #define SWRESET 0x0000 /* bit 13 [W] - reset ADC, internal calibration*/ #define NO_SWRESET 0x2000 /* ADC operating modes */ #define SAMPLE_MODE NO_SWRESET | \ FINE | \ NO_FREERUN | \ ADCH2 | \ EVEN_INPUT | \ UNITY_GAIN | \ UNIPOLAR #define CALIBRATE_MODE NO_SWRESET | \ FINE | \ NO_FREERUN | \ ADCH2 | \ UNIPOLAR | \ UNITY_GAIN #define RESET_COMMAND SWRESET #define INIT_MODE NO_SWRESET | NO_FREERUN /* channel select - to be ORed onto control command */ #define CH01 0x0008 /* channel 1 select */ #define CH02 0x0010 /* channel 2 select */ #define CH03 0x0009 /* channel 3 select */ #define CH04 0x0011 /* channel 4 select */ #define CH05 0x000a /* channel 5 select */ #define CH06 0x0012 /* channel 6 select */ #define CH07 0x000b /* channel 7 select */ #define CH08 0x0013 /* channel 8 select */ #define CH09 0x000c /* channel 9 select */ #define CH10 0x0014 /* channel 10 select */ #define CH11 0x000d /* channel 11 select */ #define CH12 0x0015 /* channel 12 select */ #define CH13 0x000e /* channel 13 select */ #define CH14 0x0016 /* channel 14 select */ #define CH15 0x000f /* channel 15 select */ #define CH16 0x0017 /* channel 16 select */ /* channel select - to be ORed onto control command */ #define REF 0x0008 /* channel 1 select */ #define AX 0x0009 /* channel 3 select */ #define AY 0x000a /* channel 5 select */ #define AZ 0x000b /* channel 7 select */ #define RX 0x000c /* channel 9 select */ #define RY 0x000d /* channel 11 select */ #define RZ 0x000e /* channel 13 select */ #define TEMP 0x000f /* channel 15 select */ /* IP16ADC Register Addresses */ #define ID_PROM 0xfff58080 /* Ctl/Status Register */ #define CTL_STATUS_REG 0xfff58000 /* Ctl/Status Register */ #define DATA_REG 0xfff58004 /* Data Register */ #define VECTOR_REG 0xfff58009 /* Vector Register */ #define CONV_REQ_REG 0xfff5800c /* Conversion Request */ /* Trigger Register */ #define IP16ADC(reg) ( (__volatile__ Word *) (reg) ) /* ID PROM Gain and Offset */ #define OFFSET_BIPOLAR_5V ID_PROM + 0x24 #define OFFSET_BIPOLAR_10V ID_PROM + 0x2c #define OFFSET_UNIPOLAR_5V ID_PROM + 0x20 #define OFFSET_UNIPOLAR_10V ID_PROM + 0x2c #define GAIN_BIPOLAR_5V ID_PROM + 0x34 #define GAIN_BIPOLAR_10V ID_PROM + 0x3c #define GAIN_UNIPOLAR_5V ID_PROM + 0x30 #define GAIN_UNIPOLAR_10V ID_PROM + 0x38 /* ID PROM Reference voltage */ #define REFERENCE_VOLTAGE ID_PROM + 0x18 #define REFERENCE_DIVIDER ID_PROM + 0x1c #define VCAL 0.25 /* calibration steps */ #define NCAL 41 /* number of calibration points */ #define M_PI 3.14159265358979323846 /* MotionPak accelerometer data */ #define AX_RANGE 1.0 /* units of g */ #define AY_RANGE 1.0 #define AZ_RANGE 1.0 #define AX_BIAS -0.002447 /* units of g */ #define AY_BIAS 0.004570 #define AZ_BIAS -0.000586 #define AX_BIAS_TEMP_COEFF -47.0e-6 /* units of g/deg C */ #define AY_BIAS_TEMP_COEFF -66.0e-6 #define AZ_BIAS_TEMP_COEFF -48.0e-6 #define AX_SCALE_FACTOR 7.469 /* units of V/g */ #define AY_SCALE_FACTOR 7.478 #define AZ_SCALE_FACTOR 3.727 #define AX_ALIGNMENT 0.18 /* units of degrees */ #define AY_ALIGNMENT 0.19 #define AZ_ALIGNMENT 0.10 #define GS_TO_MPSPS 9.81 /* g's to m/sec/sec conversion */ /* MotionPak rate sensor data */ #define RX_RANGE 50.0 /* units of deg/sec */ #define RY_RANGE 50.0 #define RZ_RANGE 50.0 #define RX_BIAS -0.06 /* units of deg/sec */ #define RY_BIAS 0.23 #define RZ_BIAS 0.23 #define RX_SCALE_FACTOR 0.050151 /* units of V/deg/sec */ #define RY_SCALE_FACTOR 0.049906 #define RZ_SCALE_FACTOR 0.050242 #define RX_ALIGNMENT 0.26 /* units of degrees */ #define RY_ALIGNMENT 0.41 #define RZ_ALIGNMENT 0.34 /* Timer 3 Stuff */ #define SAMPLE_FREQUENCY 100.0 #define DECIMATED_SAMPLE_FREQUENCY 50.0 #define SAMPLE_PERIOD (Flt32) (1.0/SAMPLE_FREQUENCY) #define DECIMATION (Int32) (SAMPLE_FREQUENCY/DECIMATED_SAMPLE_FREQUENCY) #define DT (Flt32) (DECIMATION*SAMPLE_PERIOD) #define INT_VEC_T3 (MCC_INT_VEC_BASE + MCC_INT_TT3) #define COUNTS 10 #define KMAX 10 #define NMAX 1 #define LMAX 8 #define MMAX (1 << LMAX) /* #define PRINT */ /* #define CALIBRATE */ #define MONITOR #define TIME 0.250 #define DELAY 21 #define KALMAN_DM_PERIOD 20000 /* 50Hz sampling */ #define CHANNEL_DELAY (5.0 - 1.7)/0.4 #define SAMPLE_DELAY (10.0 - 1.7)/0.4 #define RELEASED /* for released versions of code */ #ifdef RELEASED #define PARAMETER_FILE "/usr/vw5.1.1/mbari/config/kalman/hf_parameters" #else #define PARAMETER_FILE "/usr/vw5.1.1/mbari/kalman/hf_parameters" #endif #if 1 #define IP16ADC_INSTALLED #endif #ifdef IP16ADC_INSTALLED #define LINEAR_CORRECTION #define MOTIONPAK_CORRECTION #define FIR_FILTER #endif /* #define KALMAN_FILTER_BYPASS */ /* #define SIGNAL_STATUS_RUNNING */ /* #define SIGNAL_FIR_FILTER */ /* #define SIGNAL_MAIN */ /* #define SIGNAL_INTERRUPT */ /* #define SIGNAL_CHANNEL_SAMPLE */ #define FAIL_LED_BLINKING /* #define SIGNAL_SAMPLE_INPUT */ /* #define SIGNAL_LINEAR_CORRECTION */ /* #define SIGNAL_MOTIONPAK_CORRECTION */ /* #define SIGNAL_BUFFER_INPUT */ #define FAIL_LED_COUNT = 1000 /* input data */ #define FIR_FILTER_LENGTH 101 #define INPUT_BUFFER_SIZE 1000 #define CALIBRATION_LENGTH 500 #define EXPERIMENT_TIME 10 /* total time in minutes */ #define OUTPUT_BUFFER_SIZE 50 * 60 /* 60 second buffer length */ #define NUMBER_OF_BUFFERS EXPERIMENT_TIME /* Data Structures */ typedef struct{ char* name; MBool status; MBool state; Int16 index; Int16 size; Flt32* time; Flt32* ref; Flt32* ax; Flt32* ay; Flt32* az; Flt32* rx; Flt32* ry; Flt32* rz; Flt32* temp; } MotionPak_Input_Buffer; /* FIR filter coefficient structure */ typedef struct { MBool state; Int16 length; Flt32* b; } FIR_Filter; /* IP-16ADC input sample structure */ typedef struct { MBool state; Flt32 time; Flt32 Channel_1; Flt32 Channel_2; Flt32 Channel_3; Flt32 Channel_4; Flt32 Channel_5; Flt32 Channel_6; Flt32 Channel_7; Flt32 Channel_8; Flt32 Channel_9; Flt32 Channel_10; Flt32 Channel_11; Flt32 Channel_12; Flt32 Channel_13; Flt32 Channel_14; Flt32 Channel_15; Flt32 Channel_16; } IP16ADC_Input; /* calibrated MotionPak data structure */ typedef struct { MBool state; Flt64 time; Flt32 ref; Flt32 ax; Flt32 ay; Flt32 az; Flt32 rx; Flt32 ry; Flt32 rz; Flt32 temp; } MotionPak_Output; typedef struct { char* name; MBool status; MBool state; Int16 index; Int16 size; Flt32* time; Flt32* ref; Flt32* ax; Flt32* ay; Flt32* az; Flt32* rx; Flt32* ry; Flt32* rz; Flt32* temp; } MotionPak_Output_Buffer; /* calibration structure for IP16ADC */ typedef struct { Int16 Offset_Bipolar_10V; /* calibration values stored in ID PROM */ Int16 Offset_Bipolar_5V; /* calculated using internal VREF and AGND */ Int16 Offset_Unipolar_10V; Int16 Offset_Unipolar_5V; Int16 Gain_Bipolar_10V; Int16 Gain_Bipolar_5V; Int16 Gain_Unipolar_10V; Int16 Gain_Unipolar_5V; Int16 Ref_Voltage; Int16 Ref_Divider; Flt32 Offset_Channel_1; /* calibration values for external inputs */ Flt32 Gain_Channel_1; Flt32 Calibration_Table_Channel_1[NCAL]; Flt32 Offset_Channel_2; Flt32 Gain_Channel_2; Flt32 Calibration_Table_Channel_2[NCAL]; Flt32 Offset_Channel_3; Flt32 Gain_Channel_3; Flt32 Calibration_Table_Channel_3[NCAL]; Flt32 Offset_Channel_4; Flt32 Gain_Channel_4; Flt32 Calibration_Table_Channel_4[NCAL]; Flt32 Offset_Channel_5; Flt32 Gain_Channel_5; Flt32 Calibration_Table_Channel_5[NCAL]; Flt32 Offset_Channel_6; Flt32 Gain_Channel_6; Flt32 Calibration_Table_Channel_6[NCAL]; Flt32 Offset_Channel_7; Flt32 Gain_Channel_7; Flt32 Calibration_Table_Channel_7[NCAL]; Flt32 Offset_Channel_8; Flt32 Gain_Channel_8; Flt32 Calibration_Table_Channel_8[NCAL]; Flt32 Offset_Channel_9; Flt32 Gain_Channel_9; Flt32 Calibration_Table_Channel_9[NCAL]; Flt32 Offset_Channel_10; Flt32 Gain_Channel_10; Flt32 Calibration_Table_Channel_10[NCAL]; Flt32 Offset_Channel_11; Flt32 Gain_Channel_11; Flt32 Calibration_Table_Channel_11[NCAL]; Flt32 Offset_Channel_12; Flt32 Gain_Channel_12; Flt32 Calibration_Table_Channel_12[NCAL]; Flt32 Offset_Channel_13; Flt32 Gain_Channel_13; Flt32 Calibration_Table_Channel_13[NCAL]; Flt32 Offset_Channel_14; Flt32 Gain_Channel_14; Flt32 Calibration_Table_Channel_14[NCAL]; Flt32 Offset_Channel_15; Flt32 Gain_Channel_15; Flt32 Calibration_Table_Channel_15[NCAL]; Flt32 Offset_Channel_16; Flt32 Gain_Channel_16; Flt32 Calibration_Table_Channel_16[NCAL]; } MotionPak_Calibration; /* subroutine decarations */ void Timer3_Init(Flt32); void Timer3_Interrupt_Service_Routine(void); void Timer3_Enable(void); void Timer3_Disable(void); void Timer3_Connect(FUNCPTR, int); int Sample_Interrupt_Routine(int); int Calibration_Interrupt_Routine(int); STATUS MotionPak_Initialize(void); STATUS FIR_Filter_Initialize(FIR_Filter*, int); void FIR_Filter_Compute(MotionPak_Input_Buffer*, FIR_Filter*, MotionPak_Output*); void FIR_Filter_Free(FIR_Filter*); STATUS MotionPak_Input_Buffer_Initialize(MotionPak_Input_Buffer*, int); void MotionPak_Input_Buffer_Update(MotionPak_Output*, MotionPak_Input_Buffer*); void MotionPak_Input_Buffer_Free(MotionPak_Input_Buffer*); STATUS MotionPak_Output_Buffer_Initialize(MotionPak_Output_Buffer*, int); STATUS MotionPak_Output_Buffer_Update(MotionPak_Output_Buffer*, MotionPak_Output*); void MotionPak_Output_Buffer_Free(MotionPak_Output_Buffer*); int MotionPak_Write_Buffer(MotionPak_Output_Buffer*, FILE*); int MotionPak_Write_Output(MotionPak_Output*, FILE*); int MotionPak_Read_Buffer(MotionPak_Output_Buffer*, FILE*); void File_Convert_Binary_to_ASCII(char*, int, int); void Get_Time(Flt64*, Flt64*); Flt64 Get_Absolute_Time(void); void Interrupt_Service(int); void Delay(int); void Calibrate_External_Input(void); void Calibrate_Parallel_Input(void); void Read_Calibration_Table(MotionPak_Calibration*); void Sample_Input(IP16ADC_Input*, Word); void Sample_Input_with_Interrupts(IP16ADC_Input*, Word); void Linear_Correction(IP16ADC_Input*, MotionPak_Calibration*); void MotionPak_Correction(IP16ADC_Input*, MotionPak_Output*); void Sample_Test(void); STATUS MotionPak_Data_Logger(void); STATUS MotionPak_Data_Logger_DM(void); int Logger_Sample_Interrupt_Routine(int); #endif