/*****************************************************************************/ /* Copyright 1993 MBARI */ /*****************************************************************************/ /* Summary : Sensor Filter Module for Tiburon ROV Control System */ /* Filename : sensor.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 05/24/93 */ /* Modified : 10/11/96 */ /* Archived : */ /*****************************************************************************/ /* Modification History : */ /* $Header: sensor.c,v 1.1 97/12/04 15:27:41 oreilly Exp $ * $Log: sensor.c,v $ * Revision 1.1 97/12/04 15:27:41 15:27:41 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /*****************************************************************************/ #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 /* MBARI style guide type declarations */ #include /* MBARI time declarations */ #include /* data manager declarations */ #include /* data manager error declarations */ #include /* vehicle turns definitions */ #include /* motion pak definitions */ #include /* kalman filter definitions */ #include "control.h" /* control system definitions */ #include "filterFP.h" /* floating point filter definitions */ #include "trig.h" #include "turnsUtils.h" #define SENSOR_PERIOD 50000 /* sensor period (microsec) */ #define TRANS_DOF 3 /* number of translation dof */ #define SHARPS_X_OFFSET 1.219 /* offset of SHARPS txdcr */ #define SHARPS_Y_OFFSET 0.292 /* from rov cg */ #define SHARPS_Z_OFFSET -0.787 #define BUTTER_ORDER2 2 /* Butterworth filter order */ #define BUTTER_ORDER4 4 /* Butterworth filter order */ #define BUTTER_ORDER8 8 /* Butterworth filter order */ /* sensors group id bits */ typedef struct { DWord sharpsBit; /* sharps bit */ DWord depthBit; /* sensor depth bit */ DWord altitudeBit; /* sensor altitude bit */ DWord kalmanAttitudeBit; /* Kalman filter attitude bit*/ DWord shipHeadingBit; } sensorFilterBits; /* translation dof */ typedef enum {X, Y, Z } transDof; /* function prototypes */ #ifdef __STDC__ Errno initSensorFilter(controlDMItems *controlDmi, SEM_ID sensorSem, DM_Group *sensorGroup, sensorFilterBits *sensorId); Void diffiltDepth(DM_Item dmiRawDepth, DM_Item dmiDepth, DM_Item dmiDepthRate); Void diffiltAltitude(DM_Item dmiRawAltitude, DM_Item dmiAltitude, DM_Item dmiAltitudeRate); Flt64 butterFilt1(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]); Flt64 butterFilt2(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]); Flt64 butterFilt3(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]); Flt64 butterFilt4(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]); Flt64 butterFilt5(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]); Flt64 butterFilt6(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]); Int16 turnsCounter(Flt32 heading, Flt32 shipHeading, MBool resetTurns, controlDMItems *controlDmi, Flt32 *hSum, Flt32 *turnsSetpoint); #endif #define dprint if (debug) logMsg /*****************************************************************************/ /* Function : sensorFilter */ /* Purpose : Gets the sensor measurements. */ /* Inputs : Control data manager items stucture pointer. */ /* Outputs : None. */ /*****************************************************************************/ Void sensorFilter(controlDMItems *controlDmi) { extern FILE *fpw; SEM_ID sensorSem; /* task wakeup semaphore */ DM_Group sensorGroup; /* data manager group */ DWord sensorGroupBits; /* sensor group changes */ sensorFilterBits sensorId; /* sensor id bits */ MBool sharpsDataValid; /* sensor data valid flags */ MBool headingValid; MBool headingRateValid; MBool depthDataValid = TRUE; /* dummy flag */ MBool altimeterDataValid; MBool positionValid = FALSE; /* auto control valid flags */ MBool rateValid = FALSE; MBool depthValid = FALSE; MBool altitudeValid = FALSE; MBool kalmanHeadingDataValid; /* Kalman heading valid flag */ MBool prevKalmanHeadingDataValid;/* prev Kalman heading valid */ MBool resetTurns = FALSE; /* reset turns counter flag */ MBool first = TRUE; /* first valid heading flag */ Int16 turnsCount = 0; /* rov heading turns count */ Int16 prevTurnsCount = 0; /* rov heading turns count */ Flt32 turnsSum = 0.0; /* rov heading turns sum */ Flt32 turnsSetpoint = 0.0; Flt32 sharpsPosition[TRANS_DOF] =/* SHARPS positions */ { 0.0, 0.0, 0.0 }; Flt32 sharpsRate[TRANS_DOF] = /* SHARPS rates */ { 0.0, 0.0, 0.0 }; Flt32 kalmanHeadingDegrees = 0.0;/* Kalman filter heading */ Flt32 heading = 0.0; /* heading */ Flt32 headingRate = 0.0; /* heading rate */ Flt32 cosH, sinH; /* heading sine, cosine */ Flt32 vehicleX, vehicleY, vehicleZ;/* rov position and rate */ Flt32 vehicleRateX, vehicleRateY;/* accounting for SHARPS */ /* transponder position */ Flt32 shipHeading; VehicleAttitude kalmanAttitude; /* Kalman filter attitude */ VehicleAttitudeStatus kalmanAttitudeStatus; /* Kalman attitude status */ DM_Time sampleTime; Errno err; static char errorBuf[100]; MBool debug = FALSE; /* initialize task wakeup semaphore */ /* dprint("semBCreate()\n"); */ if ((sensorSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\n" "could not initialize sensor filter semaphore\n"); controlShutDown("tsensorFilter"); } /* start consumers and create data manager group for sensor measurements and */ /* start providers for filtered sensor values */ /* initialize sensor values */ if (initSensorFilter(controlDmi, sensorSem, &sensorGroup, &sensorId) == ERROR) controlShutDown("tsensorFilter"); FOREVER { /* wait for change in sensor measurement values */ semTake(sensorSem, WAIT_FOREVER); /* dprint("Sensor changed\n"); */ /* determine which sensor value changed */ sensorGroupBits = dm_get_group_changes(sensorGroup); /* update positions and rates */ if (sensorGroupBits & sensorId.sharpsBit) { dm_read(controlDmi->sharpsDataValid, (Void *) &sharpsDataValid, sizeof(sharpsDataValid), (DM_Time * ) NULL); if (sharpsDataValid) { /* determine vehicle cg position and rate from SHARPS measured position and */ /* rate and SHARPS transducer offset */ if (headingValid) /* calculate vehicle position*/ { dm_read(controlDmi->sensorHeading, (Void *) &heading, sizeof(heading), (DM_Time * ) NULL); dm_read(controlDmi->sharpsX, (Void *) &sharpsPosition[X], sizeof(sharpsPosition[X]), (DM_Time * ) NULL); dm_read(controlDmi->sharpsY, (Void *) &sharpsPosition[Y], sizeof(sharpsPosition[Y]), (DM_Time * ) NULL); dm_read(controlDmi->sharpsZ, (Void *) &sharpsPosition[Z], sizeof(sharpsPosition[Z]), (DM_Time * ) NULL); cosH = cos(heading); sinH = sin(heading); vehicleX = sharpsPosition[X] - (SHARPS_X_OFFSET * cosH + SHARPS_Y_OFFSET * sinH); vehicleY = sharpsPosition[Y] - (SHARPS_Y_OFFSET * cosH - SHARPS_X_OFFSET * sinH); vehicleZ = sharpsPosition[Z] - SHARPS_Z_OFFSET; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->sensorX, (Void *) &vehicleX, sizeof(vehicleX), &sampleTime); dm_write(controlDmi->sensorY, (Void *) &vehicleY, sizeof(vehicleY), &sampleTime); dm_write(controlDmi->sensorSharpsZ, (Void *) &vehicleZ, sizeof(vehicleZ), &sampleTime); if (!positionValid) /* reset position valid TRUE */ { positionValid = TRUE; dm_write(controlDmi->positionValid, (Void *) &positionValid, sizeof(positionValid), &sampleTime); } dm_read(controlDmi->headingRateValid, (Void *) &headingRateValid, sizeof(headingRateValid), (DM_Time * ) NULL); if (headingRateValid) /* calculate vehicle rate */ { dm_read(controlDmi->sensorRateHeading, (Void *) &headingRate, sizeof(headingRate), (DM_Time * ) NULL); dm_read(controlDmi->sharpsRateX, (Void *) &sharpsRate[X], sizeof(sharpsRate[X]), (DM_Time * ) NULL); dm_read(controlDmi->sharpsRateY, (Void *) &sharpsRate[Y], sizeof(sharpsRate[Y]), (DM_Time * ) NULL); dm_read(controlDmi->sharpsRateZ, (Void *) &sharpsRate[Z], sizeof(sharpsRate[Z]), (DM_Time * ) NULL); vehicleRateX = sharpsRate[X] + headingRate * (SHARPS_X_OFFSET * sinH - SHARPS_Y_OFFSET * cosH); vehicleRateY = sharpsRate[Y] + headingRate * (SHARPS_Y_OFFSET * sinH + SHARPS_X_OFFSET * cosH); dm_write(controlDmi->sensorRateX, (Void *) &vehicleRateX, sizeof(vehicleRateX), &sampleTime); dm_write(controlDmi->sensorRateY, (Void *) &vehicleRateY, sizeof(vehicleRateY), &sampleTime); dm_write(controlDmi->sensorRateSharpsZ, (Void *) &sharpsRate[Z], sizeof(sharpsRate[Z]), &sampleTime); if (!rateValid) /* reset rate valid TRUE */ { rateValid = TRUE; dm_write(controlDmi->rateValid, (Void *)&rateValid, sizeof(rateValid), &sampleTime); } } else /* reset rate valid FALSE */ { if (rateValid) { rateValid = FALSE; gettimeofday(&sampleTime, (struct timezone *)NULL); dm_write(controlDmi->rateValid, (Void *)&rateValid, sizeof(rateValid), &sampleTime); } } } else /* reset position valid FALSE*/ { /* and reset rate valid FALSE*/ if (positionValid) { positionValid = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->positionValid, (Void *) &positionValid, sizeof(positionValid), &sampleTime); } if (rateValid) { rateValid = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->rateValid, (Void *) &rateValid, sizeof(rateValid), &sampleTime); } } } else /* sharps data not valid */ { if (positionValid) { positionValid = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->positionValid, (Void *)&positionValid, sizeof(positionValid), &sampleTime); } if (rateValid) { rateValid = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->rateValid, (Void *) &rateValid, sizeof(rateValid), &sampleTime); } } } /* update depth and depth rate */ if (sensorGroupBits & sensorId.depthBit) { /* dm_read(controlDmi->depthDataValid, (Void *) &depthDataValid, sizeof(depthDataValid), (DM_Time * ) NULL); */ if (depthDataValid) { diffiltDepth(controlDmi->depth, controlDmi->sensorDepth, controlDmi->sensorRateDepth); if (!depthValid) { depthValid = TRUE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->depthValid, (Void *) &depthValid, sizeof(depthValid), &sampleTime); } } else if (depthValid) { depthValid = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->depthValid, (Void *) &depthValid, sizeof(depthValid), &sampleTime); } } /* update altitude and altitude rate */ if (sensorGroupBits & sensorId.altitudeBit) { dm_read(controlDmi->altimeterDataValid, (Void *) &altimeterDataValid, sizeof(altimeterDataValid), (DM_Time * ) NULL); if (altimeterDataValid) { diffiltAltitude(controlDmi->altitude, controlDmi->sensorAltitude, controlDmi->sensorRateAltitude); if (!altitudeValid) { altitudeValid = TRUE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->altitudeValid, (Void *)&altitudeValid, sizeof(altitudeValid), &sampleTime); } } else if (altitudeValid) { altitudeValid = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->altitudeValid, (Void *) &altitudeValid, sizeof(altitudeValid), &sampleTime); } } /* update Kalman filter heading, rate, data valid flag and turns count */ if ((sensorGroupBits & sensorId.kalmanAttitudeBit) || (sensorGroupBits & sensorId.shipHeadingBit)) { /* dprint("Kalman or ship attitude changed\n");*/ dm_read(controlDmi->kalmanAttitude, (Void *) &kalmanAttitude, sizeof(kalmanAttitude), (DM_Time * ) NULL); dm_read(controlDmi->kalmanAttitudeStatus, (Void *) &kalmanAttitudeStatus, sizeof(kalmanAttitudeStatus), (DM_Time * ) NULL); if (kalmanAttitude.heading != NAN) kalmanHeadingDegrees = kalmanAttitude.heading * 180.0 / PI; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->kalmanHeading, (Void *) &kalmanAttitude.heading, sizeof(kalmanAttitude.heading), &sampleTime); dm_write(controlDmi->kalmanHeadingDegrees, (Void *) &kalmanHeadingDegrees, sizeof(kalmanHeadingDegrees), &sampleTime); dm_write(controlDmi->kalmanRateHeading, (Void *) &kalmanAttitude.headingRate, sizeof(kalmanAttitude.headingRate), &sampleTime); if (kalmanAttitudeStatus.headingStatus == GYRO_COMPASS_FILTERED) kalmanHeadingDataValid = TRUE; else kalmanHeadingDataValid = FALSE; if (kalmanHeadingDataValid != prevKalmanHeadingDataValid) dm_write(controlDmi->kalmanHeadingDataValid, (Void *) &kalmanHeadingDataValid, sizeof(kalmanHeadingDataValid), &sampleTime); prevKalmanHeadingDataValid = kalmanHeadingDataValid; dm_read(controlDmi->resetTurns, (Void *) &resetTurns, sizeof(resetTurns), (DM_Time * ) NULL); if (resetTurns) { /* dprint("reset turns!!!\n"); */ } dm_read(controlDmi->shipHeading, (Void *) &shipHeading, sizeof(shipHeading), (DM_Time *)NULL); shipHeading = DEGS_TO_RADS(shipHeading); if (kalmanAttitude.heading != NAN) { /* dprint("Got valid kalman heading...\n"); */ turnsCount = turnsCounter(kalmanAttitude.heading, shipHeading, resetTurns, controlDmi, &turnsSum, &turnsSetpoint); gettimeofday(&sampleTime, (struct timezone *) NULL); if (turnsCount != prevTurnsCount) dm_write(controlDmi->turnsCount, (Void *) &turnsCount, sizeof(turnsCount), &sampleTime); dm_write(controlDmi->turnsSum, (Void *) &turnsSum, sizeof(turnsSum), &sampleTime); /* Write turns setpoint*/ dm_write(controlDmi->turnsSetpoint, (Void *) &turnsSetpoint, sizeof(turnsSetpoint), &sampleTime); prevTurnsCount = turnsCount; if (first) { dm_write(controlDmi->turnsSetpoint, (Void *)&turnsSetpoint, sizeof(turnsSetpoint), &sampleTime); first = FALSE; } } else { /* dprint("Got INVALID kalman heading...\n"); */ ; } /* Always update turns setpoint*/ dm_write(controlDmi->turnsSetpoint, (Void *) &turnsSetpoint, sizeof(turnsSetpoint), &sampleTime); if (resetTurns) { /* dprint("Set reset back to false...\n"); */ resetTurns = FALSE; if ((err = dm_write(controlDmi->resetTurns, (Void *)&resetTurns, sizeof(resetTurns), &sampleTime)) != SUCCESS) { sprintDmError(err, "dm_write() resetTurns", errorBuf); logMsg("%s\n", errorBuf); } } } } /* FOREVER*/ } /* sensor filter*/ /*****************************************************************************/ /* Function : initSensorFilter */ /* Purpose : Performs sensor filter data manager items initialization and */ /* checks. */ /* Inputs : Control data manager items structure pointer, wakeup semaphore,*/ /* data manager group pointer and group id bits pointer. */ /* Outputs : Return OK or ERROR. */ /*****************************************************************************/ Errno initSensorFilter(controlDMItems *controlDmi, SEM_ID sensorSem, DM_Group *sensorGroup, sensorFilterBits *sensorId) { MBool resetTurns = FALSE; /* reset turns count flag */ MBool kalmanHeadingDataValid; /* heading data valid flag */ Int16 turnsCount = 0; /* turns count */ Flt32 turnsSum = 0.0; /* turns sum */ Flt32 filteredSensor = 0.0; /* filtered sensors */ VehicleAttitudeStatus kalmanAttitudeStatus; /* Kalman attitude status */ Errno status; /* error code */ DM_Time sampleTime; /* update time */ /* start consumer items and check status */ status = dm_start_consumer(controlDmi->sharpsX, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - sharps x position", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sharpsY, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - sharps y position", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sharpsZ, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - sharps z position", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sharpsRateX, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - sharps x rate", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sharpsRateY, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - sharps y rate", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sharpsRateZ, DM_ASYNC, sensorSem); if (checkConsumer(status, "Sensor Filter - sharps z rate", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorHeading, DM_ASYNC, sensorSem); if (checkConsumer(status, "Sensor Filter - heading sensor", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorRateHeading, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - heading rate sensor ", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->depth, DM_ASYNC, sensorSem); if (checkConsumer(status, "Sensor Filter - depth", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->altitude, DM_ASYNC, sensorSem); if (checkConsumer(status, "Sensor Filter - altitude", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->kalmanAttitude, DM_ASYNC, sensorSem); if (checkConsumer(status, "Sensor Filter - Kalman filter attitude", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sharpsDataValid, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - sharps data valid", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->headingValid, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - heading valid", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->headingRateValid, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - heading rate valid", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->depthDataValid, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - depth data valid", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->altimeterDataValid, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - altimeter data valid", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->kalmanAttitudeStatus, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - Kalman filter attitude status", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->resetTurns, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - reset turns flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->launchAngle, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - launchAngle", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->initialTurnsSum, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Sensor Filter - initialTurnsSum", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->shipHeading, DM_ASYNC, sensorSem); if (checkConsumer(status, "Sensor Filter - shipHeading", FALSE) == ERROR) return(ERROR); /* create data manager group */ *sensorGroup = dm_create_group(); /* add consumer items to group */ /* last item written by SHARPS task */ status = dm_group_add_item(*sensorGroup, controlDmi->sharpsRateZ, &sensorId->sharpsBit); if (checkGroup(status, "Sensor Filter - sharps z rate") == ERROR) return(ERROR); status = dm_group_add_item(*sensorGroup, controlDmi->depth, &sensorId->depthBit); if (checkGroup(status, "Sensor Filter - depth") == ERROR) return(ERROR); status = dm_group_add_item(*sensorGroup, controlDmi->altitude, &sensorId->altitudeBit); if (checkGroup(status, "Sensor Filter - altitude") == ERROR) return(ERROR); status = dm_group_add_item(*sensorGroup, controlDmi->kalmanAttitude, &sensorId->kalmanAttitudeBit); if (checkGroup(status, "Sensor Filter - Kalman filter attitude") == ERROR) return(ERROR); status = dm_group_add_item(*sensorGroup, controlDmi->shipHeading, &sensorId->shipHeadingBit); if (checkGroup(status, "Sensor Filter - Ship heading") == ERROR) return(ERROR); /* start provider items and check status */ status = dm_start_provider(controlDmi->sensorX, DM_ASYNC); if (checkProvider(status, "Sensor Filter - x position") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorY, DM_ASYNC); if (checkProvider(status, "Sensor Filter - y position") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorSharpsZ, DM_ASYNC); if (checkProvider(status, "Sensor Filter - z position") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorDepth, DM_ASYNC); if (checkProvider(status, "Sensor Filter - depth") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorAltitude, DM_ASYNC); if (checkProvider(status, "Sensor Filter - altitude") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorRateX, DM_ASYNC); if (checkProvider(status, "Sensor Filter - x rate") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorRateY, DM_ASYNC); if (checkProvider(status, "Sensor Filter - y rate") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorRateSharpsZ, DM_ASYNC); if (checkProvider(status, "Sensor Filter - z rate") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorRateDepth, DM_ASYNC); if (checkProvider(status, "Sensor Filter - depth rate") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->sensorRateAltitude, DM_ASYNC); if (checkProvider(status, "Sensor Filter - altitude rate") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->depthValid, DM_ASYNC); if (checkProvider(status, "Sensor Filter - depth valid") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->altitudeValid, DM_ASYNC); if (checkProvider(status, "Sensor Filter - altitude valid") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->positionValid, DM_ASYNC); if (checkProvider(status, "Sensor Filter - position valid") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->rateValid, DM_ASYNC); if (checkProvider(status, "Sensor Filter - rate valid") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->turnsCount, DM_STATIC); if (checkProvider(status, "Sensor Filter - turns count") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->turnsSum, DM_ASYNC); if (checkProvider(status, "Sensor Filter - turns sum") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->turnsSetpoint, DM_STATIC); if (checkProvider(status, "Sensor Filter - turns setpoint") == ERROR) return(ERROR); status = dm_start_multiple(controlDmi->resetTurns); if (checkProvider(status, "Sensor Filter - reset turns flag") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->kalmanHeading, DM_ASYNC); if (checkProvider(status, "Sensor Filter - Kalman filter heading") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->kalmanHeadingDegrees, DM_ASYNC); if (checkProvider(status, "Sensor Filter - Kalman filter heading (degrees)") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->kalmanRateHeading, DM_ASYNC); if (checkProvider(status, "Sensor Filter - Kalman filter heading rate") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->kalmanHeadingDataValid, DM_ASYNC); if (checkProvider(status, "Sensor Filter - Kalman filter heading data valid flag") == ERROR) return(ERROR); /* write initialized data manager items */ gettimeofday(&sampleTime, (struct timezone *) NULL); status = dm_write(controlDmi->sensorX, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - x position") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorY, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - y position") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorSharpsZ, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - z position") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorDepth, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - depth") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorAltitude, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - altitude") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorRateX, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - x rate") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorRateY, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - y rate") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorRateSharpsZ, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - z rate") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorRateDepth, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - depth rate") == ERROR) return(ERROR); status = dm_write(controlDmi->sensorRateAltitude, (Void *) &filteredSensor, sizeof(filteredSensor), &sampleTime); if (checkWrite(status, "Sensor filter - altitude rate") == ERROR) return(ERROR); status = dm_write(controlDmi->turnsCount, (Void *) &turnsCount, sizeof(turnsCount), &sampleTime); if (checkWrite(status, "Sensor filter - turns count") == ERROR) return(ERROR); status = dm_write(controlDmi->turnsSum, (Void *) &turnsSum, sizeof(turnsSum), &sampleTime); if (checkWrite(status, "Sensor filter - turns sum") == ERROR) return(ERROR); status = dm_write(controlDmi->resetTurns, (Void *) &resetTurns, sizeof(resetTurns), &sampleTime); if (checkWrite(status, "Sensor filter - reset turns") == ERROR) return(ERROR); dm_read(controlDmi->kalmanAttitudeStatus, (Void *) &kalmanAttitudeStatus, sizeof(kalmanAttitudeStatus), (DM_Time * ) NULL); if (kalmanAttitudeStatus.headingStatus == GYRO_COMPASS_FILTERED) kalmanHeadingDataValid = TRUE; else kalmanHeadingDataValid = FALSE; status = dm_write(controlDmi->kalmanHeadingDataValid, (Void *) &kalmanHeadingDataValid, sizeof(kalmanHeadingDataValid), &sampleTime); return SUCCESS; } /* initSensorFilter*/ /*****************************************************************************/ /* Function : diffiltDepth */ /* Purpose : Differentiates depth measurements and filters the resulting */ /* rates. */ /* Inputs : Filtered depth, depth and depth rate data manager item handles */ /* Outputs : None. */ /*****************************************************************************/ Void diffiltDepth(DM_Item dmiRawDepth, DM_Item dmiDepth, DM_Item dmiDepthRate) { MLocal MBool init = TRUE; /* initialization flag */ MLocal Flt64 prevTime; /* previous time */ MLocal Flt64 depthPrev[BUTTER_ORDER4]; /* previous depth measurement*/ MLocal Flt64 filteredDepthPrev[BUTTER_ORDER4];/* previous filtered depths*/ MLocal Flt64 ratePrev[BUTTER_ORDER4] = /* previous rate measurements*/ { 0.0, 0.0, 0.0, 0.0 }; MLocal Flt64 filteredRatePrev[BUTTER_ORDER4] =/* previous filtered rates */ { 0.0, 0.0, 0.0, 0.0 }; Int16 order; /* counter */ Flt32 filtDepth; /* dm item depth */ Flt32 filtDepthRate; /* dm item depth rate */ Flt64 depth; /* raw depth measurement */ Flt64 filteredDepth; /* filtered depth */ Flt64 dDepth; /* delta depth */ Flt64 depthRate; /* derived rate */ Flt64 filteredDepthRate; /* filtered depth rates */ Flt64 time; /* measurement time */ Flt64 samplePeriod; /* sample period */ DM_Time sensorTime; /* dm item measurement time */ DM_Time sampleTime; /* dm item sample time */ /* read the new depth measurement */ dm_read(dmiRawDepth, (Void *) &depth, sizeof(depth), &sensorTime); time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; /* filter the measurement */ if (init) { for (order = 0; order < BUTTER_ORDER4; order++) { depthPrev[order] = depth; filteredDepthPrev[order] = depth; } prevTime = time; init = FALSE; } filteredDepth = butterFilt5(depth, depthPrev, filteredDepthPrev); /* determine depth rate */ samplePeriod = time - prevTime; prevTime = time; if (samplePeriod < 0.00001) { depthRate = ratePrev[0]; filteredDepthRate = filteredRatePrev[0]; } else { depthRate = (filteredDepth - filteredDepthPrev[0]) / samplePeriod; /* filter result */ filteredDepthRate = butterFilt5(depthRate, ratePrev, filteredRatePrev); } /* update previous values */ for (order = (BUTTER_ORDER4-1); order > 0; order--) { depthPrev[order] = depthPrev[order-1]; filteredDepthPrev[order] = filteredDepthPrev[order-1]; ratePrev[order] = ratePrev[order-1]; filteredRatePrev[order] = filteredRatePrev[order-1]; } depthPrev[0] = depth; filteredDepthPrev[0] = filteredDepth; ratePrev[0] = depthRate; filteredRatePrev[0] = filteredDepthRate; /* update data manager items */ filtDepth = (Flt32) filteredDepth; filtDepthRate = (Flt32) filteredDepthRate; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(dmiDepth, (Void *) &filtDepth, sizeof(filtDepth), &sampleTime); dm_write(dmiDepthRate, (Void *) &filtDepthRate, sizeof(filtDepthRate), &sampleTime); } /* diffiltDepth*/ /*****************************************************************************/ /* Function : diffiltAltitude */ /* Purpose : Differentiates altitude measurements and filters the resulting */ /* rates. */ /* Inputs : Filtered altitude, altitude and altitude rate data manager item*/ /* handles. */ /* Outputs : None. */ /*****************************************************************************/ Void diffiltAltitude(DM_Item dmiRawAltitude, DM_Item dmiAltitude, DM_Item dmiAltitudeRate) { MLocal MBool init = TRUE; /* initialization flag */ MLocal Flt64 prevTime; /* previous time */ MLocal Flt64 altitudePrev[BUTTER_ORDER2]; /* prev altitude measurements*/ MLocal Flt64 filteredAltitudePrev[BUTTER_ORDER2];/*prev filtered altitude*/ MLocal Flt64 ratePrev[BUTTER_ORDER2] = /* previous rate measurements*/ { 0.0, 0.0 }; MLocal Flt64 filteredRatePrev[BUTTER_ORDER2] =/* previous filtered rates */ { 0.0, 0.0 }; Int16 order; /* counter */ Flt32 altitude; /* raw altitude measurement */ Flt32 filtAltitude; /* dm item altitude */ Flt32 filtAltitudeRate; /* dm item altitude rate */ Flt64 filteredAltitude; /* filtered altitude */ Flt64 dAltitude; /* delta altitude */ Flt64 altitudeRate; /* derived rate */ Flt64 filteredAltitudeRate; /* filtered altitude rates */ Flt64 time; /* measurement time */ Flt64 samplePeriod; /* sample period */ DM_Time sensorTime; /* dm item measurement time */ DM_Time sampleTime; /* dm item sample time */ /* read the new altitude measurement */ dm_read(dmiRawAltitude, (Void *) &altitude, sizeof(altitude), &sensorTime); time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; /* filter the measurement */ if (init) { for (order = 0; order < BUTTER_ORDER2; order++) { altitudePrev[order] = (Flt64) altitude; filteredAltitudePrev[order] = (Flt64) altitude; } prevTime = time; init = FALSE; } filteredAltitude = butterFilt6((Flt64) altitude, altitudePrev, filteredAltitudePrev); /* determine altitude rate */ samplePeriod = time - prevTime; prevTime = time; if (samplePeriod < 0.00001) { altitudeRate = ratePrev[0]; filteredAltitudeRate = filteredRatePrev[0]; } else { altitudeRate = (filteredAltitude - filteredAltitudePrev[0]) / samplePeriod; /* filter result */ filteredAltitudeRate = butterFilt6(altitudeRate, ratePrev, filteredRatePrev); } /* update previous values */ for (order = (BUTTER_ORDER2-1); order > 0; order--) { altitudePrev[order] = altitudePrev[order-1]; filteredAltitudePrev[order] = filteredAltitudePrev[order-1]; ratePrev[order] = ratePrev[order-1]; filteredRatePrev[order] = filteredRatePrev[order-1]; } altitudePrev[0] = (Flt64) altitude; filteredAltitudePrev[0] = filteredAltitude; ratePrev[0] = altitudeRate; filteredRatePrev[0] = filteredAltitudeRate; /* update data manager items */ filtAltitude = (Flt32) filteredAltitude; filtAltitudeRate = (Flt32) filteredAltitudeRate; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(dmiAltitude, (Void *) &filtAltitude, sizeof(filtAltitude), &sampleTime); dm_write(dmiAltitudeRate, (Void *) &filtAltitudeRate, sizeof(filtAltitudeRate), &sampleTime); } /* diffiltAltitude*/ /*****************************************************************************/ /* Function : butterFilt1 */ /* Purpose : Fourth order Butterworth filter with 0.025 Hz cut off frequency*/ /* Inputs : Current value and previous unfiltered and filtered values. */ /* Outputs : Filtered value. */ /*****************************************************************************/ Flt64 butterFilt1(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]) { /* numerator coefficients */ MLocal Flt64 bCoeff[] = { 0.03728051645169e-7, 0.14912204360229e-7, 0.22368316976440e-7, 0.14912199475248e-7, 0.03728053754593e-7 }; /* denominator coeffieients */ MLocal Flt64 aCoeff[] = { 1.0, -3.95895331864708, 5.87770027353614, -3.87853054905173, 0.95978365381150 }; Flt64 filt; /* filtered value */ filt = bCoeff[0] * raw + bCoeff[1] * rawPrev[0] + bCoeff[2] * rawPrev[1] + bCoeff[3] * rawPrev[2] + bCoeff[4] * rawPrev[3] - aCoeff[1] * filtPrev[0] - aCoeff[2] * filtPrev[1] - aCoeff[3] * filtPrev[2] - aCoeff[4] * filtPrev[3]; return(filt); } /* butterFilt1*/ /*****************************************************************************/ /* Function : butterFilt2 */ /* Purpose : Fourth order Butterworth filter with 0.5 Hz cut off frequency. */ /* Inputs : Current value and previous unfiltered and filtered values. */ /* Outputs : Filtered value. */ /*****************************************************************************/ Flt64 butterFilt2(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]) { /* numerator coefficients */ MLocal Flt64 bCoeff[] = { 0.00041659920441, 0.00166639681763, 0.00249959522644, 0.00166639681763, 0.00041659920441 }; /* denominator coeffieients */ MLocal Flt64 aCoeff[] = { 1.0, -3.18063854887472, 3.86119434899421, -2.11215535511097, 0.43826514226198 }; Flt64 filt; /* filtered value */ filt = bCoeff[0] * raw + bCoeff[1] * rawPrev[0] + bCoeff[2] * rawPrev[1] + bCoeff[3] * rawPrev[2] + bCoeff[4] * rawPrev[3] - aCoeff[1] * filtPrev[0] - aCoeff[2] * filtPrev[1] - aCoeff[3] * filtPrev[2] - aCoeff[4] * filtPrev[3]; return(filt); } /* butterFilt2*/ /*****************************************************************************/ /* Function : butterFilt3 */ /* Purpose : Eighth order Butterworth filter with 0.5 Hz cut off frequency. */ /* Inputs : Current value and previous unfiltered and filtered values. */ /* Outputs : Filtered value. */ /*****************************************************************************/ Flt64 butterFilt3(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]) { /* numerator coefficients */ /* butter(8,0.1), ~ 2 second lag MLocal Flt64 bCoeff[] = { 0.00176255372963e-4, 0.01410042986372e-4, 0.04935150428764e-4, 0.09870300914372e-4, 0.12337876057700e-4, 0.09870300917925e-4, 0.04935150423435e-4, 0.01410042988148e-4, 0.00176255372353e-4 }; */ /* butter(8,0.04), ~ 5 second lag */ MLocal Flt64 bCoeff[] = { 0.00177837966575e-7, 0.01422708173493e-7, 0.04979430201502e-7, 0.09959009616978e-7, 0.12448523989406e-7, 0.09959045144114e-7, 0.04979387568937e-7, 0.01422725048883e-7, 0.00177835079995e-7 }; /* denominator coeffieients */ /* 0.5 Hz cutoff MLocal Flt64 aCoeff[] = { 1.0, -6.39036456310855, 18.00033833573992, -29.17109937488289, 29.73137543832751, -19.50563176812669, 8.04099593299896, -1.90366889113259, 0.19810001155979 }; */ /* 0.2 Hz cutoff*/ MLocal Flt64 aCoeff[] = { 1.0, -7.35591423914845, 23.69703935914539, -43.66582962023367, 50.33661933673948, -37.17134735658716, 17.17127547086699, -4.53667256190413, 0.52482965664806 }; Flt64 filt; /* filtered value */ filt = bCoeff[0] * raw + bCoeff[1] * rawPrev[0] + bCoeff[2] * rawPrev[1] + bCoeff[3] * rawPrev[2] + bCoeff[4] * rawPrev[3] + bCoeff[5] * rawPrev[4] + bCoeff[6] * rawPrev[5] + bCoeff[7] * rawPrev[6] + bCoeff[8] * rawPrev[7] - aCoeff[1] * filtPrev[0] - aCoeff[2] * filtPrev[1] - aCoeff[3] * filtPrev[2] - aCoeff[4] * filtPrev[3] - aCoeff[5] * filtPrev[4] - aCoeff[6] * filtPrev[5] - aCoeff[7] * filtPrev[6] - aCoeff[8] * filtPrev[7]; return(filt); } /* butterFilt3*/ /*****************************************************************************/ /* Function : butterFilt4 */ /* Purpose : Eighth order Butterworth filter with 0.25 Hz cut off frequency.*/ /* Inputs : Current value and previous unfiltered and filtered values. */ /* Outputs : Filtered value. */ /*****************************************************************************/ Flt64 butterFilt4(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]) { /* numerator coefficients */ /* 0.25 Hz cutoff MLocal Flt64 bCoeff[] = { 0.00983559123036e-7, 0.07868478313355e-7, 0.27539623914663e-7, 0.55079389937873e-7, 0.68849026035878e-7, 0.55079411254155e-7, 0.27539602598381e-7, 0.07868490747853e-7, 0.00983556958101e-7 }; /* /* 0.05 cutoff*/ MLocal Flt64 bCoeff[] = { 0.00344169137634e-12, 0.02842170943040e-12, 0.09947598300641e-12, 0.19184653865523e-12, 0.22737367544323e-12, 0.19895196601283e-12, 0.08526512829121e-12, 0.03108624468950e-12, 0.00255351295664e-12 }; /* denominator coeffieients */ /* 0.25 Hz cutoff MLocal Flt64 aCoeff[] = { 1.0, -7.19492435842328, 22.68506299943665, -40.93508346568446, 46.23642584093405, -33.47192031399042, 15.16567105859504, -3.93176549146491, 0.44653398238846 }; */ /* 0.05 cutoff*/ MLocal Flt64 aCoeff[] = { 1.0, -7.83896798103224, 26.88571362019588, -52.69528124027716, 64.55460591611882, -50.61600367669253, 24.80581124704008, -6.94713478089517, 0.85125689554320 }; Flt64 filt; /* filtered value */ filt = bCoeff[0] * raw + bCoeff[1] * rawPrev[0] + bCoeff[2] * rawPrev[1] + bCoeff[3] * rawPrev[2] + bCoeff[4] * rawPrev[3] + bCoeff[5] * rawPrev[4] + bCoeff[6] * rawPrev[5] + bCoeff[7] * rawPrev[6] + bCoeff[8] * rawPrev[7] - aCoeff[1] * filtPrev[0] - aCoeff[2] * filtPrev[1] - aCoeff[3] * filtPrev[2] - aCoeff[4] * filtPrev[3] - aCoeff[5] * filtPrev[4] - aCoeff[6] * filtPrev[5] - aCoeff[7] * filtPrev[6] - aCoeff[8] * filtPrev[7]; return(filt); } /* butterFilt4*/ /*****************************************************************************/ /* Function : butterFilt5 */ /* Purpose : Fourth order Butterworth filter with 0.25 Hz cut off frequency */ /* for 10 Hz sample frequency. */ /* Inputs : Current value and previous unfiltered and filtered values. */ /* Outputs : Filtered value. */ /*****************************************************************************/ Flt64 butterFilt5(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]) { /* numerator coefficients */ MLocal Flt64 bCoeff[] = { 0.03123897691704e-3, 0.12495590766903e-3, 0.18743386150000e-3, 0.12495590766992e-3, 0.03123897691659e-3 }; /* denominator coeffieients */ MLocal Flt64 aCoeff[] = { 1.0, -3.58973388711218, 4.85127588251942, -2.92405265616246, 0.66301048438589 }; Flt64 filt; /* filtered value */ filt = bCoeff[0] * raw + bCoeff[1] * rawPrev[0] + bCoeff[2] * rawPrev[1] + bCoeff[3] * rawPrev[2] + bCoeff[4] * rawPrev[3] - aCoeff[1] * filtPrev[0] - aCoeff[2] * filtPrev[1] - aCoeff[3] * filtPrev[2] - aCoeff[4] * filtPrev[3]; return(filt); } /* butterFilt5*/ /*****************************************************************************/ /* Function : butterFilt6 */ /* Purpose : Second order Butterworth filter with 0.3 Hz cut off frequency */ /* for 3 Hz sample frequency. */ /* Inputs : Current value and previous unfiltered and filtered values. */ /* Outputs : Filtered value. */ /*****************************************************************************/ Flt64 butterFilt6(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]) { /* numerator coefficients */ MLocal Flt64 bCoeff[] = { 0.06745527388907, 0.13491054777814, 0.06745527388907 }; /* denominator coeffieients */ MLocal Flt64 aCoeff[] = { 1.0, -1.14298050253990, 0.41280159809619 }; Flt64 filt; /* filtered value */ filt = bCoeff[0] * raw + bCoeff[1] * rawPrev[0] + bCoeff[2] * rawPrev[1] - aCoeff[1] * filtPrev[0] - aCoeff[2] * filtPrev[1]; return(filt); } /* butterFilt6*/ /*****************************************************************************/ /* Function : turnsCounter */ /* Purpose : Calculates number of turns from heading sensor update */ /* Inputs : ROV and ship headings (in radians) */ /* Outputs : Returns number of turns */ /*****************************************************************************/ Int16 turnsCounter(Flt32 heading, Flt32 shipHeading, MBool resetTurns, controlDMItems *controlDmi, Flt32 *hSum, Flt32 *turnsSetpoint) { static MBool initialized = FALSE; /* initialized flag */ static Flt32 lastTwistAngle = 0.0; /* H(k-1) from last cycle */ static Flt32 launchAngle = PI; Flt32 twistAngle; Int16 turns = 0; /* Calculated number of turns */ Flt32 dH; /* Delta twist twist(k) - twist(k-1) */ Flt32 logSum; /* turns sum from log file */ Flt32 logOffset; /* launch offset from log file */ DM_Time t; MBool debug = FALSE; Errno err; static char errorBuf[100]; if (resetTurns) /* reset turns counter */ { if ((err = dm_read(controlDmi->initialTurnsSum, (void *)hSum, sizeof(Flt32), &t)) != SUCCESS) { sprintDmError(err, "turns reset: dm_read() initialTwist", errorBuf); logMsg("%s\n", errorBuf); } if ((err = dm_read(controlDmi->launchAngle, (void *)&launchAngle, sizeof(launchAngle), &t)) != SUCCESS) { sprintDmError(err, "turns reset: dm_read() launchAngle", errorBuf); logMsg("%s\n", errorBuf); } dprint("reset turns: initialTwist = %.5f", *hSum); dprint(" launchAngle = %.5f\n", launchAngle); } if (initialized) /* Check for first time through */ { twistAngle = heading - shipHeading - launchAngle; dH = twistAngle - lastTwistAngle; /* Calculate twist change dH */ if (dH < -PI) /* Check for 0 to 360 transition */ dH += 2.0 * PI; else if (dH > PI) dH -= 2.0 * PI; *hSum += dH; /* Add delta twist to heading twist sum */ dprint("heading=%.5f, shipHeading=%.5f, launchAngle=%.5f\n", heading, shipHeading, launchAngle); dprint("dH=%f, hSum=%f\n", dH, *hSum); } else /* initialize turns counter to last */ { /* logged count and sum */ readTurnsLog(&logSum, &logOffset); closeTurnsLog(); dprint("Initializing turns counter: " "Launch offset %7.4f Sum %7.4f\n", logOffset, logSum); *hSum = twistAngle = logSum; launchAngle = logOffset; initialized = TRUE; /* H(k-1) is now valid */ } lastTwistAngle = twistAngle; /* Save H(k-1) for next iteration */ *turnsSetpoint = shipHeading + launchAngle; turns = (Int16) (*hSum / (2.0 * PI)); return (turns); /* Return number of turns */ } /* turnsCounter */