/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Sensor Data Recording Module for Tiburon Control System */ /* Filename : record.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 12/02/93 */ /* Modified : 01/12/96 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header$ * $Log$ * */ /******************************************************************************/ #include /* VxWorks system declarations */ #include /* VxWorks standard I/O library */ #include /* VxWorks watchdog timer library */ #include /* VxWorks semaphore library */ #include /* VxWorks system time declarations */ #include /* VxWorks string library */ #include /* VxWorks task library */ #include /* MBARI style guide type declarations */ #include /* MBARI time declarations */ #include /* data manager declarations */ #include /* data manager error declarations */ #include /* control system data manager definitions */ #include /* SHARPS data manager definitions */ #include /* thruster definitions */ #include /* Kalman filter definitions */ #include /* sensor definitions */ #include /* VME IBC definitions */ #include "control.h" /* control system definitions */ #include "dopplerDM.h" /* ADV definitions */ #define RECORD_PRIORITY 75 /* record task priority */ #define RECORD_PERIOD 100000 /* record period (microsec) (10 Hz rate) */ #define DOF 4 /* number of degrees of freedom */ #define TRANS_DOF 3 /* number of translation degrees of freedom */ /* data selection */ typedef enum { TRANS, TRANSPLUS, ROT, ROTPLUS, VERT, VERTPLUS, ADV, KALMAN, MANOEUVRE_TRANS, MANOEUVRE_ROT, MANOEUVRE_VERT, SERVO_TRANS, SERVO_ROT, SERVO_DEPTH, SERVO_ALT, CONTROL } dataSelection; /* translation dof */ typedef enum { X, Y, Z } transDof; /* data manager item handles */ typedef struct { DM_Item sharpsX; /* sharps x measurement */ DM_Item sharpsY; /* sharps y measurement */ DM_Item sharpsRateX; /* sharps x rate measurement */ DM_Item sharpsRateY; /* sharps y rate measurement */ DM_Item accelerometers; /* accelerometer x, y, z measurement */ DM_Item gyroDegrees; /* gyro heading measurement */ DM_Item gyroRateDegrees; /* gyro heading rate measurement */ DM_Item gyroRadians; /* gyro heading measurement */ DM_Item gyroRateRadians; /* gyro heading rate measurement */ DM_Item pitchDegrees; /* pitch measurement */ DM_Item rollDegrees; /* roll measurement */ DM_Item depth; /* depth measurement */ DM_Item altitude; /* altitude measurement */ DM_Item advVelocity[TRANS_DOF]; /* doppler velocities */ DM_Item kalmanHeading; /* Kalman filter heading */ DM_Item kalmanRateHeading; /* Kalman filter heading rate */ DM_Item sensorX; /* sensor x measurement */ DM_Item sensorY; /* sensor y measurement */ DM_Item sensorDepth; /* filtered depth measurement */ DM_Item sensorAltitude; /* filtered altitude measurement */ DM_Item sensorHeading; /* sensor heading measurement */ DM_Item depthRate; /* sensor depth rate measurement */ DM_Item altitudeRate; /* sensor altitude rate measurement */ DM_Item sensorRateX; /* sensor x rate measurement */ DM_Item sensorRateY; /* sensor y rate measurement */ DM_Item sensorRateHeading; /* sensor heading rate measurement */ DM_Item setpointX; /* x trajectory setpoint */ DM_Item setpointY; /* y trajectory setpoint */ DM_Item setpointZ; /* z trajectory setpoint */ DM_Item setpointHeading; /* heading trajectory setpoint */ DM_Item zIntegral; /* z integration term */ DM_Item joystickX; /* joystick x command */ DM_Item joystickHeading; /* joystick heading command */ DM_Item velocityCommandX; /* joystick x velocity command */ DM_Item velocityCommandY; /* joystick y velocity command */ DM_Item velocityCommandZ; /* joystick z velocity command */ DM_Item velocityCommandHeading; /* joystick heading velocity command */ DM_Item modelThrustX; /* dynamic model thrust x command */ DM_Item modelThrustHeading; /* dynamic model heading thrust cmd */ DM_Item commandedThrust[DOF]; /* vehicle commanded thrust */ DM_Item controlThrust[NUM_THRUSTERS];/* thruster control thrust */ DM_Item motorControlThrust[NUM_THRUSTERS];/* thruster motor control thrust*/ DM_Item stopRecording; /* stop data recording flag */ } recordDMItems; /* function prototypes */ LOCAL Errno recordDMInit(recordDMItems *recordDmi); LOCAL Void getSharpsXY(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 position[], Flt32 rate[], Flt64 *time); LOCAL Void getRovXY(MBool init, recordDMItems *recordDmi, Flt32 position[], Flt32 rate[]); LOCAL Void getAccelXY(FILE *fpw, MBool init, recordDMItems *recordDmi, Int16 accelerometer[], Flt64 *accelerometerTime); LOCAL Void getAccelZ(FILE *fpw, MBool init, recordDMItems *recordDmi, Int16 accelerometer[], Flt64 *accelerometerTime); LOCAL Void getGyro(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *gyro, Flt32 *gyroRate, Flt64 *time); LOCAL Void getGyroRadians(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *gyroRadians, Flt32 *gyroRateRadians, Flt64 *time); LOCAL Void getPitch(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *pitch, Flt64 *time); LOCAL Void getRoll(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *roll, Flt64 *time); LOCAL Void getDepth(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt64 *depth, Flt32 *sensorDepth, Flt32 *depthRate, Flt64 *time); LOCAL Void getAltitude(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *altitude, Flt32* filteredAltitude, Flt32 *altitudeRate, Flt64 *time); LOCAL Void getADV(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 advVelocity[], Flt64 *time); LOCAL Void getKalman(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *heading, Flt32 *headingRate, Flt64 *time); LOCAL Void getHeading(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *heading, Flt32 *headingRate, Flt64 *time); LOCAL Void getSetpointXY(MBool init, recordDMItems *recordDmi, Flt32 setpoint[]); LOCAL Void getSetpointZ(MBool init, recordDMItems *recordDmi, Flt32 setpoint[]); LOCAL Void getSetpointH(MBool init, recordDMItems *recordDmi, Flt32 setpoint[]); LOCAL Void getZIntegral(MBool init, recordDMItems *recordDmi, Flt32 *zIntegral); LOCAL Void getJoystick(FILE *fpw, MBool init, recordDMItems *recordDmi, Int16 joystick[], Flt64 *joyTime); LOCAL Void getVelocityCommandXY(MBool init, recordDMItems *recordDmi, Flt32 velocityCommand[]); LOCAL Void getVelocityCommandZ(MBool init, recordDMItems *recordDmi, Flt32 velocityCommand[]); LOCAL Void getVelocityCommandH(MBool init, recordDMItems *recordDmi, Flt32 velocityCommand[]); LOCAL Void getModelThrust(MBool init, recordDMItems *recordDmi, Flt32 modelThrust[]); LOCAL Void getThrustXY(MBool init, recordDMItems *recordDmi, Flt32 thrust[]); LOCAL Void getThrustZ(MBool init, recordDMItems *recordDmi, Flt32 thrust[]); LOCAL Void getThrustH(MBool init, recordDMItems *recordDmi, Flt32 thrust[]); LOCAL Void getControlThrust(MBool init, recordDMItems *recordDmi, Flt32 controlThrust[]); LOCAL Void getMotorControlThrust(MBool init, recordDMItems *recordDmi, Int16 motorControlThrust[]); /******************************************************************************/ /* Function : recordTask */ /* Purpose : Records sensor data. */ /* Inputs : None. */ /* Outputs : Returns ERROR on fatal error. */ /******************************************************************************/ STATUS recordTask(char* selection) { recordDMItems recordDmi; /* data manager items */ dataSelection select; /* data selection */ WDOG_ID recordPeriod; /* watchdog timer period */ SEM_ID recordSem; /* task wakeup semaphore */ FILE *fpw1, *fpw2; /* file pointers */ MBool init = TRUE; /* initialization flag */ MBool stopRecording = FALSE; /* stop recording flag */ Flt32 sharpsPosition[TRANS_DOF]; /* SHARPS positions, rates */ Flt32 sharpsRate[TRANS_DOF]; /* and time */ Flt64 sharpsTime; Flt32 vehiclePosition[TRANS_DOF]; /* rov positions and rates */ Flt32 vehicleRate[TRANS_DOF]; /* accounting for SHARPS */ /* transponder position */ Int16 accelerometer[TRANS_DOF]; /* accelerations and times */ Flt64 accelerometerTime; Flt32 gyro, gyroRate; /* gyro degrees, rate & time */ Flt32 gyroRadians, gyroRateRadians; /* gyro radians and rate */ Flt64 gyroTime; Flt32 pitch; /* pitch degrees and time */ Flt64 pitchTime; Flt32 roll; /* roll degrees and time */ Flt64 rollTime; Flt64 depth; /* depth metres */ Flt32 filteredDepth; /* filtered depth metres */ Flt32 depthRate; /* depth rate */ Flt64 depthTime; /* depth time */ Flt32 altitude, altitudeRate; /* altitude metres and rate */ Flt32 filteredAltitude; /* filtered altitude metres */ Flt64 altitudeTime; /* altitude time */ Flt32 heading, headingRate; /* heading, rate and time */ Flt64 headingTime; Flt32 advVelocity[TRANS_DOF]; /* ADV velocity and time */ Flt64 advTime; Flt32 kalmanHeading; /* Kalman filter heading, rate*/ Flt32 kalmanRateHeading; /* and time */ Flt64 kalmanHeadingTime; Flt32 setpoint[DOF]; /* trajectory setpoint */ Flt32 zIntegral; /* z integration term */ Int16 joystick[DOF]; /* joystick commands and time */ Flt64 joystickTime; Flt32 velocityCommand[DOF]; /* joystick commanded velocity*/ Flt32 modelThrust[DOF]; /* model thrust commands */ Flt32 commandedThrust[DOF]; /* vehicle commanded thrust */ Flt32 controlThrust[NUM_THRUSTERS]; /* thruster control thrust */ Int16 motorControlThrust[NUM_THRUSTERS];/* motor control thrust */ /* determine the data selection */ if (strcmp(selection, "trans") == 0) select = TRANS; else if (strcmp(selection, "transplus") == 0) select = TRANSPLUS; else if (strcmp(selection, "rot") == 0) select = ROT; else if (strcmp(selection, "rotplus") == 0) select = ROTPLUS; else if (strcmp(selection, "vert") == 0) select = VERT; else if (strcmp(selection, "vertplus") == 0) select = VERTPLUS; else if (strcmp(selection, "adv") == 0) select = ADV; else if (strcmp(selection, "kalman") == 0) select = KALMAN; else if (strcmp(selection, "mvtrans") == 0) select = MANOEUVRE_TRANS; else if (strcmp(selection, "mvrot") == 0) select = MANOEUVRE_ROT; else if (strcmp(selection, "mvvert") == 0) select = MANOEUVRE_VERT; else if (strcmp(selection, "svtrans") == 0) select = SERVO_TRANS; else if (strcmp(selection, "svrot") == 0) select = SERVO_ROT; else if (strcmp(selection, "svdepth") == 0) select = SERVO_DEPTH; else if (strcmp(selection, "svalt") == 0) select = SERVO_ALT; else if (strcmp(selection, "ctrl") == 0) select = CONTROL; else { printf("Usage : sp recordTask,""""trans"""" (sharps/accel x"); printf(", y & pitch, roll data)\n"); printf(" """"transplus"""" (sharps/accel x"); printf(", y + rov x, y &\n"); printf(" pitch, roll data)\n"); printf(" """"rot"""" (gyro "); printf("data)\n"); printf(" """"rotplus"""" (gyro "); printf("+ pitch, roll data)\n"); printf(" """"vert"""" (depth, altitude"); printf(" data)\n"); printf(" """"vertplus"""" (depth, altitude"); printf(" + pitch, roll data)\n"); printf(" """"adv"""" (adv velocity "); printf("data)\n"); printf(" """"kalman"""" (kalman heading "); printf("data)\n"); printf(" """"mvtrans"""" (velocity x, y "); printf("data)\n"); printf(" """"mvrot"""" (velocity "); printf("heading data)\n"); printf(" """"mvvert"""" (velocity z "); printf("data)\n"); printf(" """"svtrans"""" (servo x, y "); printf("data)\n"); printf(" """"svrot"""" (servo heading "); printf("data)\n"); printf(" """"svdepth"""" (servo depth data)\n"); printf(" """"svalt"""" (servo altitude data)\n"); printf(" """"ctrl"""" (control data)\n"); return(ERROR); } /* set record task priority */ taskPrioritySet(taskIdSelf(), RECORD_PRIORITY); /* initialize task wakeup semaphores */ if ( ((recordSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) || ((recordPeriod = wdCreate()) == NULL) ) { logMsg("Control System FAILURE :\n could not initialize record semaphore s\n"); return(ERROR); } /* open data files */ switch (select) { case TRANS : if ( (fpw1 = fopen("/usr/vw/logs/xDat.out", "w")) == NULL) { logMsg("error opening xDat.out\n"); return(ERROR); } if ( (fpw2 = fopen("/usr/vw/logs/yDat.out", "w")) == NULL) { logMsg("error opening yDat.out\n"); return(ERROR); } /* fprintf(fpw1, "sharpsTime x xRate accelTime xAccel "); */ fprintf(fpw1, "sharpsTime x xRate "); fprintf(fpw1,"xThrust rollTime roll\n"); /* fprintf(fpw2, "sharpsTime y yRate accelTime yAccel "); */ fprintf(fpw2, "sharpsTime y yRate "); fprintf(fpw2,"yThrust pitchTime pitch\n"); break; case TRANSPLUS : if ( (fpw1 = fopen("/usr/vw/logs/xDatP.out", "w")) == NULL) { logMsg("error opening xDatP.out\n"); return(ERROR); } if ( (fpw2 = fopen("/usr/vw/logs/yDatP.out", "w")) == NULL) { logMsg("error opening yDatP.out\n"); return(ERROR); } fprintf(fpw1, "sharpsTime x xRate xRov xRateRov "); /* fprintf(fpw1, "accelTime xAccel xThrust rollTime "); */ fprintf(fpw1, "xThrust rollTime "); fprintf(fpw1, "roll\n"); fprintf(fpw2, "sharpsTime y yRate yRov yRateRov "); /* fprintf(fpw2, "accelTime yAccel yThrust pitchTime "); */ fprintf(fpw2, "yThrust pitchTime "); fprintf(fpw2, "pitch\n"); break; case ROT : if ( (fpw1 = fopen("/usr/vw/logs/hDat.out", "w")) == NULL) { logMsg("error opening hDat.out\n"); return(ERROR); } fprintf(fpw1, "gyroTime gyro gyroRate hThrust\n"); break; case ROTPLUS : if ( (fpw1 = fopen("/usr/vw/logs/hDatP.out", "w")) == NULL) { logMsg("error opening hDatP.out\n"); return(ERROR); } if ( (fpw2 = fopen("/usr/vw/logs/prDatP.out", "w")) == NULL) { logMsg("error opening prDatP.out\n"); return(ERROR); } fprintf(fpw1, "gyroTime gyro gyroRate hThrust\n"); fprintf(fpw2, "pitchTime pitch rollTime roll\n"); break; case VERT : if ( (fpw1 = fopen("/usr/vw/logs/zDat.out", "w")) == NULL) { logMsg("error opening zDat.out\n"); return(ERROR); } /* fprintf(fpw1, "depthTime depth depthRate altTime"); fprintf(fpw1, "altitude altRate accelTime accel "); fprintf(fpw1, "zThrust\n"); */ fprintf(fpw1, "depthTime depth filtDepth depthRate "); fprintf(fpw1, "altTime altitude filtAlt altRate "); fprintf(fpw1, "zThrust\n"); break; case VERTPLUS : if ( (fpw1 = fopen("/usr/vw/logs/zDatP.out", "w")) == NULL) { logMsg("error opening zDatP.out\n"); return(ERROR); } if ( (fpw2 = fopen("/usr/vw/logs/daDatP.out", "w")) == NULL) { logMsg("error opening daDatP.out\n"); return(ERROR); } /* fprintf(fpw1, "depthTime depth depthRate altTime"); fprintf(fpw1, "altitude altRate accelTime accel "); fprintf(fpw1, "zThrust\n"); */ fprintf(fpw1, "depthTime depth depthRate altTime "); fprintf(fpw1, "altitude altRate zThrust\n"); fprintf(fpw2, "pitchTime pitch rollTime roll\n"); break; case ADV : if ( (fpw1 = fopen("/usr/vw/logs/advDat.out", "w")) == NULL) { logMsg("error opening advDat.out\n"); return(ERROR); } fprintf(fpw1, "advTime xVelocity yVelocity zVelocity\n"); break; case KALMAN : if ( (fpw1 = fopen("/usr/vw/logs/kDat.out", "w")) == NULL) { logMsg("error opening kDat.out\n"); return(ERROR); } fprintf(fpw1, "kalmanHeadingTime kalmanHeading "); fprintf(fpw1, "kalmanHeadingRate hThrust\n"); break; case MANOEUVRE_TRANS : if ( (fpw1 = fopen("/usr/vw/logs/mvxDat.out", "w")) == NULL) { logMsg("error opening mvxDat.out\n"); return(ERROR); } if ( (fpw2 = fopen("/usr/vw/logs/mvyDat.out", "w")) == NULL) { logMsg("error opening mvyDat.out\n"); return(ERROR); } fprintf(fpw1, "sharpsTime x xRate xRov xRateRov "); fprintf(fpw1, "xVelocityCommand xThrust\n"); fprintf(fpw2, "sharpsTime y yRate yRov yRateRov "); fprintf(fpw2, "yVelocityCommand yThrust\n"); break; case MANOEUVRE_ROT : if ( (fpw1 = fopen("/usr/vw/logs/mvhDat.out", "w")) == NULL) { logMsg("error opening mvhDat.out\n"); return(ERROR); } fprintf(fpw1, "headingTime heading headingRate "); fprintf(fpw1, "hVelocityCommand hThrust\n"); break; case MANOEUVRE_VERT : if ( (fpw1 = fopen("/usr/vw/logs/mvzDat.out", "w")) == NULL) { logMsg("error opening mvzDat.out\n"); return(ERROR); } fprintf(fpw1, "altTime altitude altRate "); fprintf(fpw1, "zVelocityCommand zThrust\n"); break; case SERVO_TRANS : if ( (fpw1 = fopen("/usr/vw/logs/svxDat.out", "w")) == NULL) { logMsg("error opening svxDat.out\n"); return(ERROR); } if ( (fpw2 = fopen("/usr/vw/logs/svyDat.out", "w")) == NULL) { logMsg("error opening svyDat.out\n"); return(ERROR); } fprintf(fpw1, "sharpsTime x xRate xRov xRateRov "); fprintf(fpw1, "xSetpoint xThrust\n"); fprintf(fpw2, "sharpsTime y yRate yRov yRateRov "); fprintf(fpw2, "ySetpoint yThrust\n"); break; case SERVO_ROT : if ( (fpw1 = fopen("/usr/vw/logs/svhDat.out", "w")) == NULL) { logMsg("error opening svhDat.out\n"); return(ERROR); } fprintf(fpw1, "headingTime heading headingRate hSetpoint"); fprintf(fpw1, " hThrust\n"); break; case SERVO_DEPTH : if ( (fpw1 = fopen("/usr/vw/logs/svdDat.out", "w")) == NULL) { logMsg("error opening svdDat.out\n"); return(ERROR); } fprintf(fpw1, "depthTime depth depthFilt depthRate "); fprintf(fpw1, "zSetpoint zIntegral zThrust\n"); break; case SERVO_ALT : if ( (fpw1 = fopen("/usr/vw/logs/svaDat.out", "w")) == NULL) { logMsg("error opening svaDat.out\n"); return(ERROR); } fprintf(fpw1, "altTime altitude altFilt altRate "); fprintf(fpw1, "zSetpoint zThrust\n"); break; case CONTROL : if ( (fpw1 = fopen("/usr/vw/logs/ctrlCDat.out", "w")) == NULL) { logMsg("error opening ctrlCDat.out\n"); return(ERROR); } if ( (fpw2 = fopen("/usr/vw/logs/ctrlTDat.out", "w")) == NULL) { logMsg("error opening ctrlTDat.out\n"); return(ERROR); } fprintf(fpw1, "time joystickX joystickH velX velH "); fprintf(fpw1, "modelX modelH thrustX thrustH "); fprintf(fpw2, "thrustPH thrustSH thrustFL thrustAL "); fprintf(fpw2, "motorPH motorSH motorFL motorAL\n"); break; } /* lookup data manager item handles and start consumers */ if (recordDMInit(&recordDmi) == ERROR) return(ERROR); FOREVER { /* set record rate with watchdog timer */ /* do not use MBARI type Int16 to typecast int here */ wdStart(recordPeriod, (sysClkRateGet() / (USECS_PER_SEC / RECORD_PERIOD)), (FUNCPTR) semGive, (int) recordSem); /* get stop recording flag */ dm_read(recordDmi.stopRecording, (Void *) &stopRecording, sizeof(stopRecording), (DM_Time *) NULL); if (stopRecording) { switch (select) { case TRANS : case TRANSPLUS : case ROTPLUS : case VERTPLUS : case MANOEUVRE_TRANS : case SERVO_TRANS : case CONTROL : fclose(fpw1); fclose(fpw2); break; case ROT : case VERT : case ADV : case KALMAN : case MANOEUVRE_ROT : case MANOEUVRE_VERT : case SERVO_ROT : case SERVO_DEPTH : case SERVO_ALT : fclose(fpw1); break; } return(OK); } switch (select) { case TRANS : /* read x and y SHARPS positions and rates, accelerometers and commanded */ /* thrust */ getSharpsXY(fpw1, init, &recordDmi, sharpsPosition, sharpsRate, &sharpsTime); /* getAccelXY(fpw1, init, &recordDmi, accelerometer, &accelerometerTime); */ getThrustXY(init, &recordDmi, commandedThrust); getPitch(fpw1, init, &recordDmi, &pitch, &pitchTime); getRoll(fpw1, init, &recordDmi, &roll, &rollTime); /* fprintf(fpw1, "%lf %f %f %lf %d %f %lf %f\n", sharpsTime, sharpsPosition[X], sharpsRate[X], accelerometerTime, accelerometer[X], commandedThrust[X_INDEX], rollTime, roll); fprintf(fpw2, "%lf %f %f %lf %d %f %lf %f\n", sharpsTime, sharpsPosition[Y], sharpsRate[Y], accelerometerTime, accelerometer[Y], commandedThrust[Y_INDEX], pitchTime, pitch); */ fprintf(fpw1, "%lf %f %f %f %lf %f\n", sharpsTime, sharpsPosition[X], sharpsRate[X], commandedThrust[X_INDEX], rollTime, roll); fprintf(fpw2, "%lf %f %f %f %lf %f\n", sharpsTime, sharpsPosition[Y], sharpsRate[Y], commandedThrust[Y_INDEX], pitchTime, pitch); break; case TRANSPLUS : /* read x and y sharps positions and rates, rov transformed positions and */ /* rates, accelerometers and commanded thrust */ getSharpsXY(fpw1, init, &recordDmi, sharpsPosition, sharpsRate, &sharpsTime); getRovXY(init, &recordDmi, vehiclePosition, vehicleRate); /* getAccelXY(fpw1, init, &recordDmi, accelerometer, &accelerometerTime); */ getThrustXY(init, &recordDmi, commandedThrust); getPitch(fpw1, init, &recordDmi, &pitch, &pitchTime); getRoll(fpw1, init, &recordDmi, &roll, &rollTime); /* fprintf(fpw1, "%lf %f %f %f %f %lf %d %f %lf %f\n", sharpsTime, sharpsPosition[X], sharpsRate[X], vehiclePosition[X], vehicleRate[X], accelerometerTime, accelerometer[X], commandedThrust[X_INDEX], rollTime, roll); fprintf(fpw2, "%lf %f %f %f %f %lf %d %f %lf %f\n", sharpsTime, sharpsPosition[Y], sharpsRate[Y], vehiclePosition[Y], vehicleRate[Y], accelerometerTime, accelerometer[Y], commandedThrust[Y_INDEX], pitchTime, pitch); */ fprintf(fpw1, "%lf %f %f %f %f %f %lf %f\n", sharpsTime, sharpsPosition[X], sharpsRate[X], vehiclePosition[X], vehicleRate[X], commandedThrust[X_INDEX], rollTime, roll); fprintf(fpw2, "%lf %f %f %f %f %f %lf %f\n", sharpsTime, sharpsPosition[Y], sharpsRate[Y], vehiclePosition[Y], vehicleRate[Y], commandedThrust[Y_INDEX], pitchTime, pitch); break; case ROT : /* read gyro and commanded heading thrust */ getGyro(fpw1, init, &recordDmi, &gyro, &gyroRate, &gyroTime); getThrustH(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f\n", gyroTime, gyro, gyroRate, commandedThrust[HEADING_INDEX]); break; case ROTPLUS : /* read gyro, pitch, roll and commanded heading thrust */ getGyro(fpw1, init, &recordDmi, &gyro, &gyroRate, &gyroTime); getPitch(fpw1, init, &recordDmi, &pitch, &pitchTime); getRoll(fpw1, init, &recordDmi, &roll, &rollTime); getThrustH(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f\n", gyroTime, gyro, gyroRate, commandedThrust[HEADING_INDEX]); fprintf(fpw2,"%lf %f %lf %f\n", pitchTime, pitch, rollTime, roll); break; case VERT : /* read depth and depth rate, altitude and altitude rate, accelerometer and */ /* commanded thrust */ getDepth(fpw1, init, &recordDmi, &depth, &filteredDepth, &depthRate, &depthTime); getAltitude(fpw1, init, &recordDmi, &altitude, &filteredAltitude, &altitudeRate, &altitudeTime); /* getAccelZ(fpw1, init, &recordDmi, accelerometer, &accelerometerTime); */ getThrustZ(init, &recordDmi, commandedThrust); /* fprintf(fpw1, "%lf %lf %f %lf %f %f %lf %d %f\n", depthTime, depth, depthRate, altitudeTime, altitude, altitudeRate, accelerometerTime, accelerometer[Z], commandedThrust[Z_INDEX]); */ fprintf(fpw1, "%lf %lf %f %f %lf %f %f %f\n", depthTime, depth, filteredDepth, depthRate, altitudeTime, altitude, altitudeRate, filteredAltitude, commandedThrust[Z_INDEX]); break; case VERTPLUS : /* read depth and depth rate, altitude and altitude rate, accelerometer, */ /* commanded thrust, and pitch and roll */ getDepth(fpw1, init, &recordDmi, &depth, &filteredDepth, &depthRate, &depthTime); getAltitude(fpw1, init, &recordDmi, &altitude, &filteredAltitude, &altitudeRate, &altitudeTime); /* getAccelZ(fpw1, init, &recordDmi, accelerometer, &accelerometerTime); */ getThrustZ(init, &recordDmi, commandedThrust); getPitch(fpw1, init, &recordDmi, &pitch, &pitchTime); getRoll(fpw1, init, &recordDmi, &roll, &rollTime); /* fprintf(fpw1, "%lf %f %f %f %lf %d %f %lf %f\n", sharpsTime, sharpsPosition[Z], sharpsRate[Z], accelerometerTime, accelerometer[Z], commandedThrust[Z_INDEX], pitchTime, pitch); */ fprintf(fpw1, "%lf %lf %f %lf %f %f %f\n", depthTime, depth, depthRate, altitudeTime, altitude, altitudeRate, commandedThrust[Z_INDEX]); fprintf(fpw2, "%lf %f %lf %f\n", pitchTime, pitch, rollTime, roll); break; case ADV : /* read ADV velocities */ getADV(fpw1, init, &recordDmi, advVelocity, &advTime); fprintf(fpw1, "%lf %f %f %f\n", advTime, advVelocity[X_INDEX], advVelocity[Y_INDEX], advVelocity[Z_INDEX]); break; case KALMAN : /* read Kalman filter heading and commanded heading thrust */ getKalman(fpw1, init, &recordDmi, &kalmanHeading, &kalmanRateHeading, &kalmanHeadingTime); getThrustH(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f\n", kalmanHeadingTime, kalmanHeading, kalmanRateHeading, commandedThrust[HEADING_INDEX]); break; case MANOEUVRE_TRANS: /* read x and y sharps positions and rates, rov transformed positions and */ /* rates, velocity command and commanded thrust */ getSharpsXY(fpw1, init, &recordDmi, sharpsPosition, sharpsRate, &sharpsTime); getRovXY(init, &recordDmi, vehiclePosition, vehicleRate); getVelocityCommandXY(init, &recordDmi, velocityCommand); getThrustXY(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f %f %f %f\n", sharpsTime, sharpsPosition[X], sharpsRate[X], vehiclePosition[X], vehicleRate[X], velocityCommand[X_INDEX], commandedThrust[X_INDEX]); fprintf(fpw2, "%lf %f %f %f %f %f %f\n", sharpsTime, sharpsPosition[Y], sharpsRate[Y], vehiclePosition[Y], vehicleRate[Y], velocityCommand[Y_INDEX], commandedThrust[Y_INDEX]); break; case MANOEUVRE_ROT : /* read heading sensor, velocity heading command and commanded heading thrust */ getHeading(fpw1, init, &recordDmi, &heading, &headingRate, &headingTime); getVelocityCommandH(init, &recordDmi, velocityCommand); getThrustH(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f %f\n", headingTime, heading, headingRate, velocityCommand[HEADING_INDEX], commandedThrust[HEADING_INDEX]); break; case MANOEUVRE_VERT : /* read altitude and altitude rate, velocity command and commanded thrust */ getAltitude(fpw1, init, &recordDmi, &altitude, &filteredAltitude, &altitudeRate, &altitudeTime); getVelocityCommandZ(init, &recordDmi, velocityCommand); getThrustZ(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f %f\n", altitudeTime, altitude, altitudeRate, velocityCommand[Z_INDEX], commandedThrust[Z_INDEX]); break; case SERVO_TRANS: /* read x and y sharps positions and rates, rov transformed positions and */ /* rates, setpoints and commanded thrust */ getSharpsXY(fpw1, init, &recordDmi, sharpsPosition, sharpsRate, &sharpsTime); getRovXY(init, &recordDmi, vehiclePosition, vehicleRate); getSetpointXY(init, &recordDmi, setpoint); getThrustXY(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f %f %f %f\n", sharpsTime, sharpsPosition[X], sharpsRate[X], vehiclePosition[X], vehicleRate[X], setpoint[X_INDEX], commandedThrust[X_INDEX]); fprintf(fpw2, "%lf %f %f %f %f %f %f\n", sharpsTime, sharpsPosition[Y], sharpsRate[Y], vehiclePosition[Y], vehicleRate[Y], setpoint[Y_INDEX], commandedThrust[Y_INDEX]); break; case SERVO_ROT : /* read heading, heading setpoint and commanded heading thrust */ getHeading(fpw1, init, &recordDmi, &heading, &headingRate, &headingTime); getSetpointH(init, &recordDmi, setpoint); getThrustH(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f %f\n", headingTime, heading, headingRate, setpoint[HEADING_INDEX], commandedThrust[HEADING_INDEX]); break; case SERVO_DEPTH : /* read depth, setpoint and commanded thrust */ getDepth(fpw1, init, &recordDmi, &depth, &filteredDepth, &depthRate, &depthTime); getSetpointZ(init, &recordDmi, setpoint); getZIntegral(init, &recordDmi, &zIntegral); getThrustZ(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %lf %f %f %f %f %f\n", depthTime, depth, filteredDepth, depthRate, setpoint[Z_INDEX], zIntegral, commandedThrust[Z_INDEX]); break; case SERVO_ALT : /* read altitude, setpoint and commanded thrust */ getAltitude(fpw1, init, &recordDmi, &altitude, &filteredAltitude, &altitudeRate, &altitudeTime); getSetpointZ(init, &recordDmi, setpoint); getThrustZ(init, &recordDmi, commandedThrust); fprintf(fpw1, "%lf %f %f %f %f %f\n", altitudeTime, altitude, filteredAltitude, altitudeRate, setpoint[Z_INDEX], commandedThrust[Z_INDEX]); break; case CONTROL : /* read joystick, velocity command, model thrust, control thrust and motor */ /* control thrust */ getJoystick(fpw1, init, &recordDmi, joystick, &joystickTime); getVelocityCommandXY(init, &recordDmi, velocityCommand); getVelocityCommandH(init, &recordDmi, velocityCommand); getModelThrust(init, &recordDmi, modelThrust); getThrustXY(init, &recordDmi, commandedThrust); getThrustH(init, &recordDmi, commandedThrust); getControlThrust(init, &recordDmi, controlThrust); getMotorControlThrust(init, &recordDmi, motorControlThrust); fprintf(fpw1, "%lf %d %d %f %f %f %f %f %f\n", joystickTime, joystick[X_INDEX], joystick[HEADING_INDEX], velocityCommand[X_INDEX], velocityCommand[HEADING_INDEX], modelThrust[X_INDEX], modelThrust[HEADING_INDEX], commandedThrust[X_INDEX], commandedThrust[HEADING_INDEX]); fprintf(fpw2, "%f %f %f %f %d %d %d %d\n", controlThrust[PORT_HORIZ], controlThrust[STBD_HORIZ], controlThrust[FWD_LATERAL], controlThrust[AFT_LATERAL], motorControlThrust[PORT_HORIZ], motorControlThrust[STBD_HORIZ], motorControlThrust[FWD_LATERAL], motorControlThrust[AFT_LATERAL]); break; } /* switch */ /* wait for wakeup semaphore */ semTake(recordSem, WAIT_FOREVER); if (init) init = FALSE; } /* FOREVER */ } /* recordTask */ /******************************************************************************/ /* Function : recordDMInit */ /* Purpose : Looks up the item handle and starts consumers for the data */ /* manager items being recorded. */ /* Inputs : Record data manager items structure pointer. */ /* Outputs : Returns data manager error code. */ /******************************************************************************/ LOCAL Errno recordDMInit(recordDMItems *recordDmi) { /* data manager sensor item name */ /* prefix */ char sensorDMPrefix[] = SENSOR_DM_PREFIX; /* data manager sensor item name */ char *sensorDMName; char *thrusterDMPrefix[] = /* data manager thruster item name */ { PORT_HORIZ_THRUSTER_DM, /* prefix */ STBD_HORIZ_THRUSTER_DM, FWD_LATERAL_THRUSTER_DM, AFT_LATERAL_THRUSTER_DM, PORT_VERT_THRUSTER_DM, STBD_VERT_THRUSTER_DM }; /* data manager thruster item name */ char *thrusterDMName; /* accelerometers item name */ MLocal char accelerometersName[MAX_NMLEN]; /* gyro degrees item name */ MLocal char gyroDegreesName[MAX_NMLEN]; /* gyro rate degrees item name */ MLocal char gyroRateDegreesName[MAX_NMLEN]; /* gyro radians item name */ MLocal char gyroRadiansName[MAX_NMLEN]; /* gyro rate radians item name */ MLocal char gyroRateRadiansName[MAX_NMLEN]; /* pitch degrees item name */ MLocal char pitchDegreesName[MAX_NMLEN]; /* roll degrees item name */ MLocal char rollDegreesName[MAX_NMLEN]; /* motor thrust item name */ MLocal char motorThrustName[NUM_THRUSTERS][MAX_NMLEN]; MBool stopRecording = FALSE; /* stop recording flag */ Int16 item; /* data manager item counter */ Int16 dof; /* dof counter */ Int16 thruster; /* thruster counter */ Errno status; /* data manager error code */ struct /* data manager items init values */ { char *name; /* data manager array name */ DM_Num item_num; /* item number */ DM_Item *item; /* data manager item handle */ } dmiInit[] = { { SHARPS_X_DM, 0, &recordDmi->sharpsX }, { SHARPS_Y_DM, 0, &recordDmi->sharpsY }, { SHARPS_RATE_X_DM, 0, &recordDmi->sharpsRateX }, { SHARPS_RATE_Y_DM, 0, &recordDmi->sharpsRateY }, /* { accelerometersName, 0, &recordDmi->accelerometers }, */ { gyroDegreesName, 0, &recordDmi->gyroDegrees }, { gyroRateDegreesName, 0, &recordDmi->gyroRateDegrees }, { gyroRadiansName, 0, &recordDmi->gyroRadians }, { gyroRateRadiansName, 0, &recordDmi->gyroRateRadians }, { pitchDegreesName, 0, &recordDmi->pitchDegrees }, { rollDegreesName, 0, &recordDmi->rollDegrees }, { VEHICLE_DEPTH_DM, 0, &recordDmi->depth }, { VEHICLE_ALTITUDE_DM, 0, &recordDmi->altitude }, { KALMAN_HEADING_DM, 0, &recordDmi->kalmanHeading }, { KALMAN_HEADING_RATE_DM, 0, &recordDmi->kalmanRateHeading }, { SENSOR_X_DM, 0, &recordDmi->sensorX }, { SENSOR_Y_DM, 0, &recordDmi->sensorY }, { SENSOR_DEPTH_DM, 0, &recordDmi->sensorDepth }, { SENSOR_ALTITUDE_DM, 0, &recordDmi->sensorAltitude }, { SENSOR_HEADING_DM, 0, &recordDmi->sensorHeading }, { SENSOR_RATE_X_DM, 0, &recordDmi->sensorRateX }, { SENSOR_RATE_Y_DM, 0, &recordDmi->sensorRateY }, { SENSOR_RATE_DEPTH_DM, 0, &recordDmi->depthRate}, { SENSOR_RATE_ALTITUDE_DM, 0, &recordDmi->altitudeRate}, { SENSOR_RATE_HEADING_DM, 0, &recordDmi->sensorRateHeading }, { SETPOINT_X_DM, 0, &recordDmi->setpointX }, { SETPOINT_Y_DM, 0, &recordDmi->setpointY }, { SETPOINT_Z_DM, 0, &recordDmi->setpointZ }, { SETPOINT_HEADING_DM, 0, &recordDmi->setpointHeading }, { Z_INTEGRAL_DM, 0, &recordDmi->zIntegral }, { JOYSTICK_X_VALUE_DM, 0, &recordDmi->joystickX }, { JOYSTICK_HEADING_VALUE_DM, 0, &recordDmi->joystickHeading }, { VELOCITY_X_COMMAND_DM, 0, &recordDmi->velocityCommandX }, { VELOCITY_Y_COMMAND_DM, 0, &recordDmi->velocityCommandY }, { VELOCITY_Z_COMMAND_DM, 0, &recordDmi->velocityCommandZ }, { VELOCITY_HEADING_COMMAND_DM, 0, &recordDmi->velocityCommandHeading }, { MODEL_X_THRUST_DM, 0, &recordDmi->modelThrustX }, { MODEL_HEADING_THRUST_DM, 0, &recordDmi->modelThrustHeading }, { motorThrustName[PORT_HORIZ], 0, &recordDmi->motorControlThrust[PORT_HORIZ] }, { motorThrustName[STBD_HORIZ], 0, &recordDmi->motorControlThrust[STBD_HORIZ] }, { motorThrustName[FWD_LATERAL], 0, &recordDmi->motorControlThrust[FWD_LATERAL] }, { motorThrustName[AFT_LATERAL], 0, &recordDmi->motorControlThrust[AFT_LATERAL] }, { motorThrustName[PORT_VERT], 0, &recordDmi->motorControlThrust[PORT_VERT] }, { motorThrustName[STBD_VERT], 0, &recordDmi->motorControlThrust[STBD_VERT] }, { NULL, 0, NO_ITEM } }; /* create sensor data manager item names */ /* sensorDMName = ACCELEROMETERS_DM; strcpy(accelerometersName, sensorDMPrefix); strcat(accelerometersName, sensorDMName); */ sensorDMName = GYRO_DEGREES_DM; strcpy(gyroDegreesName, sensorDMPrefix); strcat(gyroDegreesName, sensorDMName); sensorDMName = GYRO_RATE_DEGREES_DM; strcpy(gyroRateDegreesName, sensorDMPrefix); strcat(gyroRateDegreesName, sensorDMName); sensorDMName = GYRO_RADIANS_DM; strcpy(gyroRadiansName, sensorDMPrefix); strcat(gyroRadiansName, sensorDMName); sensorDMName = GYRO_RATE_RADIANS_DM; strcpy(gyroRateRadiansName, sensorDMPrefix); strcat(gyroRateRadiansName, sensorDMName); sensorDMName = PITCH_DEGREES_DM; strcpy(pitchDegreesName, sensorDMPrefix); strcat(pitchDegreesName, sensorDMName); sensorDMName = ROLL_DEGREES_DM; strcpy(rollDegreesName, sensorDMPrefix); strcat(rollDegreesName, sensorDMName); /* create motor control thrust data manager item name */ thrusterDMName = MOTOR_CMD_THRUST_DM; for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { strcpy(motorThrustName[thruster], thrusterDMPrefix[thruster]); strcat(motorThrustName[thruster], thrusterDMName); } /* lookup data manager item handles */ for (item = 0; dmiInit[item].name != NULL; item++) { *(dmiInit[item].item) = dm_lookup(dmiInit[item].name, dmiInit[item].item_num); if (checkLookup(*(dmiInit[item].item), dmiInit[item].name) == ERROR) return(ERROR); } for (dof = 0; dof < TRANS_DOF; dof++) { recordDmi->advVelocity[dof] = dm_lookup(DOPPLER_BOTTOM_TRACK_VELOCITY_DM, dof); if (checkLookup(recordDmi->advVelocity[dof], DOPPLER_BOTTOM_TRACK_VELOCITY_DM) == ERROR) return(ERROR); } for (dof = 0; dof < DOF; dof++) { recordDmi->commandedThrust[dof] = dm_lookup(COMMANDED_THRUST_DM, dof); if (checkLookup(recordDmi->commandedThrust[dof], COMMANDED_THRUST_DM) == ERROR) return(ERROR); } for (thruster = 0; thruster < NUM_THRUSTERS; thruster++) { recordDmi->controlThrust[thruster] = dm_lookup(CONTROL_THRUST_DM, thruster); if (checkLookup(recordDmi->controlThrust[thruster], CONTROL_THRUST_DM) == ERROR) return(ERROR); } /* start consumer items and check status */ for (item = 0; dmiInit[item].name != NULL; item++) { status = dm_start_consumer(*(dmiInit[item].item), RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, dmiInit[item].name, TRUE) == ERROR) return(ERROR); } status = dm_start_consumer(recordDmi->advVelocity[X_INDEX], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - doppler velocity x", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(recordDmi->advVelocity[Y_INDEX], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - doppler velocity y", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(recordDmi->advVelocity[Z_INDEX], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - doppler velocity z", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(recordDmi->commandedThrust[X_INDEX], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - commanded thrust x", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(recordDmi->commandedThrust[Y_INDEX], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - commanded thrust y", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(recordDmi->commandedThrust[Z_INDEX], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - commanded thrust z", TRUE) == ERROR) return(ERROR); status = dm_start_consumer(recordDmi->commandedThrust[HEADING_INDEX], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - commanded thrust heading", TRUE) == ERROR) return(ERROR); for (thruster = 0; thruster < NUM_THRUSTERS; thruster++) { status = dm_start_consumer(recordDmi->controlThrust[thruster], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, CONTROL_THRUST_DM, TRUE) == ERROR) return(ERROR); } for (thruster = 0; thruster < NUM_THRUSTERS; thruster++) { status = dm_start_consumer(recordDmi->motorControlThrust[thruster], RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, motorThrustName[thruster], TRUE) == ERROR) return(ERROR); } /* create data manager item to stop recording task */ status = dm_create(STOP_RECORDING_DM, 1, &recordDmi->stopRecording, DM_MBOOL, 1, DM_ENDT); /* start provider, write value and start consumer for stop recording flag */ status = dm_start_multiple(recordDmi->stopRecording); status = dm_write(recordDmi->stopRecording, (Void *) &stopRecording, sizeof(stopRecording), (DM_Time * ) NULL); if (checkWrite(status, "Data Recording - stop recording flag") == ERROR) return(ERROR); status = dm_start_consumer(recordDmi->stopRecording, RECORD_PERIOD, SEM_NULL); if (checkConsumer(status, "Data Recording - stop recording flag", TRUE) == ERROR) return(ERROR); return(OK); } /* recordDMInit */ /******************************************************************************/ /* Function : getSharpsXY */ /* Purpose : Gets SHARPS x and y position and rates. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, position pointer, rate pointer and time */ /* pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getSharpsXY(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 position[], Flt32 rate[], Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->sharpsX, (Void *) &position[X], sizeof(position[X]), &sensorTime); if (init) checkRead(status, "Data Recording - sharps x", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "sharps time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->sharpsY, (Void *) &position[Y], sizeof(position[Y]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - sharps y", TRUE); status = dm_read(recordDmi->sharpsRateX, (Void *) &rate[X], sizeof(rate[X]), (DM_Time * ) NULL); if (init) checkRead(status, "Date Recording - sharps x rate", TRUE); status = dm_read(recordDmi->sharpsRateY, (Void *) &rate[Y], sizeof(rate[Y]), (DM_Time * ) NULL); if (init) checkRead(status, "Date Recording - sharps y rate", TRUE); } /* getSharpsXY */ /******************************************************************************/ /* Function : getRovXY */ /* Purpose : Gets SHARPS x and y position and rates transformed to vehicle */ /* centroid. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer, position pointer, and rate pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getRovXY(MBool init, recordDMItems *recordDmi, Flt32 position[], Flt32 rate[]) { Errno status; status = dm_read(recordDmi->sensorX, (Void *) &position[X], sizeof(position[X]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - vehicle x position", TRUE); status = dm_read(recordDmi->sensorY, (Void *) &position[Y], sizeof(position[Y]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - vehicle y position", TRUE); status = dm_read(recordDmi->sensorRateX, (Void *) &rate[X], sizeof(rate[X]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - vehicle x rate", TRUE); status = dm_read(recordDmi->sensorRateY, (Void *) &rate[Y], sizeof(rate[Y]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - vehicle y rate", TRUE); } /* getRovXY */ /******************************************************************************/ /* Function : getAccelXY */ /* Purpose : Gets accelerometer x and y values. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, accelerometer pointer and time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getAccelXY(FILE *fpw, MBool init, recordDMItems *recordDmi, Int16 accelerometer[], Flt64 *accelerometerTime) { Int16 accel[TRANS_DOF]; Int16 i; DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->accelerometers, (Void *) &accel, sizeof(accel), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - accelerometers", TRUE); *accelerometerTime = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *accelerometerTime; fprintf(fpw, "accelerometer time offset %lf\n", timeOffset); } *accelerometerTime -= timeOffset; for (i = 0; i < TRANS_DOF; i++) accelerometer[i] = accel[i]; } /* getAccelXY */ /******************************************************************************/ /* Function : getAccelZ */ /* Purpose : Gets accelerometer z values. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, accelerometer pointer and time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getAccelZ(FILE *fpw, MBool init, recordDMItems *recordDmi, Int16 accelerometer[], Flt64 *accelerometerTime) { Int16 accel[TRANS_DOF]; Int16 i; DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->accelerometers, (Void *) &accel, sizeof(accel), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - accelerometers", TRUE); *accelerometerTime = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *accelerometerTime; fprintf(fpw, "accelerometer time offset %lf\n", timeOffset); } *accelerometerTime -= timeOffset; for (i = 0; i < TRANS_DOF; i++) accelerometer[i] = accel[i]; } /* getAccelZ */ /******************************************************************************/ /* Function : getGyro */ /* Purpose : Gets gyro and gyro rate values. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, gyro pointer, gyro rate pointer and time */ /* pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getGyro(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *gyro, Flt32 *gyroRate, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->gyroDegrees, (Void *) gyro, sizeof(*gyro), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recordin - gyro degrees", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "gyro time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->gyroRateDegrees, (Void *) gyroRate, sizeof(*gyroRate), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - gyro rate degrees", TRUE); } /* getGyro */ /******************************************************************************/ /* Function : getGyroRadians */ /* Purpose : Gets gyro and gyro rate values. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, gyro pointer, gyro rate pointer and time */ /* pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getGyroRadians(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *gyroRadians, Flt32 *gyroRateRadians, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->gyroRadians, (Void *) gyroRadians, sizeof(*gyroRadians), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recordin - gyro radians", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "gyro time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->gyroRateRadians, (Void *) gyroRateRadians, sizeof(*gyroRateRadians), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - gyro rate radians", TRUE); } /* getGyroRadians */ /******************************************************************************/ /* Function : getPitch */ /* Purpose : Gets pitch reading. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, pitch pointer and time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getPitch(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *pitch, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->pitchDegrees, (Void *) pitch, sizeof(*pitch), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - pitch", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "pitch time offset %lf\n", timeOffset); } *time -= timeOffset; } /* getPitch */ /******************************************************************************/ /* Function : getRoll */ /* Purpose : Gets roll reading. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, roll pointer and time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getRoll(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *roll, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->rollDegrees, (Void *) roll, sizeof(*roll), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - roll", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "roll time offset %lf\n", timeOffset); } *time -= timeOffset; } /* getRoll */ /******************************************************************************/ /* Function : getDepth */ /* Purpose : Gets depth reading. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, depth pointer, depth rate time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getDepth(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt64 *depth, Flt32 *sensorDepth, Flt32 *depthRate, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->depth, (Void *) depth, sizeof(*depth), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - depth", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "depth time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->sensorDepth, (Void *) sensorDepth, sizeof(*sensorDepth), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - filtered depth", TRUE); status = dm_read(recordDmi->depthRate, (Void *) depthRate, sizeof(*depthRate), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - depth rate", TRUE); } /* getDepth */ /******************************************************************************/ /* Function : getAltitude */ /* Purpose : Gets altitude and atlitude rate values. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, altitude pointer, altitude rate pointer and */ /* time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getAltitude(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *altitude, Flt32* filteredAltitude, Flt32 *altitudeRate, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->altitude, (Void *) altitude, sizeof(*altitude), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - altitude", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "altitude time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->sensorAltitude, (Void *) filteredAltitude, sizeof(*filteredAltitude), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - filtered altitude", TRUE); status = dm_read(recordDmi->altitudeRate, (Void *) altitudeRate, sizeof(*altitudeRate), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - altitude rate", TRUE); } /* getAltitude */ /******************************************************************************/ /* Function : getADV */ /* Purpose : Gets ADV velocity. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, ADV velocity pointer and time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getADV(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 advVelocity[], Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->advVelocity[X_INDEX], (Void *) &advVelocity[X_INDEX], sizeof(advVelocity[X_INDEX]), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recording - ADV x velocity", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "adv time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->advVelocity[Y_INDEX], (Void *) &advVelocity[Y_INDEX], sizeof(advVelocity[Y_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - ADV y velocity", TRUE); status = dm_read(recordDmi->advVelocity[Z_INDEX], (Void *) &advVelocity[Z_INDEX], sizeof(advVelocity[Z_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - ADV z velocity", TRUE); } /* getADV */ /******************************************************************************/ /* Function : getKalman */ /* Purpose : Gets Kalman filter heading and rate values. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, heading pointer, heading rate pointer and */ /* time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getKalman(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *heading, Flt32 *headingRate, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->kalmanHeading, (Void *) heading, sizeof(*heading), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recordin - Kalman filter heading", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "heading time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->kalmanRateHeading, (Void *) headingRate, sizeof(*headingRate), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - Kalman filter heading rate", TRUE); } /* getKalman */ /******************************************************************************/ /* Function : getHeading */ /* Purpose : Gets sensor heading and rate values. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, heading pointer, heading rate pointer and */ /* time pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getHeading(FILE *fpw, MBool init, recordDMItems *recordDmi, Flt32 *heading, Flt32 *headingRate, Flt64 *time) { DM_Time sensorTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->sensorHeading, (Void *) heading, sizeof(*heading), (DM_Time * ) &sensorTime); if (init) checkRead(status, "Data Recordin - sensor heading", TRUE); *time = (Flt64) sensorTime.tv_sec + (Flt64) sensorTime.tv_usec / 1000000.0; if (init) { timeOffset = *time; fprintf(fpw, "heading time offset %lf\n", timeOffset); } *time -= timeOffset; status = dm_read(recordDmi->sensorRateHeading, (Void *) headingRate, sizeof(*headingRate), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - sensor rate heading", TRUE); } /* getHeading */ /******************************************************************************/ /* Function : getSetpointXY */ /* Purpose : Gets x and y trajectory setpoints. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and setpoint pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getSetpointXY(MBool init, recordDMItems *recordDmi, Flt32 setpoint[]) { Errno status; status = dm_read(recordDmi->setpointX, (Void *) &setpoint[X_INDEX], sizeof(setpoint[X_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - setpoint x", TRUE); status = dm_read(recordDmi->setpointY, (Void *) &setpoint[Y_INDEX], sizeof(setpoint[Y_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - setpoint y", TRUE); } /* getSetpointXY */ /******************************************************************************/ /* Function : getSetpointZ */ /* Purpose : Gets z trajectory setpoint. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and setpoint pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getSetpointZ(MBool init, recordDMItems *recordDmi, Flt32 setpoint[]) { Errno status; status = dm_read(recordDmi->setpointZ, (Void *) &setpoint[Z_INDEX], sizeof(setpoint[Z_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - setpoint z", TRUE); } /* getSetpointZ */ /******************************************************************************/ /* Function : getSetpointH */ /* Purpose : Gets heading trajectory setpoint. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and setpoint pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getSetpointH(MBool init, recordDMItems *recordDmi, Flt32 setpoint[]) { Errno status; status = dm_read(recordDmi->setpointHeading, (Void *) &setpoint[HEADING_INDEX], sizeof(setpoint[HEADING_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - setpoint heading", TRUE); } /* getSetpointH*/ /******************************************************************************/ /* Function : getZIntegral */ /* Purpose : Gets z integration term. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and z integral pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getZIntegral(MBool init, recordDMItems *recordDmi, Flt32 *zIntegral) { Errno status; status = dm_read(recordDmi->zIntegral, (Void *) zIntegral, sizeof(*zIntegral), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - z integral", TRUE); } /* getZIntegral */ /******************************************************************************/ /* Function : getJoystick */ /* Purpose : Gets x and y joystick commanded velocity. */ /* Inputs : File pointer, initialization flag, record data manager items */ /* structure pointer, joystick pointer and time. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getJoystick(FILE *fpw, MBool init, recordDMItems *recordDmi, Int16 joystick[], Flt64 *joyTime) { DM_Time joystickTime; Errno status; static Flt64 timeOffset; status = dm_read(recordDmi->joystickX, (Void *) &joystick[X_INDEX], sizeof(joystick[X_INDEX]), (DM_Time * ) &joystickTime); if (init) checkRead(status, "Data Recording - joystick x", TRUE); *joyTime = (Flt64) joystickTime.tv_sec + (Flt64) joystickTime.tv_usec / 1000000.0; if (init) { timeOffset = *joyTime; fprintf(fpw, "joystick time offset %lf\n", timeOffset); } *joyTime -= timeOffset; status = dm_read(recordDmi->joystickHeading, (Void *) &joystick[HEADING_INDEX], sizeof(joystick[HEADING_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - joystick y", TRUE); } /* getJoystick */ /******************************************************************************/ /* Function : getVelocityCommandXY */ /* Purpose : Gets x and y joystick commanded velocity. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and velocity command pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getVelocityCommandXY(MBool init, recordDMItems *recordDmi, Flt32 velocityCommand[]) { Errno status; status = dm_read(recordDmi->velocityCommandX, (Void *) &velocityCommand[X_INDEX], sizeof(velocityCommand[X_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - velocity x command", TRUE); status = dm_read(recordDmi->velocityCommandY, (Void *) &velocityCommand[Y_INDEX], sizeof(velocityCommand[Y_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - velocity y command", TRUE); } /* getVelocityCommandXY */ /******************************************************************************/ /* Function : getVelocityCommandZ */ /* Purpose : Gets z joystick commanded velocity. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and velocity command pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getVelocityCommandZ(MBool init, recordDMItems *recordDmi, Flt32 velocityCommand[]) { Errno status; status = dm_read(recordDmi->velocityCommandZ, (Void *) &velocityCommand[Z_INDEX], sizeof(velocityCommand[Z_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - velocity z command", TRUE); } /* getVelocityCommandZ */ /******************************************************************************/ /* Function : getVelocityCommandH */ /* Purpose : Gets heading joystick commanded velocity. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and velocity command pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getVelocityCommandH(MBool init, recordDMItems *recordDmi, Flt32 velocityCommand[]) { Errno status; status = dm_read(recordDmi->velocityCommandHeading, (Void *) &velocityCommand[HEADING_INDEX], sizeof(velocityCommand[HEADING_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - velocity heading command", TRUE); } /* getVelocityCommandH*/ /******************************************************************************/ /* Function : getmodelThrust */ /* Purpose : Gets the dynamic model thrust. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and thrust pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getModelThrust(MBool init, recordDMItems *recordDmi, Flt32 modelThrust[]) { Errno status; status = dm_read(recordDmi->modelThrustX, (Void *) &modelThrust[X_INDEX], sizeof(modelThrust[X_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - model x thrust", TRUE); status = dm_read(recordDmi->modelThrustHeading, (Void *) &modelThrust[HEADING_INDEX], sizeof(modelThrust[HEADING_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - model heading thrust", TRUE); } /* getModelThrust */ /******************************************************************************/ /* Function : getThrustXY */ /* Purpose : Gets x and y commanded thrust. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and thrust pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getThrustXY(MBool init, recordDMItems *recordDmi, Flt32 thrust[]) { Errno status; status = dm_read(recordDmi->commandedThrust[X_INDEX], (Void *) &thrust[X_INDEX], sizeof(thrust[X_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - commanded x thrust", TRUE); status = dm_read(recordDmi->commandedThrust[Y_INDEX], (Void *) &thrust[Y_INDEX], sizeof(thrust[Y_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - commanded y thrust", TRUE); } /* getThrustXY */ /******************************************************************************/ /* Function : getThrustZ */ /* Purpose : Gets z commanded thrust. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and thrust pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getThrustZ(MBool init, recordDMItems *recordDmi, Flt32 thrust[]) { Errno status; status = dm_read(recordDmi->commandedThrust[Z_INDEX], (Void *) &thrust[Z_INDEX], sizeof(thrust[Z_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - commanded z thrust", TRUE); } /* getThrustZ */ /******************************************************************************/ /* Function : getThrustH */ /* Purpose : Gets heading commanded thrust. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and thrust pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getThrustH(MBool init, recordDMItems *recordDmi, Flt32 thrust[]) { Errno status; status = dm_read(recordDmi->commandedThrust[HEADING_INDEX], (Void *) &thrust[HEADING_INDEX], sizeof(thrust[HEADING_INDEX]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - commanded heading thrust", TRUE); } /* getThrustH */ /******************************************************************************/ /* Function : getControlThrust */ /* Purpose : Gets the control thrust. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and thrust pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getControlThrust(MBool init, recordDMItems *recordDmi, Flt32 thrust[]) { Errno status; status = dm_read(recordDmi->controlThrust[PORT_HORIZ], (Void *) &thrust[PORT_HORIZ], sizeof(thrust[PORT_HORIZ]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - port horiz control thrust", TRUE); status = dm_read(recordDmi->controlThrust[STBD_HORIZ], (Void *) &thrust[STBD_HORIZ], sizeof(thrust[STBD_HORIZ]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - stbd horiz control thrust", TRUE); status = dm_read(recordDmi->controlThrust[FWD_LATERAL], (Void *) &thrust[FWD_LATERAL], sizeof(thrust[FWD_LATERAL]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - fwd lat control thrust", TRUE); status = dm_read(recordDmi->controlThrust[AFT_LATERAL], (Void *) &thrust[AFT_LATERAL], sizeof(thrust[AFT_LATERAL]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - aft lat control thrust", TRUE); } /* getControlThrust */ /******************************************************************************/ /* Function : getMotorControlThrust */ /* Purpose : Gets the motor control thrust. */ /* Inputs : Initialization flag, record data manager items structure */ /* pointer and thrust pointer. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getMotorControlThrust(MBool init, recordDMItems *recordDmi, Int16 thrust[]) { Errno status; status = dm_read(recordDmi->motorControlThrust[PORT_HORIZ], (Void *) &thrust[PORT_HORIZ], sizeof(thrust[PORT_HORIZ]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - port horiz motor thrust", TRUE); status = dm_read(recordDmi->motorControlThrust[STBD_HORIZ], (Void *) &thrust[STBD_HORIZ], sizeof(thrust[STBD_HORIZ]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - stbd horiz motor thrust", TRUE); status = dm_read(recordDmi->motorControlThrust[FWD_LATERAL], (Void *) &thrust[FWD_LATERAL], sizeof(thrust[FWD_LATERAL]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - fwd lat motor thrust", TRUE); status = dm_read(recordDmi->motorControlThrust[AFT_LATERAL], (Void *) &thrust[AFT_LATERAL], sizeof(thrust[AFT_LATERAL]), (DM_Time * ) NULL); if (init) checkRead(status, "Data Recording - aft lat motor thrust", TRUE); } /* getMotorControlThrust */