/******************************************************************************/ /* Copyright 1997 MBARI */ /******************************************************************************/ /* Summary : Acoustic Doppler Velocimeter Distance Logger */ /* Filename : advDistanceLog.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 03/04/97 */ /* Modified : */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: advDistanceLog.c,v 1.1 97/11/20 09:13:25 oreilly Exp $ * $Log: advDistanceLog.c,v $ * Revision 1.1 97/11/20 09:13:25 09:13:25 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /******************************************************************************/ #include /* VxWorks system declarations */ #include /* VxWorks standard I/O library */ #include /* VxWorks semaphore library */ #include /* MBARI style guide type declarations */ #include /* MBARI general purpose constants */ #include /* MBARI time declarations */ #include /* data manager declarations */ #include /* data manager error declarations */ #include /* doppler data manager definitions */ #define NX 3 /* number of cartesian dof */ #define X_INDEX 0 /* x velocity index */ typedef struct /* data manager item handles */ { DM_Item velocity[NX]; /* doppler velocities */ DM_Item start; /* start ADV distance log */ DM_Item stop; /* stop ADV distance log */ DM_Item reset; /* reset ADV distance log */ DM_Item distance; /* distance logged */ DM_Item distanceTotal; /* total of distances logged */ } advDMItems; typedef struct /* group items id bits */ { DWord velocity; /* doppler velocity bit */ DWord start; /* start ADV distance log bit */ DWord stop; /* stop ADV distance log bit */ DWord reset; /* reset ADV distance log bit */ } advIdBits; /* function prototypes */ #if __STDC__ Errno advDMInit(advDMItems *advDmi, DM_Group *initGroup, DM_Group *logGroup, advIdBits *advBits, SEM_ID initSem, SEM_ID logSem); #endif /******************************************************************************/ /* Function : advDistanceLogTask */ /* Purpose : Integrates the Acoustic Doppler Velocimeter (ADV) forward */ /* velocity to determine the distance travelled. */ /* Inputs : None. */ /* Outputs : Returns ERROR on fatal error. */ /******************************************************************************/ STATUS advDistanceLogTask(void) { advDMItems advDmi; /* ADV data manager items */ SEM_ID advInitSem; /* task wakeup semaphore */ SEM_ID logSem; /* data available semaphore */ DM_Group initGroup; /* ADV init log data manager group */ DM_Group logGroup; /* ADV log data manager group */ DWord initGroupBits; /* ADV init group changes bit vector */ DWord logGroupBits; /* ADV log group changes bit vector */ advIdBits advBits; /* ADV id bits */ MBool start; /* start distance log flag */ MBool stop; /* stop distance log flag */ MBool reset; /* reset distance log flag */ MBool logging; /* logging distance flag */ MBool initialized = FALSE; /* logger initialized flag */ Flt32 velocity; /* forward vehicle velocity */ Flt32 prevVelocity; /* previous velocity */ Flt32 dd; /* delta distance */ Flt32 distance; /* distance logged */ Flt32 distanceTotal = 0.0; /* total of distances logged */ DM_Time sampleTime; /* dm item sample time */ Flt64 currentTime; /* current velocity sample time */ Flt64 prevTime; /* previous velocity sample time */ /* initialize task wakeup and data logging semaphores */ if ((advInitSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("ADV Distance Log FAILURE :\n could not initialize wakeup semaphore\n"); return(ERROR); } if ((logSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("ADV Distance Log FAILURE :\n could not initialize logging semaphores\n"); return(ERROR); } /* create data manager items and start providers */ if (advDMInit(&advDmi, &initGroup, &logGroup, &advBits, advInitSem, logSem) == ERROR) return(ERROR); for (;;) { /* wait for start or reset of logger */ semTake(advInitSem, WAIT_FOREVER); /* determine which item was written */ initGroupBits = dm_get_group_changes(initGroup); /* reset distance logger */ if (initGroupBits & advBits.reset) { dm_read(advDmi.reset, (Void *) &reset, sizeof(reset), (DM_Time *) NULL); if (reset) /* reset distance and total */ { distance = 0.0; distanceTotal = 0.0; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(advDmi.distance, (Void *) &distance, sizeof(distance), &sampleTime); dm_write(advDmi.distanceTotal, (Void *) &distanceTotal, sizeof(distanceTotal), &sampleTime); } } /* advBits.reset */ /* start distance logger */ if (initGroupBits & advBits.start) { dm_read(advDmi.start, (Void *) &start, sizeof(start), (DM_Time *) NULL); if (start) { distance = 0.0; logging = TRUE; while (logging) { /* wait for velocity data or logger stop */ semTake(logSem, WAIT_FOREVER); /* determine which item was written */ logGroupBits = dm_get_group_changes(logGroup); /* read velocity, calculate distance and update item */ if (logGroupBits & advBits.velocity) { dm_read(advDmi.velocity[X_INDEX], (Void *) &velocity, sizeof(velocity), (DM_Time *) &sampleTime); currentTime = (Flt64) sampleTime.tv_sec + (Flt64) sampleTime.tv_usec / 1000000.0; if (initialized) { dd = 0.5 * (velocity + prevVelocity) * (currentTime - prevTime); distance += dd; distanceTotal += dd; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(advDmi.distance, (Void *) &distance, sizeof(distance), &sampleTime); dm_write(advDmi.distanceTotal, (Void *) &distanceTotal, sizeof(distanceTotal), &sampleTime); } else initialized = TRUE; prevVelocity = velocity; prevTime = currentTime; } /* advBits.velocity */ /* stop data logger */ if (logGroupBits & advBits.stop) { dm_read(advDmi.stop, (Void *) &stop, sizeof(stop), (DM_Time *) NULL); if (stop) /* update distance total */ { logging = FALSE; initialized = FALSE; } } /* advBits.stop */ } /* while (logging) */ } /* if (start) */ } /* advBits.start */ } /* for (;;) */ } /* advDistanceLogTask */ /******************************************************************************/ /* Function : advDMInit */ /* Purpose : Creates data manager items, starts consumers and providers, */ /* creates groups and checks status. */ /* Inputs : ADV data manager items, init group, log group, group id bits and*/ /* init and log semaphores. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Errno advDMInit(advDMItems *advDmi, DM_Group *initGroup, DM_Group *logGroup, advIdBits *advBits, SEM_ID initSem, SEM_ID logSem) { Int16 item; /* data manager item counter */ Errno status; /* error code */ struct /* data manager items created */ { char *name; /* data manager array name */ DM_Type type; /* data manager item type */ DM_Num num; /* number of data manager items */ DM_Item *item; /* data manager item handle */ } dmiCreate[] = { { DOPPLER_WATER_MASS_VELOCITY_DM, DM_FLT32, NX, advDmi->velocity }, { DOPPLER_START_DISTANCE_LOG_DM, DM_MBOOL, 1, &advDmi->start }, { DOPPLER_STOP_DISTANCE_LOG_DM, DM_MBOOL, 1, &advDmi->stop }, { DOPPLER_RESET_DISTANCE_LOG_DM, DM_MBOOL, 1, &advDmi->reset }, { DOPPLER_DISTANCE_DM, DM_FLT32, 1, &advDmi->distance }, { DOPPLER_DISTANCE_TOTAL_DM, DM_FLT32, 1, &advDmi->distanceTotal }, { NULL, DM_ENDT, 0, NO_ITEM } }; /* create data manager items and check status */ for (item = 0; dmiCreate[item].item != NO_ITEM; item++) { status = dm_create(dmiCreate[item].name, dmiCreate[item].num, dmiCreate[item].item, dmiCreate[item].type, 1, DM_ENDT); if (checkCreate(status, dmiCreate[item].name, FALSE) == ERROR) return(ERROR); } /* start consumer items and check status */ status = dm_start_consumer(advDmi->velocity[X_INDEX], DM_ASYNC, logSem); if (checkConsumer(status, "ADV Distance Log - x velocity") == ERROR) return(ERROR); status = dm_start_consumer(advDmi->start, DM_ASYNC, initSem); if (checkConsumer(status, "ADV Distance Log - start log") == ERROR) return(ERROR); status = dm_start_consumer(advDmi->stop, DM_ASYNC, logSem); if (checkConsumer(status, "ADV Distance Log - stop log") == ERROR) return(ERROR); status = dm_start_consumer(advDmi->reset, DM_ASYNC, initSem); if (checkConsumer(status, "ADV Distance Log - reset log") == ERROR) return(ERROR); /* create data manager groups */ *initGroup = dm_create_group(); *logGroup = dm_create_group(); /* add data manager items to groups */ status = dm_group_add_item(*initGroup, advDmi->start, &advBits->start); if (checkGroup(status, "ADV Distance Log - start log") == ERROR) return(ERROR); status = dm_group_add_item(*initGroup, advDmi->reset, &advBits->reset); if (checkGroup(status, "ADV Distance Log - reset log") == ERROR) return(ERROR); status = dm_group_add_item(*logGroup, advDmi->velocity[X_INDEX], &advBits->velocity); if (checkGroup(status, "ADV Distance Log - x velocity") == ERROR) return(ERROR); status = dm_group_add_item(*logGroup, advDmi->stop, &advBits->stop); if (checkGroup(status, "ADV Distance Log - stop log") == ERROR) return(ERROR); /* start provider items and check status */ status = dm_start_provider(advDmi->distance, DM_ASYNC); if (checkProvider(status, "ADV Distance Log - distance") == ERROR) return(ERROR); status = dm_start_provider(advDmi->distanceTotal, DM_ASYNC); if (checkProvider(status, "ADV Distance Log - distance total") == ERROR) return(ERROR); } /* advDMInit */