/***************************************************************************/ /* Copyright 1995 MBARI */ /***************************************************************************/ /* Summary : */ /* Filename : */ /* Author : Michael B. Matthews */ /* Project : Tiburon ROV */ /* Version : Version 1.0 */ /* Created : February 12, 1996 */ /* Modified : */ /* Archived : */ /***************************************************************************/ #include /* VxWorks systems declarations */ #include /* VxWorks standard I/O library */ #include /* VxWorks floating point math library*/ #include /* VxWorks semaphore library */ #include /* VxWorks watchdog timer library */ #include /* VxWorks system time declarations */ #include /* VxWorks string library */ #include #include /* MBARI type declarations */ #include /* MBARI time declarations */ #include /* kalman data manager definitions */ #include /* sensor data concentrator definitns */ #include /* VME IBC definitions */ #include /* */ #include /* data manager declarations */ #include /* data manager error declarations */ #include "fppLib.h" /* vxWorks vxTas routine definition */ #include "/usr/vw/config/mv162/mv162.h" #include "arch/mc68k/ivMc68k.h" #include "drv/vme/vmechip2.h" #include "Matrix.h" #include "motionpak.h" #include "kalman.h" #include "heading_filter.h" #define DM_ITEM_NAME_LEN 60 /* data manager item name length */ /* global variables */ SEM_ID Sem; MBool Timer3_Running; FUNCPTR Timer3_Routine; int Timer3_Arg; MotionPak_Calibration calibration; MotionPak_Input_Buffer* inbuff; /* for benchmarking */ Matrix* S; Matrix* P; Matrix* Q; Vector* x; Vector* y; /******************************************************************************/ /* Function : Sample_Input */ /* Purpose : Samples eight channels of IP-16ADC connected to MotionPak */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void Sample_Input(IP16ADC_Input* sample, Word mode) { Reg Nat16 data; #ifdef SIGNAL_SAMPLE_INPUT *MCC_RESET_CR &= ~(RESET_CR_BDFLO); #endif #ifdef IP16ADC_INSTALLED /* start conversion and read channel 2; MUX to channel 4 */ *IP16ADC(CONV_REQ_REG) = 0xff; /* start conversion */ Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } /* wait for SDL to go high*/ *IP16ADC(CTL_STATUS_REG) = mode | BIPOLAR | CH04; /* MUX to next channel */ data = *IP16ADC(DATA_REG); /* input completed sample */ sample->Channel_2 = (Int16) data * 5.0 / 65536; /* convert to FP volts */ Delay((int) CHANNEL_DELAY); /* start conversion and read channel 4; MUX to channel 6 */ *IP16ADC(CONV_REQ_REG) = 0xff; Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } *IP16ADC(CTL_STATUS_REG) = mode | BIPOLAR | CH06; data = *IP16ADC(DATA_REG); data -= 0x8000; sample->Channel_4 = (Int16) data * 10.0 / 65536; Delay((int) CHANNEL_DELAY); /* start conversion and read channel 6; MUX to channel 8 */ *IP16ADC(CONV_REQ_REG) = 0xff; Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } *IP16ADC(CTL_STATUS_REG) = mode | BIPOLAR | CH08; data = *IP16ADC(DATA_REG); data -= 0x8000; sample->Channel_6 = (Int16) data * 10.0 / 65536; Delay((int) CHANNEL_DELAY); /* start conversion and read channel 8; MUX to channel 10 */ *IP16ADC(CONV_REQ_REG) = 0xff; Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } *IP16ADC(CTL_STATUS_REG) = mode | BIPOLAR | CH10; data = *IP16ADC(DATA_REG); data -= 0x8000; sample->Channel_8 = (Int16) data * 10.0 / 65536; Delay((int) CHANNEL_DELAY); /* start conversion and read channel 10; MUX to channel 12 */ *IP16ADC(CONV_REQ_REG) = 0xff; Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } *IP16ADC(CTL_STATUS_REG) = mode | BIPOLAR | CH12; data = *IP16ADC(DATA_REG); data -= 0x8000; sample->Channel_10 = (Int16) data * 10.0 / 65536; Delay((int) CHANNEL_DELAY); /* start conversion and read channel 12; MUX to channel 14 */ *IP16ADC(CONV_REQ_REG) = 0xff; Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } *IP16ADC(CTL_STATUS_REG) = mode | BIPOLAR | CH14; data = *IP16ADC(DATA_REG); data -= 0x8000; sample->Channel_12 = (Int16) data * 10.0 / 65536; Delay((int) CHANNEL_DELAY); /* start conversion and read channel 14; MUX to channel 16 */ *IP16ADC(CONV_REQ_REG) = 0xff; Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } *IP16ADC(CTL_STATUS_REG) = mode | BIPOLAR | CH16; data = *IP16ADC(DATA_REG); data -= 0x8000; sample->Channel_14 = (Int16) data * 10.0 / 65536; Delay((int) CHANNEL_DELAY); /* start conversion and read channel 16; MUX to channel 2 */ *IP16ADC(CONV_REQ_REG) = 0xff; Delay((int) SAMPLE_DELAY); while ( !(*IP16ADC(CTL_STATUS_REG) & SDL) ) { } *IP16ADC(CTL_STATUS_REG) = mode | UNIPOLAR | CH02; data = *IP16ADC(DATA_REG); data -= 0x8000; sample->Channel_16 = (Int16) data * 10.0 / 65536; #else sample->Channel_2 = 1.0; sample->Channel_4 = 2.0; sample->Channel_6 = 3.0; sample->Channel_8 = 4.0; sample->Channel_10 = 5.0; sample->Channel_12 = 6.0; sample->Channel_14 = 7.0; sample->Channel_16 = 8.0; #endif #ifdef SIGNAL_SAMPLE_INPUT *MCC_RESET_CR = RESET_CR_BDFLO; #endif } /* delays approx. 1.7 + 0.4 n microseconds */ void Delay(int n) /*****************/ { int j; for (j = 0; j < n; j++) { } } /******************************************************************************/ /* Function : Linear_Correction */ /* Purpose : Piecewise linear correction of input channel */ /* Inputs : */ /* Outputs : */ /* Notes : Uses calibration table to do a piecewise linear interpolation */ /******************************************************************************/ void Linear_Correction(IP16ADC_Input* sample, MotionPak_Calibration* cal) { Reg Nat16 p; Reg Flt32 y, y1, y2; Reg Flt32 x1, x2; #ifdef SIGNAL_LINEAR_CORRECTION *MCC_RESET_CR &= ~(RESET_CR_BDFLO); #endif #ifdef LINEAR_CORRECTION y = sample->Channel_4; p = y/VCAL + (NCAL-1)/2; x1 = VCAL*(p - (NCAL-1)/2); x2 = VCAL*((p+1) - (NCAL-1)/2); y1 = cal->Calibration_Table_Channel_4[p]; y2 = cal->Calibration_Table_Channel_4[p+1]; sample->Channel_4 = x1 + (y-y1)/(y2-y1)*(x2-x1); y = sample->Channel_6; p = y/VCAL + (NCAL-1)/2; x1 = VCAL*(p - (NCAL-1)/2); x2 = VCAL*((p+1) - (NCAL-1)/2); y1 = cal->Calibration_Table_Channel_6[p]; y2 = cal->Calibration_Table_Channel_6[p+1]; sample->Channel_6 = x1 + (y-y1)/(y2-y1)*(x2-x1); y = sample->Channel_8; p = y/VCAL + (NCAL-1)/2; x1 = VCAL*(p - (NCAL-1)/2); x2 = VCAL*((p+1) - (NCAL-1)/2); y1 = cal->Calibration_Table_Channel_8[p]; y2 = cal->Calibration_Table_Channel_8[p+1]; sample->Channel_8 = x1 + (y-y1)/(y2-y1)*(x2-x1); y = sample->Channel_10; p = y/VCAL + (NCAL-1)/2; x1 = VCAL*(p - (NCAL-1)/2); x2 = VCAL*((p+1) - (NCAL-1)/2); y1 = cal->Calibration_Table_Channel_10[p]; y2 = cal->Calibration_Table_Channel_10[p+1]; sample->Channel_10 = x1 + (y-y1)/(y2-y1)*(x2-x1); y = sample->Channel_12; p = y/VCAL + (NCAL-1)/2; x1 = VCAL*(p - (NCAL-1)/2); x2 = VCAL*((p+1) - (NCAL-1)/2); y1 = cal->Calibration_Table_Channel_12[p]; y2 = cal->Calibration_Table_Channel_12[p+1]; sample->Channel_12 = x1 + (y-y1)/(y2-y1)*(x2-x1); y = sample->Channel_14; p = y/VCAL + (NCAL-1)/2; x1 = VCAL*(p - (NCAL-1)/2); x2 = VCAL*((p+1) - (NCAL-1)/2); y1 = cal->Calibration_Table_Channel_14[p]; y2 = cal->Calibration_Table_Channel_14[p+1]; sample->Channel_14 = x1 + (y-y1)/(y2-y1)*(x2-x1); #endif #ifdef SIGNAL_LINEAR_CORRECTION *MCC_RESET_CR = RESET_CR_BDFLO; #endif } /******************************************************************************/ /* Function : Read_Calibration_Table */ /* Purpose : Reads in calibration table for ADC and filterboard */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void Read_Calibration_Table(MotionPak_Calibration* calibration) { #ifdef LINEAR_CORRECTION char* filename = "/usr/vw5.1.1/mbari/kalman/calibration_table"; FILE* fp; FILE* fp2; int i; Flt32 a; printf("Read_Calibration_Table\n"); printf("opening file %s\n", filename); if ( (fp2 = fopen(filename, "r")) == NULL ){ printf("error opening file - %s\n", filename); exit(1); } for (i = 0; i < NCAL; i++){ fscanf(fp2, "%f %f %f %f %f %f %f %f %f\n", &a, &(calibration->Calibration_Table_Channel_2[i]), &(calibration->Calibration_Table_Channel_4[i]), &(calibration->Calibration_Table_Channel_6[i]), &(calibration->Calibration_Table_Channel_8[i]), &(calibration->Calibration_Table_Channel_10[i]), &(calibration->Calibration_Table_Channel_12[i]), &(calibration->Calibration_Table_Channel_14[i]), &(calibration->Calibration_Table_Channel_16[i]) ); } fclose(fp2); printf("\nCalibration Table\n"); for (i = 0; i < NCAL; i++){ printf( "%5.2f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f\n", i-(NCAL-1)/2.0, calibration->Calibration_Table_Channel_2[i], calibration->Calibration_Table_Channel_4[i], calibration->Calibration_Table_Channel_6[i], calibration->Calibration_Table_Channel_8[i], calibration->Calibration_Table_Channel_10[i], calibration->Calibration_Table_Channel_12[i], calibration->Calibration_Table_Channel_14[i], calibration->Calibration_Table_Channel_16[i] ); } printf("\n\n"); #endif } /******************************************************************************/ /* Function : MotionPak_Correction */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void MotionPak_Correction(IP16ADC_Input* sample, MotionPak_Output* output) { #ifdef SIGNAL_MOTIONPAK_CORRECTION *MCC_RESET_CR &= ~(RESET_CR_BDFLO); #endif #ifdef MOTIONPAK_CORRECTION output->time = sample->time; output->ref = sample->Channel_2; output->ax = sample->Channel_4 / AX_SCALE_FACTOR - AX_BIAS; output->ay = sample->Channel_6 / AY_SCALE_FACTOR - AY_BIAS; output->az = sample->Channel_8 / AZ_SCALE_FACTOR - AZ_BIAS; output->rx = sample->Channel_10 / RX_SCALE_FACTOR - RX_BIAS; output->ry = sample->Channel_12 / RY_SCALE_FACTOR - RY_BIAS; output->rz = sample->Channel_14 / RZ_SCALE_FACTOR - RZ_BIAS; output->temp = sample->Channel_16 / 0.00511 - 273.0; #else output->time = sample->time; output->ref = sample->Channel_2; output->ax = sample->Channel_4; output->ay = sample->Channel_6; output->az = sample->Channel_8; output->rx = sample->Channel_10; output->ry = sample->Channel_12; output->rz = sample->Channel_14; output->temp = sample->Channel_16; #endif #ifdef SIGNAL_MOTIONPAK_CORRECTION *MCC_RESET_CR = RESET_CR_BDFLO; #endif } /******************************************************************************/ /* Function : Get_Time */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /* Notes : This routine has about 16uS of overhead */ /******************************************************************************/ void Get_Time(Flt64* t, Flt64* dt) { static MBool init = TRUE; /* initialization flag */ DM_Time sampleTime; /* update time */ Flt64 time = 0.0; /* absolute time */ static Flt64 time_last = 0.0; /* absolute time */ static Flt64 timeOffset; /* time offset */ gettimeofday(&sampleTime, (struct timezone *) NULL); time = (Flt64) sampleTime.tv_sec + (Flt64) sampleTime.tv_usec / 1000000.0; if (init){ timeOffset = time; init = FALSE; } time -= timeOffset; *dt = time - time_last; *t = time; time_last = time; } /******************************************************************************/ /* Function : Get_Absolute_Time */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /* Notes : This routine has about 16uS of overhead */ /******************************************************************************/ Flt64 Get_Absolute_Time(void) { DM_Time sampleTime; Flt64 t; gettimeofday(&sampleTime, (struct timezone *) NULL); t = (Flt64) sampleTime.tv_sec + (Flt64) sampleTime.tv_usec / 1000000.0; return t; } /******************************************************************************/ /* Function : FIR_Filter_Compute */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void FIR_Filter_Compute(MotionPak_Input_Buffer* u, FIR_Filter* b, MotionPak_Output* y) { Reg int i,j,k,l,n; #ifdef SIGNAL_FIR_FILTER *MCC_RESET_CR &= ~(RESET_CR_BDFLO); #endif j = u->index; n = u->size; y->time = u->time[j]; #ifndef FIR_FILTER y->ref = u->ref[j]; y->ax = u->ax[j]; y->ay = u->ay[j]; y->az = u->az[j]; y->rx = u->rx[j]; y->ry = u->ry[j]; y->rz = u->rz[j]; y->temp = u->temp[j]; #else y->ref = 0.0; y->ax = 0.0; y->ay = 0.0; y->az = 0.0; y->rx = 0.0; y->ry = 0.0; y->rz = 0.0; y->temp = 0.0; for (i = 0; i < b->length; i++){ l = j - i; if (l < 0) l += n; y->ref += u->ref[l] * b->b[i]; y->ax += u->ax[l] * b->b[i]; y->ay += u->ay[l] * b->b[i]; y->az += u->az[l] * b->b[i]; y->rx += u->rx[l] * b->b[i]; y->ry += u->ry[l] * b->b[i]; y->rz += u->rz[l] * b->b[i]; y->temp += u->temp[l] * b->b[i]; } #endif #ifdef SIGNAL_FIR_FILTER *MCC_RESET_CR = RESET_CR_BDFLO; #endif } /******************************************************************************/ /* Function : FIR_Filter_Initialize */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS FIR_Filter_Initialize(FIR_Filter* p, int n) { #ifdef FIR_FILTER #ifdef RELEASED char* filename = "/usr/vw5.1.1/mbari/config/kalman/FIR_coefficients"; #else char* filename = "/usr/vw5.1.1/mbari/kalman/FIR_coefficients"; #endif FILE* fp; int i; Flt32 s = 0.0; printf("FIR_Filter_Initialize\n"); p->length = n; if ( !(p->b = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR);; printf("\nopening file %s\n", filename); if ( (fp = fopen(filename, "r")) == NULL ){ printf("error opening file - %s\n", filename); exit(1); } for (i = 0; i < n; i++){ fscanf(fp, "%e\n", &(p->b[i])); /* printf("%e\n", p->b[i]); */ } fclose(fp); for (i = 0; i < n; i++) s += p->b[i]; printf("FIR filter normalization - %f\n", s); printf("\n"); return(OK); #endif } /******************************************************************************/ /* Function : FIR_Filter_Free */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void FIR_Filter_Free(FIR_Filter* p) { #ifdef FIR_FILTER free(p->b); #endif } /******************************************************************************/ /* Function : MotionPak_Input_Buffer_Update */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void MotionPak_Input_Buffer_Update(MotionPak_Output* x, MotionPak_Input_Buffer* u) { Reg int k; u->index++; u->index = (u->index + u->size) % u->size; k = u->index; #ifdef SIGNAL_BUFFER_INPUT *MCC_RESET_CR &= ~(RESET_CR_BDFLO); #endif u->time[k] = x->time; u->ref[k] = x->ref; u->ax[k] = x->ax; u->ay[k] = x->ay; u->az[k] = x->az; u->rx[k] = x->rx; u->ry[k] = x->ry; u->rz[k] = x->rz; u->temp[k] = x->temp; #ifdef SIGNAL_BUFFER_INPUT *MCC_RESET_CR = RESET_CR_BDFLO; #endif } /******************************************************************************/ /* Function : MotionPak_Input_Buffer_Initialize */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS MotionPak_Input_Buffer_Initialize(MotionPak_Input_Buffer* p, int n) { Reg int i; printf("MotionPak_Input_Buffer_Initialize\n"); p->index = -1; p->size = n; if ( !(p->time = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ref = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ax = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ay = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->az = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->rx = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ry = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->rz = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->temp = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); for (i = 0; i < n; i++){ p->time[i] = 0.0; p->ref[i] = 0.0; p->ax[i] = 0.0; p->ay[i] = 0.0; p->az[i] = 0.0; p->rx[i] = 0.0; p->ry[i] = 0.0; p->rz[i] = 0.0; p->temp[i] = 0.0; } return(OK); } /******************************************************************************/ /* Function : MotionPak_Input_Buffer_Free */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void MotionPak_Input_Buffer_Free(MotionPak_Input_Buffer* p) { free(p->time); free(p->ref); free(p->ax); free(p->ay); free(p->az); free(p->rx); free(p->ry); free(p->rz); free(p->temp); } /******************************************************************************/ /* Function : Timer3_Connect */ /* Purpose : */ /* Inputs : Function pointer to Timer3 interrupt service routine */ /* Outputs : */ /******************************************************************************/ void Timer3_Connect(FUNCPTR routine, int arg) { Timer3_Routine = routine; Timer3_Arg = arg; } /******************************************************************************/ /* Function : Timer3_Disable */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void Timer3_Disable(void) { printf("Timer3_Disable\n"); if (Timer3_Running == TRUE){ *MCC_TIMER3_CR = TIMER3_CR_DIS; /* and disable counter */ *MCC_T3_IRQ_CR = T3_IRQ_CR_DIS; /* disable interrupts */ Timer3_Running = FALSE; *MCC_RESET_CR = RESET_CR_BDFLO; } } /******************************************************************************/ /* Function : Timer3_Enable */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void Timer3_Enable(void) { printf("Timer3_Enable\n"); if (Timer3_Running == FALSE){ *MCC_T3_IRQ_CR = T3_IRQ_CR_IEN | T3_IRQ_CR_ICLR | SYS_CLK_LEVEL; *MCC_TIMER3_CR = TIMER3_CR_CEN | TIMER3_CR_COC; Timer3_Running = TRUE; } } /******************************************************************************/ /* Function : Timer3_Init */ /* Purpose : Initializes Timer3 on the MVME162 board's MCC chip */ /* Inputs : Timer period in seconds (small unit is 1.0 microsecond) */ /* Outputs : */ /* Notes : Timer3 clock is 1.0MHz */ /******************************************************************************/ void Timer3_Init(Flt32 period) { printf("Timer3_Init\n"); *MCC_T3_IRQ_CR = T3_IRQ_CR_DIS; /* disable interrupts */ *MCC_TIMER3_CR = TIMER3_CR_DIS; /* and disable counter */ *MCC_T3_IRQ_CR |= T3_IRQ_CR_ICLR; *MCC_TIMER3_CMP = (int) 1000000*period; *MCC_TIMER3_CNT = 0; intConnect((VOIDFUNCPTR *) INUM_TO_IVEC(INT_VEC_T3), Timer3_Interrupt_Service_Routine, NULL); Timer3_Running = FALSE; } /******************************************************************************/ /* Function : Interrupt_Disable */ /******************************************************************************/ void Interrupt_Disable(void) { Timer3_Disable(); } /******************************************************************************/ /* Function : Timer3_Interrupt_Service_Routine */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void Timer3_Interrupt_Service_Routine(void) { if (Timer3_Routine != NULL) (*Timer3_Routine) (Timer3_Arg); *MCC_T3_IRQ_CR |= T3_IRQ_CR_ICLR; } /******************************************************************************/ /* Function : Sample_Interrupt_Routine */ /* Purpose : Timer3 interrupt service routine for sampling */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ int Sample_Interrupt_Routine(int p) { #ifdef FAIL_LED_BLINKING static int i = 0; #endif static int k = 0; static int j = 0; static MBool state = FALSE; static Flt64 btime = 0; Flt64 time, dtime; IP16ADC_Input sample; MotionPak_Output output; FP_CONTEXT fp_context; int level; /* level = intLock(); */ fppSave(&fp_context); Get_Time(&time, &dtime); sample.time = time; Sample_Input(&sample, SAMPLE_MODE); Linear_Correction(&sample, &calibration); MotionPak_Correction(&sample, &output); MotionPak_Input_Buffer_Update(&output, inbuff); if (++k == DECIMATION){ k = 0; semGive(Sem); } fppRestore(&fp_context); /* intUnlock(level); */ return 0; } /******************************************************************************/ /* Function : MotionPak_Initialize */ /* Purpose : Initializes the IP-16ADC converter connected to the MotionPak */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS MotionPak_Initialize(void) { #ifdef IP16ADC_INSTALLED printf("MotionPak_Initialize\n"); *IP16ADC(CTL_STATUS_REG) = RESET_COMMAND; *IP16ADC(CTL_STATUS_REG) = INIT_MODE; while ( !(*IP16ADC(CTL_STATUS_REG) & STDBY) ) { } *IP16ADC(CTL_STATUS_REG) = SAMPLE_MODE | BIPOLAR | CH02; #else printf("IP16ADC board not installed\n"); #endif return(OK); } /******************************************************************************/ /* Function : Read_Heading_Filter_Parameters */ /* Purpose : Reads in parameter file */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS Read_Heading_Filter_Parameters(char* file, Heading_State* x, Heading_Filter_Parameters* p) { FILE* fp; char* string; string = (char*) malloc(50); printf("opening parameter file %s\n", file); if ( (fp = fopen(file, "r")) == NULL ){ logMsg("error opening parameter file - %s\n", file); exit(1); } /* read header */ fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); fgets(string, 100, fp); /* read initial covariances */ fscanf(fp, "%s %e\n", string, &(p->alpha)); fscanf(fp, "%s %e\n", string, &(p->beta)); fscanf(fp, "%s %e\n", string, &(p->omega0)); fscanf(fp, "%s %e\n", string, &(p->omega1)); fscanf(fp, "%s %e\n", string, &(p->xi0)); fscanf(fp, "%s %e\n", string, &(p->xi1)); fgets(string, 100, fp); /* read initial covariances */ fscanf(fp, "%s %e\n", string, &(p->p1)); fscanf(fp, "%s %e\n", string, &(p->p2)); fscanf(fp, "%s %e\n", string, &(p->p3)); fscanf(fp, "%s %e\n", string, &(p->p4)); fscanf(fp, "%s %e\n", string, &(p->p5)); fscanf(fp, "%s %e\n", string, &(p->p6)); fgets(string, 100, fp); /* read initial states */ fscanf(fp, "%s %e\n", string, &(x->ax)); fscanf(fp, "%s %e\n", string, &(x->ay)); fscanf(fp, "%s %e\n", string, &(x->az)); fscanf(fp, "%s %e\n", string, &(x->psi)); fscanf(fp, "%s %e\n", string, &(x->theta)); fscanf(fp, "%s %e\n", string, &(x->phi)); fscanf(fp, "%s %e\n", string, &(x->p)); fscanf(fp, "%s %e\n", string, &(x->q)); fscanf(fp, "%s %e\n", string, &(x->r)); fscanf(fp, "%s %e\n", string, &(x->b1)); fscanf(fp, "%s %e\n", string, &(x->b2)); fscanf(fp, "%s %e\n", string, &(x->g1)); fscanf(fp, "%s %e\n", string, &(x->g2)); fgets(string, 100, fp); /* read system noise parameters */ fscanf(fp, "%s %e\n", string, &(p->w1)); fscanf(fp, "%s %e\n", string, &(p->w2)); fscanf(fp, "%s %e\n", string, &(p->w3)); fscanf(fp, "%s %e\n", string, &(p->w4)); fscanf(fp, "%s %e\n", string, &(p->w5)); fscanf(fp, "%s %e\n", string, &(p->w6)); fgets(string, 100, fp); /* read sensor noise parameters */ fscanf(fp, "%s %e\n", string, &(p->v1)); fscanf(fp, "%s %e\n", string, &(p->v2)); fscanf(fp, "%s %e\n", string, &(p->v3)); fscanf(fp, "%s %e\n", string, &(p->v4)); /* square std deviation to get variance */ p->w1 *= p->w1; p->w2 *= p->w2; p->w3 *= p->w3; p->w4 *= p->w4; p->w5 *= p->w5; p->w6 *= p->w6; p->v1 *= p->v1; p->v2 *= p->v2; p->v3 *= p->v3; p->v4 *= p->v4; free(string); fclose(fp); return(OK); } /******************************************************************************/ /* Function : Read_Kalman_Filter_Parameters */ /* Purpose : Reads in parameter file */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS Read_Kalman_Filter_Parameters(char* file, State* x, Kalman_Filter_Parameters* p) { FILE* fp; char* string; string = (char*) malloc(50); printf("opening parameter file %s\n", file); if ( (fp = fopen(file, "r")) == NULL ){ logMsg("error opening parameter file - %s\n", file); exit(1); } /* read header */ fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); printf("%s", string); fgets(string, 100, fp); fgets(string, 100, fp); /* read initial covariances */ fscanf(fp, "%s %e\n", string, &(p->alpha)); fscanf(fp, "%s %e\n", string, &(p->omega0)); fscanf(fp, "%s %e\n", string, &(p->omega1)); fscanf(fp, "%s %e\n", string, &(p->xi0)); fscanf(fp, "%s %e\n", string, &(p->xi1)); fgets(string, 100, fp); /* read initial covariances */ fscanf(fp, "%s %e\n", string, &(p->p1)); fscanf(fp, "%s %e\n", string, &(p->p2)); fscanf(fp, "%s %e\n", string, &(p->p3)); fscanf(fp, "%s %e\n", string, &(p->p4)); fscanf(fp, "%s %e\n", string, &(p->p5)); fscanf(fp, "%s %e\n", string, &(p->p6)); fgets(string, 100, fp); /* read initial states */ fscanf(fp, "%s %e\n", string, &(x->ax)); fscanf(fp, "%s %e\n", string, &(x->ay)); fscanf(fp, "%s %e\n", string, &(x->az)); fscanf(fp, "%s %e\n", string, &(x->psi)); fscanf(fp, "%s %e\n", string, &(x->theta)); fscanf(fp, "%s %e\n", string, &(x->phi)); fscanf(fp, "%s %e\n", string, &(x->p)); fscanf(fp, "%s %e\n", string, &(x->q)); fscanf(fp, "%s %e\n", string, &(x->r)); fgets(string, 100, fp); /* read system noise parameters */ fscanf(fp, "%s %e\n", string, &(p->w1)); fscanf(fp, "%s %e\n", string, &(p->w2)); fscanf(fp, "%s %e\n", string, &(p->w3)); fscanf(fp, "%s %e\n", string, &(p->w4)); fscanf(fp, "%s %e\n", string, &(p->w5)); fscanf(fp, "%s %e\n", string, &(p->w6)); fgets(string, 100, fp); /* read sensor noise parameters */ fscanf(fp, "%s %e\n", string, &(p->v1)); fscanf(fp, "%s %e\n", string, &(p->v2)); fscanf(fp, "%s %e\n", string, &(p->v3)); fscanf(fp, "%s %e\n", string, &(p->v4)); /* square std deviation to get variance */ p->w1 *= p->w1; p->w2 *= p->w2; p->w3 *= p->w3; p->w4 *= p->w4; p->w5 *= p->w5; p->w6 *= p->w6; p->v1 *= p->v1; p->v2 *= p->v2; p->v3 *= p->v3; p->v4 *= p->v4; free(string); fclose(fp); return(OK); } /******************************************************************************/ /* Function : Get_DM_Measurements */ /* Purpose : Reads compass, gyro, DVL, and USBL DM data */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS Get_DM_Measurements(kalmanDMItems* kalmanDmi, DM_Group sensorGroup, sensorBits* sensorId, Measurements* z) { DWord sensorGroupBits; MBool compassDataValid; MBool gyroDataValid; MBool DVLDataValid; /* check for sensor value change */ sensorGroupBits = dm_get_group_changes(sensorGroup); /* update compass heading */ if (sensorGroupBits & sensorId->compassBit){ dm_read(kalmanDmi->compassDataValid, (Void *) &compassDataValid, sizeof(compassDataValid), (DM_Time * ) NULL); if (compassDataValid){ dm_read(kalmanDmi->compassRadians, (Void *) &z->compass, sizeof(z->compass), (DM_Time * ) NULL); } z->compassFlag = TRUE; } /* update gyro heading and heading rate */ if (sensorGroupBits & sensorId->gyroBit){ dm_read(kalmanDmi->gyroDataValid, (Void *) &gyroDataValid, sizeof(gyroDataValid), (DM_Time * ) NULL); if (gyroDataValid){ dm_read(kalmanDmi->gyroRadians, (Void *) &z->gyro, sizeof(z->gyro), (DM_Time * ) NULL); dm_read(kalmanDmi->gyroRateRadians, (Void *) &z->gyroRate, sizeof(z->gyroRate), (DM_Time * ) NULL); } z->gyroFlag = TRUE; } return OK; } /******************************************************************************/ /* Function : Output_ROV_State_DM */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS Output_ROV_State_DM(kalmanDMItems* kalmanDmi, Heading_State* state, DM_Time* sampleTime) { /* update state */ dm_write(kalmanDmi->Heading, (Void *) &state->psi, sizeof(state->psi), sampleTime); dm_write(kalmanDmi->Heading_Rate, (Void *) &state->r, sizeof(state->r), sampleTime); dm_write(kalmanDmi->Pitch, (Void *) &state->theta, sizeof(state->phi), sampleTime); dm_write(kalmanDmi->Pitch_Rate, (Void *) &state->q, sizeof(state->q), sampleTime); dm_write(kalmanDmi->Roll, (Void *) &state->phi, sizeof(state->phi), sampleTime); dm_write(kalmanDmi->Roll_Rate, (Void *) &state->p, sizeof(state->p), sampleTime); dm_write(kalmanDmi->Acceleration_X, (Void *) &state->ax, sizeof(state->ax), sampleTime); dm_write(kalmanDmi->Acceleration_Y, (Void *) &state->ay, sizeof(state->ay), sampleTime); dm_write(kalmanDmi->Acceleration_Z, (Void *) &state->az, sizeof(state->az), sampleTime); dm_write(kalmanDmi->Heading_Rate_Bias, (Void *) &state->b1, sizeof(state->b1), sampleTime); dm_write(kalmanDmi->Heading_Rate_Bias_2D, (Void *) &state->b2, sizeof(state->b2), sampleTime); dm_write(kalmanDmi->Gyro_Drift, (Void *) &state->g1, sizeof(state->g1), sampleTime); dm_write(kalmanDmi->Gyro_Drift_2D, (Void *) &state->g2, sizeof(state->g2), sampleTime); } /******************************************************************************/ /* Function : Kalman_Filter */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS Kalman_Filter(Void) { kalmanDMItems kalmanDmi; /* data manager items */ SEM_ID kalmanSem; /* task wakeup semaphore */ sensorBits sensorId; DM_Group sensorGroup; /* data manager group */ Int16 statusKalman = START; Int16 statusKalmanlast = START; MBool init = TRUE; /* initialization flag */ DM_Time sampleTime; /* update time */ Flt64 time = 0.0; /* absolute time */ Flt64 time_last = 0.0; /* absolute time */ Flt64 timeOffset; /* time offset */ Flt64 dtime; Flt32 gyro_offset = 0; Flt32 heading_offset = 0; Reg int i = 0,j; char* parameter_file = PARAMETER_FILE; MotionPak_Output MP_inputs; MotionPak_Input_Buffer input_buffer; FIR_Filter b; Heading_State state; /* filter state vector */ Measurements inputs = {FALSE, FALSE, 0, 0, 0}; /* filter input vector */ Heading_Filter_Parameters parameters; Timer3_Disable(); inbuff = &input_buffer; if (MotionPak_Input_Buffer_Initialize(&input_buffer, INPUT_BUFFER_SIZE) == ERROR){ logMsg("MotionPak_Input_Buffer_Initialize FAILURE\n"); return(ERROR); } FIR_Filter_Initialize(&b, FIR_FILTER_LENGTH); Read_Heading_Filter_Parameters(parameter_file, &state, ¶meters); Read_Calibration_Table(&calibration); /* Heading filter initialize */ if (Heading_Filter_Initialize(&state, ¶meters) == ERROR){ logMsg("Heading_Filter_Initialize FAILURE\n"); return(ERROR); } MotionPak_Initialize(); /* initialize sample interrupt timer 3 */ Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Sample_Interrupt_Routine, NULL); /* initialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); return(ERROR); } /* initialize data manager items */ printf("Init_Kalman_Filter\n"); if (Init_Kalman_Filter(&kalmanDmi, &sensorGroup, &sensorId) == ERROR) return(ERROR); /* start sampling */ Timer3_Enable(); /* filtering loop */ for (;;){ /* wait for sampling semaphore at 50Hz */ semTake(Sem, WAIT_FOREVER); /* FIR filter buffered data */ FIR_Filter_Compute(&input_buffer, &b, &MP_inputs); /* read all available measurement DM items */ Get_DM_Measurements(&kalmanDmi, sensorGroup, &sensorId, &inputs); /* Kalman filter routine */ Heading_Filter_State_Estimation(&inputs, &MP_inputs, &state); /* update filtered data manager items */ gettimeofday(&sampleTime, (struct timezone *) NULL); time = (Flt64) sampleTime.tv_sec + (Flt64) sampleTime.tv_usec / 1000000.0; if (init){ timeOffset = time; init = FALSE; } time -= timeOffset; /* write out all ROV state DM items */ Output_ROV_State_DM(&kalmanDmi, &state, &sampleTime); #if 1 if (!(++j%200)) printf("%7.4f %7.4f %7.4f %7.4f\n", time, state.psi - heading_offset, state.r, state.b1 ); #endif /* read status words */ dm_read(kalmanDmi.Status, (Void *) &statusKalman, sizeof(statusKalman), (DM_Time * ) NULL); if (statusKalman != statusKalmanlast){ if (statusKalman == STOP) break; if (statusKalman == HEADING_RESET) heading_offset = state.psi; statusKalmanlast = statusKalman; } } Timer3_Disable(); MotionPak_Input_Buffer_Free(&input_buffer); FIR_Filter_Free(&b); printf("Kalman_Filter done\n"); return(OK); } /******************************************************************************/ /* Function : Kalman_Filter_Offline */ /* Purpose : Runs Kalman using data files */ /* Inputs : */ /* Outputs : ref - gyro input data */ /* ax - psi */ /* ay - r */ /* az - b1 */ /* rx - b2 */ /* ry - g1 */ /* rz - g2 */ /* temp - r input data */ /******************************************************************************/ STATUS Kalman_Filter_Offline(char* filei) { Int16 statusKalman = START; MBool init = TRUE; /* initialization flag */ Flt64 time = 0.0; /* absolute time */ Flt64 time_last = 0.0; /* absolute time */ Flt64 timeOffset; /* time offset */ Flt64 dtime; Flt32 gyro_offset = 0; Flt32 gyro_last = 0; Flt32 compass_last = 0; Reg int i = 0,j; char filename1[100]; char filename2[100]; char* file = "mp_data_12"; /* char* prefix = "/users/matthews/matlab/data/"; */ char* prefix = "/usr/vw5.1.1/mbari/kalman/data/"; char* suffix = "_filtered"; char* parameter_file = PARAMETER_FILE; Heading_State state; Measurements inputs = {FALSE, FALSE, 0, 0, 0}; MotionPak_Output MP_inputs; MotionPak_Output_Buffer p; FILE* fp1; FILE* fp2; Int16 nbuff = 0; MotionPak_Output Kalman_filter_output; MotionPak_Output_Buffer output_buffer; Heading_Filter_Parameters parameters; Read_Heading_Filter_Parameters(parameter_file, &state, ¶meters); strcpy(filename1, prefix); strcat(filename1, filei); printf("opening input file %s\n", filename1); if ( (fp1 = fopen(filename1, "rb")) == NULL ){ logMsg("error opening input file - %s\n", filename1); exit(1); } strcpy(filename2, prefix); strcat(filename2, filei); strcat(filename2, suffix); printf("opening output file %s\n", filename2); if ( (fp2 = fopen(filename2, "wb")) == NULL ){ logMsg("error opening output file - %s\n", filename2); return(ERROR); } /* rewind input file pointer with offset past header */ fseek(fp1, 2*sizeof(Int16) + sizeof(Flt64), SEEK_SET); /* read in first compass value for gyro offset and count buffers */ if (MotionPak_Output_Buffer_Initialize(&p, OUTPUT_BUFFER_SIZE) == ERROR){ logMsg("MotionPak_Output_Buffer_Initialize FAILURE\n"); return(ERROR); } if (MotionPak_Read_Buffer(&p, fp1) == OUTPUT_BUFFER_SIZE*9){ nbuff = 1; i = 0; j = 0; while ( (p.ax[i] == -999) && (i < OUTPUT_BUFFER_SIZE) ) i++; if (p.ax[i] == -999) p.ax[i] = 0.0; while ( (p.ay[j] == -999) && (j < OUTPUT_BUFFER_SIZE) ) j++; if (p.ay[j] == -999){ logMsg("FAILURE : no gyro data available\n"); return(ERROR); } compass_last = p.ax[i]; gyro_last = p.ay[j]; gyro_offset = p.ax[i] - p.ay[j]; printf("gyro initial - %f\n", gyro_last); printf("gyro offset - %f\n", gyro_offset); while (MotionPak_Read_Buffer(&p, fp1) == OUTPUT_BUFFER_SIZE*9) nbuff++; } else exit(0); printf("number of buffers - %d\n", nbuff); if (MotionPak_Output_Buffer_Initialize(&output_buffer, OUTPUT_BUFFER_SIZE) == ERROR){ logMsg("MotionPak_Output_Buffer_Initialize FAILURE\n"); return(ERROR); } /* first 8 bytes are start time */ time = Get_Absolute_Time(); printf("start time - %f\n", time); fwrite((char*) &time, sizeof(Flt64), 1, fp2); /* next 2 bytes are number of buffers */ fwrite((char*) &nbuff, sizeof(Int16), 1, fp2); /* next 2 bytes are buffer size */ fwrite((char*) &(output_buffer.size), sizeof(Int16), 1, fp2); /* Heading filter initialize */ state.psi = compass_last; Heading_Filter_Initialize(&state, ¶meters); /* rewind input file pointer with offset past header */ fseek(fp1, 2*sizeof(Int16) + sizeof(Flt64), SEEK_SET); /* filtering loop */ while (MotionPak_Read_Buffer(&p, fp1) == OUTPUT_BUFFER_SIZE*9){ inputs.compassFlag = FALSE; inputs.gyroFlag = FALSE; for (i = 0; i < OUTPUT_BUFFER_SIZE; i++){ /* read compass and gyro data and flag if valid; if compass or gyro */ /* moved more than 20 degrees then correct for modulo 360 overflow */ inputs.compass = p.ax[i]; if (inputs.compass != -999){ if (fabs(compass_last - inputs.compass) > 20*M_PI/180) inputs.compass += 360*ceil(compass_last/360) * M_PI/180; compass_last = inputs.compass; inputs.compassFlag = TRUE; } inputs.gyro = p.ay[i]; inputs.gyroRate = p.az[i]; if (inputs.gyro != -999){ if (fabs(gyro_last - inputs.gyro) > 20*M_PI/180) inputs.gyro += 360*ceil(gyro_last/360) * M_PI/180; gyro_last = inputs.gyro; inputs.gyro += gyro_offset; inputs.gyroFlag = TRUE; } /*****/ inputs.compassFlag = FALSE; inputs.gyroFlag = FALSE; /*****/ /* read in MP data */ MP_inputs.rx = p.rx[i]; MP_inputs.ry = p.ry[i]; MP_inputs.rz = p.rz[i]; #if 0 printf("%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", MP_inputs.time, inputs.compass, inputs.gyro, inputs.gyroRate, MP_inputs.rx, MP_inputs.ry, MP_inputs.rz ); #endif /* Kalman filter routine */ if (i != 0) Heading_Filter_State_Estimation(&inputs, &MP_inputs, &state); Kalman_filter_output.time = p.time[i]; Kalman_filter_output.ref = inputs.gyro; Kalman_filter_output.ax = state.psi; Kalman_filter_output.ay = state.r; Kalman_filter_output.az = state.b1; Kalman_filter_output.rx = state.b2; Kalman_filter_output.ry = state.g1; Kalman_filter_output.rz = state.g2; Kalman_filter_output.temp = MP_inputs.rz; /* write to output buffer */ MotionPak_Output_Buffer_Update(&output_buffer, &Kalman_filter_output); #if 0 if (!(++i%100)) printf("%7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f\n", Kalman_filter_output.time, Kalman_filter_output.ref, Kalman_filter_output.ax, Kalman_filter_output.ay, Kalman_filter_output.az, Kalman_filter_output.rx, Kalman_filter_output.ry, Kalman_filter_output.rz ); #endif } MotionPak_Write_Buffer(&output_buffer, fp2); } fclose(fp1); fclose(fp2); MotionPak_Output_Buffer_Free(&p); MotionPak_Output_Buffer_Free(&output_buffer); printf("Kalman_Filter_Offline done\n"); return(OK); } /******************************************************************************/ /* Function : Init_Kalman_Filter */ /* Purpose : Performs kalman filter data manager items initialization and */ /* checks. */ /* Inputs : Kalman data manager items structure pointer, data manager group */ /* pointer and group id bits pointer. */ /* Outputs : Return OK or ERROR. */ /******************************************************************************/ STATUS Init_Kalman_Filter(kalmanDMItems* kalmanDmi, DM_Group* sensorGroup, sensorBits* sensorId) { Errno status; /* error code */ Int16 item; /* item counter */ Int16 statusKalman = START; /* status bit for dm */ /* data mgr sensor item name prefix */ char sensorDMPrefix[] = SENSOR_DM_PREFIX; /* data manager sensor item name */ char *sensorDMName; /* compass radians item name */ MLocal char compassRadiansName[DM_ITEM_NAME_LEN]; /* compass data valid item name */ MLocal char compassDataValidName[DM_ITEM_NAME_LEN]; /* gyro radians item name */ MLocal char gyroRadiansName[DM_ITEM_NAME_LEN]; /* gyro rate radians item name */ MLocal char gyroRateRadiansName[DM_ITEM_NAME_LEN]; /* gyro data valid item name */ MLocal char gyroDataValidName[DM_ITEM_NAME_LEN]; /* data manager items create values */ struct { char *name; /* data manager array name */ DM_Type type; /* data manager item type */ DM_Num num; /* item number */ DM_Item *item; /* data manager item handle */ } dmiCreate[] = { { compassRadiansName, DM_FLT32, 1, &kalmanDmi->compassRadians }, { gyroRadiansName, DM_FLT32, 1, &kalmanDmi->gyroRadians }, { gyroRateRadiansName, DM_FLT32, 1, &kalmanDmi->gyroRateRadians }, { compassDataValidName, DM_MBOOL, 1, &kalmanDmi->compassDataValid }, { gyroDataValidName, DM_MBOOL, 1, &kalmanDmi->gyroDataValid }, { HEADING_DM, DM_FLT32, 1, &kalmanDmi->Heading }, { HEADING_RATE_DM, DM_FLT32, 1, &kalmanDmi->Heading_Rate }, { PITCH_DM, DM_FLT32, 1, &kalmanDmi->Pitch }, { PITCH_RATE_DM, DM_FLT32, 1, &kalmanDmi->Pitch_Rate }, { ROLL_DM, DM_FLT32, 1, &kalmanDmi->Roll }, { ROLL_RATE_DM, DM_FLT32, 1, &kalmanDmi->Roll_Rate }, { ACCELERATION_X_DM, DM_FLT32, 1, &kalmanDmi->Acceleration_X }, { ACCELERATION_Y_DM, DM_FLT32, 1, &kalmanDmi->Acceleration_Y }, { ACCELERATION_Z_DM, DM_FLT32, 1, &kalmanDmi->Acceleration_Z }, { HEADING_RATE_BIAS_DM, DM_FLT32, 1, &kalmanDmi->Heading_Rate_Bias }, { HEADING_RATE_BIAS_2D_DM, DM_FLT32, 1, &kalmanDmi->Heading_Rate_Bias_2D }, { GYRO_DRIFT_DM, DM_FLT32, 1, &kalmanDmi->Gyro_Drift }, { GYRO_DRIFT_2D_DM, DM_FLT32, 1, &kalmanDmi->Gyro_Drift_2D }, { NULL, DM_ENDT, 0, NO_ITEM } }; /* create sensor data manager item names */ sensorDMName = COMPASS_RADIANS_DM; strcpy(compassRadiansName, sensorDMPrefix); strcat(compassRadiansName, sensorDMName); sensorDMName = COMPASS_DATA_VALID_DM; strcpy(compassDataValidName, sensorDMPrefix); strcat(compassDataValidName, sensorDMName); sensorDMName = GYRO_RADIANS_DM; strcpy(gyroRadiansName, sensorDMPrefix); strcat(gyroRadiansName, sensorDMName); sensorDMName = GYRO_RATE_RADIANS_DM; strcpy(gyroRateRadiansName, sensorDMPrefix); strcat(gyroRateRadiansName, sensorDMName); sensorDMName = GYRO_DATA_VALID_DM; strcpy(gyroDataValidName, sensorDMPrefix); strcat(gyroDataValidName, sensorDMName); /* create data manager items */ for (item = 0; dmiCreate[item].item != NO_ITEM; item++){ status = dm_create(dmiCreate[item].name, dmiCreate[item].num, dmiCreate[item].item, dmiCreate[item].type, 1, DM_ENDT); if (checkCreate(status, dmiCreate[item].name, TRUE) == ERROR) return(ERROR); } /* create status bit data manager item */ status = dm_create(KALMAN_FILTER_STOP_DM, 1, &kalmanDmi->Status, DM_INT16, 1, DM_ENDT); if (checkCreate(status, KALMAN_FILTER_STOP_DM, TRUE) == ERROR) return(ERROR); status = dm_start_multiple(kalmanDmi->Status); dm_write(kalmanDmi->Status, (Void *) &statusKalman, sizeof(statusKalman), (DM_Time*) NULL); status = dm_start_consumer(kalmanDmi->Status, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Kalman Filter - status", TRUE) == ERROR) return(ERROR); /* start consumer items and check status */ status = dm_start_consumer(kalmanDmi->compassRadians, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Kalman Filter - compass radians", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(kalmanDmi->gyroRadians, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Kalman Filter - gyro radians", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(kalmanDmi->gyroRateRadians, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Kalman Filter - gyro rate", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(kalmanDmi->compassDataValid, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Kalman Filter - compass data valid", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(kalmanDmi->gyroDataValid, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Kalman Filter - gyro data valid", TRUE) == ERROR) return(ERROR); /* create data manager group */ *sensorGroup = dm_create_group(); /* add consumer items to group */ status = dm_group_add_item(*sensorGroup, kalmanDmi->compassRadians, &sensorId->compassBit); if (checkGroup(status, "Kalman Filter - compass") == ERROR) return(ERROR); status = dm_group_add_item(*sensorGroup, kalmanDmi->gyroRateRadians, &sensorId->gyroBit); if (checkGroup(status, "Kalman Filter - gyro") == ERROR) return(ERROR); /* start provider items and check status */ status = dm_start_provider(kalmanDmi->Heading, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - heading") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Pitch, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - pitch") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Roll, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - roll") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Heading_Rate, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - heading rate") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Pitch_Rate, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - pitch rate") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Roll_Rate, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - roll rate") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Acceleration_X, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - ax") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Acceleration_Y, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - ay") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Acceleration_Z, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - az") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Heading_Rate_Bias, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - heading rate bias") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Heading_Rate_Bias_2D, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - heading rate bias 2D") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Gyro_Drift, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - gyro drift") == ERROR) return(ERROR); status = dm_start_provider(kalmanDmi->Gyro_Drift_2D, KALMAN_DM_PERIOD); if (checkProvider(status, "Kalman Filter - gyro drift 2nd order") == ERROR) return(ERROR); } /* Init_Kalman_Filter */ /******************************************************************************/ /* Function : Benchmarking */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ #define BM 10 STATUS Benchmarking(Void) { SEM_ID kalmanSem; /* task wakeup semaphore */ Flt64 time = 0.0; /* absolute time */ Flt64 time_last = 0.0; /* absolute time */ Flt64 timeOffset; /* time offset */ Flt64 dtime; Reg int i = 0,j; MotionPak_Output MP_inputs; MotionPak_Input_Buffer input_buffer; FIR_Filter b; State state; /* filter state vector */ Measurements inputs; /* filter input vector */ /* benchmark vectors and matrices */ x = Vector_Create(BM); x->name = "x"; y = Vector_Create(BM); y->name = "y"; S = Matrix_Create(BM,BM); S->name = "S"; Matrix_Identity(S); Matrix_Scalar_Multiplication(S,10.0,S); P = Matrix_Create(BM,BM); P->name = "P"; Matrix_Identity(P); Matrix_Scalar_Multiplication(P,10.0,P); Q = Matrix_Create(BM,BM); Q->name = "Q"; Matrix_Identity(Q); Matrix_Scalar_Multiplication(Q,10.0,Q); inbuff = &input_buffer; MotionPak_Input_Buffer_Initialize(&input_buffer, INPUT_BUFFER_SIZE); FIR_Filter_Initialize(&b, FIR_FILTER_LENGTH); Read_Calibration_Table(&calibration); /* MotionPak_Initialize(); */ /* initialize sample interrupt timer 3 */ Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Sample_Interrupt_Routine, NULL); /* initialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); return(ERROR); } /* start sampling */ Timer3_Enable(); /* filtering loop */ for (;;){ /* wait for sampling semaphore at 50Hz */ semTake(Sem, WAIT_FOREVER); *MCC_RESET_CR &= ~(RESET_CR_BDFLO); /* FIR filter buffered data */ /* FIR_Filter_Compute(&input_buffer, &b, &MP_inputs); */ Matrix_Vector_Multiplication(P,x,y); *MCC_RESET_CR = RESET_CR_BDFLO; Delay(100); *MCC_RESET_CR &= ~(RESET_CR_BDFLO); Matrix_Addition(P,S,Q); *MCC_RESET_CR = RESET_CR_BDFLO; Delay(100); *MCC_RESET_CR &= ~(RESET_CR_BDFLO); Matrix_Multiplication(P,S,Q); *MCC_RESET_CR = RESET_CR_BDFLO; Delay(100); *MCC_RESET_CR &= ~(RESET_CR_BDFLO); Matrix_Inverse(P,S); *MCC_RESET_CR = RESET_CR_BDFLO; } Timer3_Disable(); MotionPak_Input_Buffer_Free(&input_buffer); FIR_Filter_Free(&b); Vector_Destroy(x); Vector_Destroy(y); Matrix_Destroy(P); Matrix_Destroy(Q); Matrix_Destroy(S); printf("done\n"); } /******************************************************************************/ /* Function : Logging */ /* Purpose : Log MotionPak, gyro, and compass at 50Hz */ /* Inputs : */ /* Outputs : */ /* Notes : struct MotionPak_Output is 41 bytes */ /******************************************************************************/ STATUS Logging(Void) { kalmanDMItems kalmanDmi; /* data manager items */ SEM_ID kalmanSem; /* task wakeup semaphore */ DM_Group sensorGroup; /* data manager group */ DWord sensorGroupBits; /* sensor group changes */ sensorBits sensorId; /* sensor id bits */ MBool compassDataValid; /* sensor data valid flags */ MBool gyroDataValid; MBool init = TRUE; /* initialization flag */ DM_Time sampleTime; /* update time */ Flt64 time = 0.0; /* absolute time */ Flt64 time_last = 0.0; /* absolute time */ Flt64 timeOffset; /* time offset */ Flt64 dtime; Reg int i = 0,j; MotionPak_Output MP_inputs; MotionPak_Input_Buffer input_buffer; FIR_Filter b; State state; /* filter state vector */ Measurements inputs; /* filter input vector */ inbuff = &input_buffer; MotionPak_Input_Buffer_Initialize(&input_buffer, INPUT_BUFFER_SIZE); FIR_Filter_Initialize(&b, FIR_FILTER_LENGTH); Read_Calibration_Table(&calibration); MotionPak_Initialize(); /* initialize sample interrupt timer 3 */ Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Sample_Interrupt_Routine, NULL); /* initialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); return(ERROR); } /* initialize data manager items */ if (Init_Kalman_Filter(&kalmanDmi, &sensorGroup, &sensorId) == ERROR) return(ERROR); /* start sampling */ Timer3_Enable(); /* logging loop */ for (;;){ /* wait for sampling semaphore at 50Hz */ semTake(Sem, WAIT_FOREVER); /* FIR filter buffered data */ FIR_Filter_Compute(&input_buffer, &b, &MP_inputs); /* time stamp MP data */ gettimeofday(&sampleTime, (struct timezone *) NULL); time = (Flt64) sampleTime.tv_sec + (Flt64) sampleTime.tv_usec / 1000000.0; if (init){ timeOffset = time; init = FALSE; } time -= timeOffset; MP_inputs.time = time; /* check for sensor value change */ sensorGroupBits = dm_get_group_changes(sensorGroup); /* read compass DM item */ if (sensorGroupBits & sensorId.compassBit){ dm_read(kalmanDmi.compassDataValid, (Void *) &compassDataValid, sizeof(compassDataValid), (DM_Time * ) NULL); if (compassDataValid){ dm_read(kalmanDmi.compassRadians, (Void *) &inputs.compass, sizeof(inputs.compass), (DM_Time * ) NULL); } inputs.compassFlag = TRUE; } /* read gyro DM item */ if (sensorGroupBits & sensorId.gyroBit){ dm_read(kalmanDmi.gyroDataValid, (Void *) &gyroDataValid, sizeof(gyroDataValid), (DM_Time * ) NULL); if (gyroDataValid){ dm_read(kalmanDmi.gyroRadians, (Void *) &inputs.gyro, sizeof(inputs.gyro), (DM_Time * ) NULL); dm_read(kalmanDmi.gyroRateRadians, (Void *) &inputs.gyroRate, sizeof(inputs.gyroRate), (DM_Time * ) NULL); } inputs.gyroFlag = TRUE; } } Timer3_Disable(); MotionPak_Input_Buffer_Free(&input_buffer); FIR_Filter_Free(&b); printf("done\n"); } /******************************************************************************/ /* Function : ds */ /* Purpose : diagnostoc display of motionpak input data */ /* Inputs : */ /* Outputs : */ /* Notes : */ /******************************************************************************/ void ds(void) { Reg int i = 0,j; Flt64 time; Flt64 dtime; MotionPak_Output filter_output; MotionPak_Input_Buffer input_buffer; FIR_Filter b; inbuff = &input_buffer; MotionPak_Input_Buffer_Initialize(&input_buffer, INPUT_BUFFER_SIZE); FIR_Filter_Initialize(&b, FIR_FILTER_LENGTH); Read_Calibration_Table(&calibration); MotionPak_Initialize(); Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Sample_Interrupt_Routine, NULL); /* iniMtialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); exit(0); } /* sample, filter, and buffer data */ Timer3_Enable(); for (;;){ semTake(Sem, WAIT_FOREVER); FIR_Filter_Compute(&input_buffer, &b, &filter_output); if (!(++j%20)){ printf("%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %9.2f\n", filter_output.time, filter_output.ref, filter_output.ax, filter_output.ay, filter_output.az, filter_output.rx, filter_output.ry, filter_output.rz, filter_output.temp ); } } Timer3_Disable(); MotionPak_Input_Buffer_Free(&input_buffer); FIR_Filter_Free(&b); printf("done\n"); } /******************************************************************************/ /* Function : Sample_Test */ /* Purpose : Read and display 8 channel of the ADC */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void Sample_Test(void) { Reg int j; Reg Nat16 data; IP16ADC_Input sample; Word mode = SAMPLE_MODE; MotionPak_Output output; Timer3_Disable(); Read_Calibration_Table(&calibration); MotionPak_Initialize(); sample.Channel_2 = 0; sample.Channel_4 = 0; sample.Channel_6 = 0; sample.Channel_8 = 0; sample.Channel_10 = 0; sample.Channel_12 = 0; sample.Channel_14 = 0; sample.Channel_16 = 0; for (;;){ Sample_Input(&sample, mode); Linear_Correction(&sample, &calibration); MotionPak_Correction(&sample, &output); if (!(++j%2000)){ printf("%9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f\n", output.ref, output.ax, output.ay, output.az, output.rx, output.ry, output.rz, output.temp ); } } } /******************************************************************************/ /* Function : Calibrate_External_Input */ /* Purpose : Collect calibration data points */ /* Inputs : */ /* Outputs : */ /* Notes : This routine sequences through all eight channels and tell the */ /* : user what voltage to apply. */ /******************************************************************************/ void Calibrate_External_Input(void) { Flt32 p = 0, q; Nat32 i, j; #ifdef RELEASED char* filename = "/usr/vw5.1.1/mbari/config/kalman/calibration_table"; #else char* filename = "/usr/vw5.1.1/mbari/kalman/calibration_table"; #endif FILE* fp; char string[150]; MotionPak_Calibration calibration; MotionPak_Output output; MotionPak_Input_Buffer input_buffer; inbuff = &input_buffer; MotionPak_Input_Buffer_Initialize(&input_buffer, CALIBRATION_LENGTH); printf("opening file %s\n", filename); if ( (fp = fopen(filename, "w")) == NULL ){ printf("error opening file - %s\n", filename); exit(1); } MotionPak_Initialize(); Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Calibration_Interrupt_Routine, NULL); /* initialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); exit(0); } /* sample, filter, and buffer data */ Timer3_Enable(); printf("Channel 2 : \n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.ref[j]; } calibration.Calibration_Table_Channel_2[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_2[i]); } printf("\nChannel 4 : violet\n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.ax[j]; } calibration.Calibration_Table_Channel_4[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_4[i]); } printf("\nChannel 6 : blue\n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.ay[j]; } calibration.Calibration_Table_Channel_6[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_6[i]); } printf("\nChannel 8 : white\n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.az[j]; } calibration.Calibration_Table_Channel_8[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_8[i]); } printf("\nChannel 10 : orange\n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.rx[j]; } calibration.Calibration_Table_Channel_10[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_10[i]); } printf("\nChannel 12 : brown\n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.ry[j]; } calibration.Calibration_Table_Channel_12[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_12[i]); } printf("\nChannel 14 : green\n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.rz[j]; } calibration.Calibration_Table_Channel_14[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_14[i]); } printf("\nChannel 16 : \n"); for (i = 0; i < NCAL; i++){ printf("%dV input ", i-(NCAL-1)/2); if (getchar() == 'a'){ getchar(); p = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ p += input_buffer.temp[j]; } calibration.Calibration_Table_Channel_16[i] = p / CALIBRATION_LENGTH; } printf("%f\n", calibration.Calibration_Table_Channel_16[i]); } for (i = 0; i < NCAL; i++){ sprintf(string, "%5.2f %11.6f %11.6f %11.6f %11.6f %11.6f %11.6f %11.6f %11.6f\n", i-(NCAL-1)/2.0, calibration.Calibration_Table_Channel_2[i], calibration.Calibration_Table_Channel_4[i], calibration.Calibration_Table_Channel_6[i], calibration.Calibration_Table_Channel_8[i], calibration.Calibration_Table_Channel_10[i], calibration.Calibration_Table_Channel_12[i], calibration.Calibration_Table_Channel_14[i], calibration.Calibration_Table_Channel_16[i] ); fwrite((char*) &string, strlen(string), 1, fp); } fclose(fp); Timer3_Disable(); MotionPak_Input_Buffer_Free(&input_buffer); printf("done\n"); } /******************************************************************************/ /* Function : Calibrate_Parallel_Input */ /* Purpose : Collect calibration data points */ /* Inputs : */ /* Outputs : */ /* Notes : This routine collects data on all channels simultaneously */ /* : assuming that the same voltage is applied to all. */ /******************************************************************************/ void Calibrate_Parallel_Input(void) { Flt32 p = 0, q, s1, s2; Flt32 ref, ax, ay, az, rx, ry, rz, temp; Int16 s; Nat32 i, j; char* filename = "/usr/vw5.1.1/mbari_development/kalman/calibration_table"; FILE* fp; char string[150]; MotionPak_Calibration calibration; MotionPak_Output output; MotionPak_Input_Buffer input_buffer; inbuff = &input_buffer; MotionPak_Input_Buffer_Initialize(&input_buffer, CALIBRATION_LENGTH); printf("opening file %s\n", filename); if ( (fp = fopen(filename, "w")) == NULL ){ printf("error opening file - %s\n", filename); exit(1); } MotionPak_Initialize(); Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Calibration_Interrupt_Routine, NULL); /* initialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); exit(0); } /* sample, filter, and buffer data */ Timer3_Enable(); printf("Channels 2,4,6,8,10,12,14,16 : \n"); for (i = 0; i < NCAL; i++){ printf("%6.2fV input ", i*VCAL-5.0); if (getchar() == 'a'){ getchar(); Timer3_Enable(); semTake(Sem, WAIT_FOREVER); semTake(Sem, WAIT_FOREVER); Timer3_Disable(); ref = 0; ax = 0; ay = 0; az = 0; rx = 0; ry = 0; rz = 0; temp = 0; for (j = 0; j < CALIBRATION_LENGTH; j++){ ref += input_buffer.ref[j]; ax += input_buffer.ax[j]; ay += input_buffer.ay[j]; az += input_buffer.az[j]; rx += input_buffer.rx[j]; ry += input_buffer.ry[j]; rz += input_buffer.rz[j]; temp += input_buffer.temp[j]; } ref /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_2[i] = ref; ax /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_4[i] = ax; ay /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_6[i] = ay; az /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_8[i] = az; rx /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_10[i] = rx; ry /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_12[i] = ry; rz /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_14[i] = rz; temp /= CALIBRATION_LENGTH; calibration.Calibration_Table_Channel_16[i] = temp; } printf("%f ", calibration.Calibration_Table_Channel_2[i]); printf("%f ", calibration.Calibration_Table_Channel_4[i]); printf("%f ", calibration.Calibration_Table_Channel_6[i]); printf("%f ", calibration.Calibration_Table_Channel_8[i]); printf("%f ", calibration.Calibration_Table_Channel_10[i]); printf("%f ", calibration.Calibration_Table_Channel_12[i]); printf("%f ", calibration.Calibration_Table_Channel_14[i]); printf("%f\n", calibration.Calibration_Table_Channel_16[i]); } for (i = 0; i < NCAL; i++){ sprintf(string, "%5.2f %11.6f %11.6f %11.6f %11.6f %11.6f %11.6f %11.6f %11.6f\n", i-(NCAL-1)/2.0, calibration.Calibration_Table_Channel_2[i], calibration.Calibration_Table_Channel_4[i], calibration.Calibration_Table_Channel_6[i], calibration.Calibration_Table_Channel_8[i], calibration.Calibration_Table_Channel_10[i], calibration.Calibration_Table_Channel_12[i], calibration.Calibration_Table_Channel_14[i], calibration.Calibration_Table_Channel_16[i] ); fwrite((char*) &string, strlen(string), 1, fp); } fclose(fp); Timer3_Disable(); MotionPak_Input_Buffer_Free(&input_buffer); printf("done\n"); } /******************************************************************************/ /* Function : Calibrate_Interrupt_Routine */ /* Purpose : Samples MotionPak as usual rate for calibration data */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ int Calibration_Interrupt_Routine(int p) { #ifdef FAIL_LED_BLINKING static int i = 0; #endif static int k = 0; static int j = 0; static MBool state = FALSE; Flt64 time, dtime; IP16ADC_Input sample; MotionPak_Output output; FP_CONTEXT fp_context; int level; level = intLock(); fppSave(&fp_context); Get_Time(&time, &dtime); sample.time = time; Sample_Input(&sample, SAMPLE_MODE); output.time = sample.time; output.ref = sample.Channel_2; output.ax = sample.Channel_4; output.ay = sample.Channel_6; output.az = sample.Channel_8; output.rx = sample.Channel_10; output.ry = sample.Channel_12; output.rz = sample.Channel_14; output.temp = sample.Channel_16; MotionPak_Input_Buffer_Update(&output, inbuff); if (++k == CALIBRATION_LENGTH){ k = 0; semGive(Sem); } #ifdef FAIL_LED_BLINKING if (!(++i % 25)){ if (i == 50){ *MCC_RESET_CR &= ~(RESET_CR_BDFLO); i = 0; } else *MCC_RESET_CR = RESET_CR_BDFLO; } #endif fppRestore(&fp_context); intUnlock(level); return 0; } /******************************************************************************/ /* Function : MotionPak_Data_Logger */ /* Purpose : Samples MotionPak as usual rate for calibration data */ /* Inputs : */ /* Outputs : */ /* Bytes Data */ /* ----- ---- */ /* 8 start time (Flt64) */ /* 2 number of buffers (Int16) */ /* 2 buffer size (Int16) */ /* BUFFER_SIZE * 4 time (Flt32) */ /* BUFFER_SIZE * 4 ax (Flt32) */ /* BUFFER_SIZE * 4 ay (Flt32) */ /* BUFFER_SIZE * 4 az (Flt32) */ /* BUFFER_SIZE * 4 rx (Flt32) */ /* BUFFER_SIZE * 4 ry (Flt32) */ /* BUFFER_SIZE * 4 rz (Flt32) */ /* BUFFER_SIZE * 4 temp (Flt32) */ /******************************************************************************/ STATUS MotionPak_Data_Logger(void) { char* filename = "/users/matthews/matlab/mp_data"; char string[100]; FILE* fp; Reg int i,j; int tid; Int16 k; Flt64 time; Flt64 dtime; Int16 statusKalman = START; Errno status; kalmanDMItems kalmanDmi; MotionPak_Output filter_output; MotionPak_Output output; MotionPak_Input_Buffer input_buffer; MotionPak_Output_Buffer* output_buffer; MotionPak_Output_Buffer output_buffer1; MotionPak_Output_Buffer output_buffer2; FIR_Filter b; inbuff = &input_buffer; MotionPak_Output_Buffer_Initialize(&output_buffer1, OUTPUT_BUFFER_SIZE); MotionPak_Output_Buffer_Initialize(&output_buffer2, OUTPUT_BUFFER_SIZE); MotionPak_Input_Buffer_Initialize(&input_buffer, INPUT_BUFFER_SIZE); FIR_Filter_Initialize(&b, FIR_FILTER_LENGTH); taskPrioritySet(taskIdSelf(), 60); printf("\n\nMotionPak Data Collection\n"); printf("collection time - %d minutes\n", EXPERIMENT_TIME); /* create status bit data manager item */ status = dm_create(KALMAN_FILTER_STOP_DM, 1, &kalmanDmi.Status, DM_MBOOL, 1, DM_ENDT); if (checkCreate(status, KALMAN_FILTER_STOP_DM, TRUE) == ERROR) return(ERROR); status = dm_start_multiple(kalmanDmi.Status); dm_write(kalmanDmi.Status, (Void *) &statusKalman, sizeof(statusKalman), (DM_Time*) NULL); status = dm_start_consumer(kalmanDmi.Status, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Kalman Filter - status", TRUE) == ERROR) return(ERROR); printf("opening data file %s\n", filename); if ( (fp = fopen(filename, "w")) == NULL ){ printf("error opening file - %s\n", filename); exit(1); } Read_Calibration_Table(&calibration); MotionPak_Initialize(); /* first 8 bytes are start time */ time = Get_Absolute_Time(); printf("start time - %f\n", time); fwrite((char*) &time, sizeof(Flt64), 1, fp); /* next 2 bytes are number of buffers */ k = NUMBER_OF_BUFFERS; fwrite((char*) &k, sizeof(Int16), 1, fp); /* next 2 bytes are buffer size */ fwrite((char*) &(output_buffer1.size), sizeof(Int16), 1, fp); output_buffer1.state = TRUE; Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Sample_Interrupt_Routine, NULL); /* initialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); exit(1); } /* sample, filter, and buffer data */ Timer3_Enable(); for (i = 0; i < NUMBER_OF_BUFFERS; i++){ for (j = 0; j < OUTPUT_BUFFER_SIZE; j++){ semTake(Sem, WAIT_FOREVER); FIR_Filter_Compute(&input_buffer, &b, &filter_output); /* write out sample to current buffer */ if (output_buffer1.state == TRUE && output_buffer2.state == FALSE) MotionPak_Output_Buffer_Update(&output_buffer1, &filter_output); if (output_buffer1.state == FALSE && output_buffer2.state == TRUE) MotionPak_Output_Buffer_Update(&output_buffer2, &filter_output); } /* write out current buffer and enable alternate buffer */ if (output_buffer1.state == TRUE){ output_buffer1.state = FALSE; output_buffer2.state = TRUE; output_buffer = &output_buffer1; } else{ output_buffer1.state = TRUE; output_buffer2.state = FALSE; output_buffer = &output_buffer2; } /* spawn task to write buffer */ if ( (tid = taskSpawn( "MotionPak_Write_Buffer", 62, VX_DEALLOC_STACK | VX_FP_TASK, 20000, (FUNCPTR) MotionPak_Write_Buffer, (int) output_buffer, (int) fp, 0, 0, 0, 0, 0, 0, 0, 0 )) == ERROR){ logMsg("Error spawing task MotionPak_Write_Buffer"); Timer3_Disable(); fclose(fp); exit(0); } /* read status bit */ dm_read(kalmanDmi.Status, (Void *) &statusKalman, sizeof(statusKalman), (DM_Time * ) NULL); if (statusKalman) break; } printf("\n"); Timer3_Disable(); fclose(fp); taskDelete(tid); MotionPak_Input_Buffer_Free(&input_buffer); MotionPak_Output_Buffer_Free(&output_buffer1); MotionPak_Output_Buffer_Free(&output_buffer2); FIR_Filter_Free(&b); printf("MotionPak_Data_Logger done\n"); exit(OK); } /******************************************************************************/ /* Function : MotionPak_Data_Logger_DM */ /* Purpose : Samples MotionPak as usual rate for calibration data */ /* Inputs : */ /* Outputs : */ /* Bytes Data */ /* ----- ---- */ /* 8 start time (Flt64) */ /* 2 number of buffers (Int16) */ /* 2 buffer size (Int16) */ /* BUFFER_SIZE * 4 time (Flt32) */ /* BUFFER_SIZE * 4 compass (Flt32) (was Vref) */ /* BUFFER_SIZE * 4 ax (Flt32) */ /* BUFFER_SIZE * 4 ay (Flt32) */ /* BUFFER_SIZE * 4 az (Flt32) */ /* BUFFER_SIZE * 4 rx (Flt32) */ /* BUFFER_SIZE * 4 ry (Flt32) */ /* BUFFER_SIZE * 4 rz (Flt32) */ /* BUFFER_SIZE * 4 gyro (Flt32) (was temp) */ /******************************************************************************/ STATUS MotionPak_Data_Logger_DM(void) { /* char* filename = "/users/matthews/matlab/data/mp_data"; */ char* filename = "/usr/vw5.1.1/mbari/kalman/data/mp_data"; char string[100]; FILE* fp; Reg int i,j,m=0; Int16 k; int tid; Flt64 time; Flt64 dtime; kalmanDMItems kalmanDmi; /* data manager items */ SEM_ID kalmanSem; /* task wakeup semaphore */ DM_Group sensorGroup; /* data manager group */ DWord sensorGroupBits; /* sensor group changes */ sensorBits sensorId; /* sensor id bits */ MBool compassDataValid; /* sensor data valid flags */ MBool gyroDataValid; Int16 statusKalman = STOP_LOGGING; MBool init = TRUE; /* initialization flag */ MBool loggerStatus = FALSE; /* start with logger off */ DM_Time sampleTime; /* update time */ Measurements inputs = {FALSE, FALSE, 0, 0, 0}; /* filter input vector */ MotionPak_Buffers MP_buffers; /* I/O buffer structure */ SEM_ID writeBufSem; /* task wakeup semaphore */ taskPrioritySet(taskIdSelf(), 70); MotionPak_Output_Buffer_Initialize(&MP_buffers.output_buffer1, OUTPUT_BUFFER_SIZE); MotionPak_Output_Buffer_Initialize(&MP_buffers.output_buffer2, OUTPUT_BUFFER_SIZE); MotionPak_Input_Buffer_Initialize(&MP_buffers.input_buffer, INPUT_BUFFER_SIZE); FIR_Filter_Initialize(&MP_buffers.filter, FIR_FILTER_LENGTH); /* initialize buffer pointers structure for interrupt */ MP_buffers.status = FALSE; MP_buffers.state = FALSE; MP_buffers.count = 0; MP_buffers.output_buffer = &MP_buffers.output_buffer1; MP_buffers.write_buffer = &MP_buffers.output_buffer2; printf("\n\nMotionPak Data Collection DM\n"); printf("collection time - %d minutes\n", EXPERIMENT_TIME); Read_Calibration_Table(&calibration); MotionPak_Initialize(); /* initialize data manager items */ printf("Init_Kalman_Filter\n"); if (Init_Kalman_Filter(&kalmanDmi, &sensorGroup, &sensorId) == ERROR){ printf("Error in Init_Kalman_Filter\n"); return(ERROR); } /* open output file */ printf("opening output data file %s\n", filename); if ( (fp = fopen(filename, "w")) == NULL ){ printf("error opening output file - %s\n", filename); exit(1); } /* first 8 bytes are start time */ time = Get_Absolute_Time(); printf("start time - %f\n", time); fwrite((char*) &time, sizeof(Flt64), 1, fp); /* next 2 bytes are number of buffers */ k = NUMBER_OF_BUFFERS; fwrite((char*) &k, sizeof(Int16), 1, fp); /* next 2 bytes are buffer size */ fwrite((char*) &MP_buffers.output_buffer1.size, sizeof(Int16), 1, fp); Timer3_Init(SAMPLE_PERIOD); Timer3_Connect(Logger_Sample_Interrupt_Routine, (int) &MP_buffers); /* initialize task wakeup semaphore */ if ( (Sem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); exit(1); } if ( (writeBufSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ){ logMsg("Filter FAILURE : could not initialize semaphore\n"); exit(1); } if ( (tid = taskSpawn("MotionPak_Write_Buffer", 71, VX_DEALLOC_STACK | VX_FP_TASK, 20000, (FUNCPTR) Logger_MotionPak_Write_Buffer, (int) &MP_buffers, (int) fp, (int) writeBufSem, 0, 0, 0, 0, 0, 0, 0 )) == ERROR){ logMsg("Error spawing task MotionPak_Write_Buffer"); } /* sample, filter, and buffer data */ Timer3_Enable(); FOREVER{ /* wait for semaphore */ semTake(Sem, WAIT_FOREVER); /* read status bit */ dm_read(kalmanDmi.Status, (Void *) &statusKalman, sizeof(statusKalman), (DM_Time * ) NULL); if (statusKalman == STOP) break; if ((statusKalman == STOP_LOGGING) && (loggerStatus == TRUE)){ loggerStatus = FALSE; printf("stop data logging\n"); } if ((statusKalman == START_LOGGING) && (loggerStatus == FALSE)){ loggerStatus = TRUE; printf("start data logging\n"); } MP_buffers.status = loggerStatus; /* check for sensor value change */ sensorGroupBits = dm_get_group_changes(sensorGroup); /* update compass heading */ if (sensorGroupBits & sensorId.compassBit){ dm_read(kalmanDmi.compassDataValid, (Void *) &compassDataValid, sizeof(compassDataValid), (DM_Time * ) NULL); if (compassDataValid){ dm_read(kalmanDmi.compassRadians, (Void *) &inputs.compass, sizeof(inputs.compass), (DM_Time * ) NULL); } } /* update gyro heading and heading rate */ if (sensorGroupBits & sensorId.gyroBit){ dm_read(kalmanDmi.gyroDataValid, (Void *) &gyroDataValid, sizeof(gyroDataValid), (DM_Time * ) NULL); if (gyroDataValid){ dm_read(kalmanDmi.gyroRadians, (Void *) &inputs.gyro, sizeof(inputs.gyro), (DM_Time * ) NULL); dm_read(kalmanDmi.gyroRateRadians, (Void *) &inputs.gyroRate, sizeof(inputs.gyroRate), (DM_Time * ) NULL); } } gettimeofday(&sampleTime, (struct timezone *) NULL); /* output gyro heading and MP rate */ dm_write(kalmanDmi.Heading, (Void *) &inputs.gyro, sizeof(inputs.gyro), &sampleTime); dm_write(kalmanDmi.Heading_Rate, (Void *) &MP_buffers.output.rz, sizeof(MP_buffers.output.rz), &sampleTime); dm_write(kalmanDmi.Pitch_Rate, (Void *) &MP_buffers.output.ry, sizeof(MP_buffers.output.ry), &sampleTime); dm_write(kalmanDmi.Roll_Rate, (Void *) &MP_buffers.output.rx, sizeof(MP_buffers.output.rx), &sampleTime); dm_write(kalmanDmi.Acceleration_X, (Void *) &MP_buffers.output.ax, sizeof(MP_buffers.output.ax), &sampleTime); dm_write(kalmanDmi.Acceleration_Y, (Void *) &MP_buffers.output.ay, sizeof(MP_buffers.output.ay), &sampleTime); dm_write(kalmanDmi.Acceleration_Z, (Void *) &MP_buffers.output.az, sizeof(MP_buffers.output.az), &sampleTime); MP_buffers.compass = inputs.compass; MP_buffers.gyro = inputs.gyro; if (MP_buffers.state){ MP_buffers.state = FALSE; if (loggerStatus) semGive(writeBufSem); /* Wake up logging task */ } if (statusKalman == STOP) break; } printf("\n"); Timer3_Disable(); fclose(fp); taskDelete(tid); MotionPak_Input_Buffer_Free(&MP_buffers.input_buffer); MotionPak_Output_Buffer_Free(&MP_buffers.output_buffer1); MotionPak_Output_Buffer_Free(&MP_buffers.output_buffer2); FIR_Filter_Free(&MP_buffers.filter); printf("MotionPak_Data_Logger_DM done\n"); return(OK); } /******************************************************************************/ /* Function : Kalman_Filter_Status */ /* Purpose : Status data logger */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ STATUS Kalman_Filter_Status(Int16 statusKalman) { DM_Item statusKalmanDmi; Int16 status; taskPrioritySet(taskIdSelf(), 20); /* lookup data manager item handle */ statusKalmanDmi = dm_lookup(KALMAN_FILTER_STOP_DM, 0); if (checkLookup(statusKalmanDmi, KALMAN_FILTER_STOP_DM) == ERROR) return(ERROR); /* start provider and write value */ status = dm_start_multiple(statusKalmanDmi); status = dm_write(statusKalmanDmi, (Void *) &statusKalman, sizeof(statusKalman), (DM_Time * ) NULL); if (checkWrite(status, "Kalman Filter Status - Kalman Filter status flag") == ERROR) return(ERROR); return(OK); } /******************************************************************************/ /* Function : MotionPak_Output_Buffer_Initialize */ /* Purpose : Creates an output buffer for MotionPak data for real-time file */ /* logging */ /* Inputs : Output buffer pointer p and size n of buffer */ /* Outputs : */ /******************************************************************************/ STATUS MotionPak_Output_Buffer_Initialize(MotionPak_Output_Buffer* p, int n) { int i; p->status = BUFFER_READY; p->state = BUFFER_OK; p->index = -1; p->size = n; p->name = NULL; if ( !(p->time = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ref = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ax = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ay = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->az = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->rx = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->ry = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->rz = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); if ( !(p->temp = (Flt32*) malloc((unsigned) n * sizeof(Flt32))) ) return(ERROR); for (i = 0; i < n; i++){ p->time[i] = 0.0; p->ref[i] = 0.0; p->ax[i] = 0.0; p->ay[i] = 0.0; p->az[i] = 0.0; p->rx[i] = 0.0; p->ry[i] = 0.0; p->rz[i] = 0.0; p->temp[i] = 0.0; } return(OK); } /******************************************************************************/ /* Function : MotionPak_Output_Buffer_Update */ /* Purpose : Updates the output buffer with new data in a modulo n schedule */ /* Inputs : Output buffer pointer p and a pointer y to the output struct */ /* Outputs : */ /******************************************************************************/ STATUS MotionPak_Output_Buffer_Update(MotionPak_Output_Buffer* p, MotionPak_Output* y) { Reg Int16 k; if (p->status == BUFFER_BUSY) return BUFFER_BUSY; p->index++; if ( p->index == (p->size - 1) ) p->state = BUFFER_FULL; else p->state = BUFFER_OK; p->index = p->index % p->size; k = p->index; p->time[k] = (Flt32) y->time; p->ref[k] = y->ref; p->ax[k] = y->ax; p->ay[k] = y->ay; p->az[k] = y->az; p->rx[k] = y->rx; p->ry[k] = y->ry; p->rz[k] = y->rz; p->temp[k] = y->temp; return OK; } /******************************************************************************/ /* Function : MotionPak_Output_Buffer_Free */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ void MotionPak_Output_Buffer_Free(MotionPak_Output_Buffer* p) { free(p->time); free(p->ref); free(p->ax); free(p->ay); free(p->az); free(p->rx); free(p->ry); free(p->rz); free(p->temp); } /******************************************************************************/ /* Function : MotionPak_Write_Buffer */ /* Purpose : Writes the output buffer to a file */ /* Inputs : Output buffer pointer p and file pointer fp */ /* Outputs : */ /******************************************************************************/ int MotionPak_Write_Buffer(MotionPak_Output_Buffer* p, FILE* fp) { Reg int i, n = 0; static Int16 count = 0; if (p->status == BUFFER_BUSY) return 0; printf("MotionPak_Write_Buffer - buffer %d", ++count); p->status = BUFFER_BUSY; n += fwrite((char*) p->time, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->ref, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->ax, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->ay, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->az, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->rx, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->ry, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->rz, sizeof(Flt32), p->size, fp); n += fwrite((char*) p->temp, sizeof(Flt32), p->size, fp); fflush( fp ); ioctl( fileno(fp), FIOSYNC, 0 ); printf(" - done\n"); p->status = BUFFER_READY; return n; } /******************************************************************************/ /* Function : Logger_MotionPak_Write_Buffer */ /* Purpose : Writes the output buffer to a file */ /* Inputs : Output buffer pointer p and file pointer fp */ /* Outputs : */ /******************************************************************************/ int Logger_MotionPak_Write_Buffer(MotionPak_Buffers* p, FILE* fp, SEM_ID writeBufSem) { Reg int i, n = 0; static Int16 count = 0; FOREVER { semTake(writeBufSem, WAIT_FOREVER); printf("Logger_MotionPak_Write_Buffer - buffer %d", ++count); /* for (i = 0; i < 20; i++) printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", p->write_buffer->time[i], p->write_buffer->ref[i], p->write_buffer->ax[i], p->write_buffer->ay[i], p->write_buffer->az[i], p->write_buffer->rx[i], p->write_buffer->ry[i], p->write_buffer->rz[i], p->write_buffer->temp[i] ); */ n += fwrite((char*) p->write_buffer->time, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->ref, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->ax, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->ay, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->az, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->rx, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->ry, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->rz, sizeof(Flt32), p->write_buffer->size, fp); n += fwrite((char*) p->write_buffer->temp, sizeof(Flt32), p->write_buffer->size, fp); fflush(fp); ioctl(fileno(fp), FIOSYNC, 0); printf(" - done\n"); } } /******************************************************************************/ /* Function : MotionPak_Write_Output */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ int MotionPak_Write_Output(MotionPak_Output* p, FILE* fp) { fwrite((char*) &(p->time), sizeof(Flt32), 1, fp); fwrite((char*) &(p->ref), sizeof(Flt32), 1, fp); fwrite((char*) &(p->ax), sizeof(Flt32), 1, fp); fwrite((char*) &(p->ay), sizeof(Flt32), 1, fp); fwrite((char*) &(p->az), sizeof(Flt32), 1, fp); fwrite((char*) &(p->rx), sizeof(Flt32), 1, fp); fwrite((char*) &(p->ry), sizeof(Flt32), 1, fp); fwrite((char*) &(p->rz), sizeof(Flt32), 1, fp); fwrite((char*) &(p->temp), sizeof(Flt32), 1, fp); } /******************************************************************************/ /* Function : MotionPak_Read_Buffer */ /* Purpose : */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ int MotionPak_Read_Buffer(MotionPak_Output_Buffer* p, FILE* fp) { Reg int n = 0; n += fread((char*) p->time, sizeof(Flt32), p->size, fp); n += fread((char*) p->ref, sizeof(Flt32), p->size, fp); n += fread((char*) p->ax, sizeof(Flt32), p->size, fp); n += fread((char*) p->ay, sizeof(Flt32), p->size, fp); n += fread((char*) p->az, sizeof(Flt32), p->size, fp); n += fread((char*) p->rx, sizeof(Flt32), p->size, fp); n += fread((char*) p->ry, sizeof(Flt32), p->size, fp); n += fread((char*) p->rz, sizeof(Flt32), p->size, fp); n += fread((char*) p->temp, sizeof(Flt32), p->size, fp); return n; } /******************************************************************************/ /* Function : File_Convert_Binary_to_ASCII */ /* Purpose : Converts a binary file in the MotionPak_Write_Buffer format to */ /* an ASCII file with a 10.6f floating point format */ /* Inputs : Input filename file1, begin buffer number, end buffer number */ /* Outputs : */ /******************************************************************************/ void File_Convert_Binary_to_ASCII(char* file1, int begin, int end) { char* matlab_prefix = "/users/matthews/matlab/motionpak/data/"; char* matlab_suffix = ".m"; char string[250]; char file2[250]; Int16 i, j, n; FILE* fp1; FILE* fp2; MotionPak_Output_Buffer p; Flt64 time; printf("\n\nConverting binary data file to ASCII file\n"); strcpy(file2, matlab_prefix); strcat(file2, file1); printf("opening binary input file %s\n", file2); if ( (fp1 = fopen(file2, "r")) == NULL ){ printf("error opening file %s\n", file2); exit(0); } strcat(file2, matlab_suffix); printf("opening ASCII output file %s\n", file2); if ( (fp2 = fopen(file2, "w")) == NULL ){ printf("error opening file %s\n", file2); exit(0); } fread((char*) &time, sizeof(Flt64), 1, fp1); printf("start time - %f\n", time); fread((char*) &n, sizeof(Int16), 1, fp1); printf("number of buffers - %d\n", n); fread((char*) &n, sizeof(Int16), 1, fp1); printf("buffer size - %d\n", n); printf("reading from buffer %d to buffer %d\n", begin, end); MotionPak_Output_Buffer_Initialize(&p, n); sprintf(string, "x=[\n"); fwrite((char*) &string, strlen(string), 1, fp2); for (j = 0; j < begin; j++) n = MotionPak_Read_Buffer(&p, fp1); for (i = 0; i <= end; i++) printf(" "); printf("]\r["); for (j = 0; j < end; j++){ n = MotionPak_Read_Buffer(&p, fp1); for (i = 0; i < p.size; i++){ sprintf(string,"%e %e %e %e %e %e %e %e %e\n", p.time[i], p.ref[i], p.ax[i], p.ay[i], p.az[i], p.rx[i], p.ry[i], p.rz[i], p.temp[i]); fwrite((char*) &string, strlen(string), 1, fp2); } printf("."); } printf("\n"); sprintf(string, "];"); fwrite((char*) &string, strlen(string), 1, fp2); fclose(fp1); fclose(fp2); MotionPak_Output_Buffer_Free(&p); printf("done\n"); exit(0); } /******************************************************************************/ /* Function : Logger_Sample_Interrupt_Routine */ /* Purpose : Timer3 interrupt service routine for logger sampling */ /* Inputs : */ /* Outputs : */ /******************************************************************************/ int Logger_Sample_Interrupt_Routine(int p) { static Int32 k = 0; static Int32 j = 0; Int16 i; static MBool state = FALSE; static Flt64 btime = 0; Flt64 time, dtime; IP16ADC_Input sample; MotionPak_Output output; MotionPak_Buffers* bp = (MotionPak_Buffers*) p; FP_CONTEXT fp_context; int level; level = intLock(); fppSave(&fp_context); Get_Time(&time, &dtime); sample.time = time; Sample_Input(&sample, SAMPLE_MODE); Linear_Correction(&sample, &calibration); MotionPak_Correction(&sample, &output); MotionPak_Input_Buffer_Update(&output, &bp->input_buffer); if (!(++k % DECIMATION)){ semGive(Sem); FIR_Filter_Compute(&bp->input_buffer, &bp->filter, &bp->output); #ifdef IP16ADC_INSTALLED /* bp->output.ref = bp->compass; bp->output.temp = bp->gyro; */ bp->output.rx *= 3.1415926/180; bp->output.ry *= 3.1415926/180; bp->output.rz *= 3.1415926/180; #else bp->output.ref = k; bp->output.ax = k; bp->output.ay = k; bp->output.az = k; bp->output.rx = k; bp->output.ry = k; bp->output.rz = k; bp->output.temp = k; #endif if (bp->status){ MotionPak_Output_Buffer_Update(bp->output_buffer, &bp->output); if (bp->output_buffer->state == BUFFER_FULL){ bp->output_buffer->state = BUFFER_OK; bp->state = TRUE; bp->temp_buffer = bp->write_buffer; bp->write_buffer = bp->output_buffer; bp->output_buffer = bp->temp_buffer; } } } fppRestore(&fp_context); intUnlock(level); return 0; }