/*****************************************************************************/ /* Copyright 1997 MBARI */ /*****************************************************************************/ /* Summary : Vehicle turns information logger. */ /* Filename : vehicleTurnsLog.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 2.0 */ /* Created : 02/28/97 */ /* Modified : */ /* Archived : */ /*****************************************************************************/ /* Modification History : */ /* $Header: vehicleTurnsLog.c,v 1.1 97/12/05 08:54:22 oreilly Exp $ * $Log: vehicleTurnsLog.c,v $ * Revision 1.1 97/12/05 08:54:22 08:54:22 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /*****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks standard library */ #include /* vxWorks standard I/O library */ #include /* vxWorks semaphore library */ #include /* vxWorks message logging declarations */ #include /* vxWorks string library declarations */ #include /* MBARI style guide type declarations */ #include /* MBARI time declarations */ #include /* data manager declarations */ #include /* data manager error declarations */ #include /* Tiburon vehicle definitions */ #include /* vehicle turns data manager item names */ extern "C" { #include } /* turns data manager item handles */ typedef struct { DM_Item vehicleAlive; /* vehicle alive flag */ DM_Item count; /* vehicle turns count */ DM_Item sum; /* vehicle turns sum */ DM_Item setpoint; /* vehicle turns setpoint */ DM_Item reset; /* vehicle turns counter reset */ DM_Item stopLog; /* stop data logging flag */ } turnsDMItems; /* turns group id bits */ typedef struct { DWord vehicleAliveBit; /* vehicle alive bit */ DWord countBit; /* turns count bit */ DWord sumBit; /* turns sum bit */ DWord setpointBit; /* turns setpoint bit */ DWord resetBit; /* turns counter reset bit */ DWord stopLogBit; /* stop data logging bit */ } turnsIdBits; /*****************************************************************************/ /* Function : vehicleTurnsLogTask */ /* Purpose : Logs vehcle turns information. */ /* Inputs : None. */ /* Outputs : Returns ERROR on fatal error. */ /*****************************************************************************/ STATUS vehicleTurnsLogTask(void) { FILE *turnsLogFile; /* vehicle turns log file pointer */ fpos_t setpointPosition; /* setpoint position in file */ fpos_t countPosition; /* count position in file */ fpos_t sumPosition; /* sum position in file */ const fpos_t *setpointPositionPtr = &setpointPosition; /* pointer to setpoint position */ const fpos_t *countPositionPtr = &countPosition; /* pointer to count position */ const fpos_t *sumPositionPtr = &sumPosition; /* pointer to sum position */ SEM_ID turnsSem; /* task wakeup semaphore */ turnsDMItems turnsDmi; /* data manager item handles */ DM_Group turnsGroup; /* turns group */ DWord turnsGroupBits; /* turns group changes */ turnsIdBits turnsId; /* turns id bits */ /* tiburon data manager name prefix */ char tiburonDMPrefix[] = TIBURON_DM_PREFIX; /* vehicle alive name suffix */ char vehicleAliveDM[] = TIBURON_ALIVE_DM; char vehicleAliveName[MAX_NMLEN];/* vehicle alive item name */ MBool vehicleAlive = FALSE; /* vehicle alive flag */ MBool resetTurnsCounter = FALSE;/* vehicle turns counter reset flg*/ MBool stopTurnsLog = FALSE; /* stop data logging flag */ Int16 turnsCount; /* vehicle turns count */ Int16 sumCounter = 0; /* turns sum update counter */ Flt32 turnsSum; /* vehicle turns sum */ Flt32 turnsSetpoint; /* vehicle turns setpoint */ Errno status; /* data manager error code */ /* initialize task wakeup semaphore */ if ((turnsSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Vehicle Turns Log FAILURE:\ncould not initialize semaphore\n"); exit(0); } /* create vehicle alive data manager item name */ strcpy(vehicleAliveName, tiburonDMPrefix); strcat(vehicleAliveName, vehicleAliveDM); /* create data manager items */ status = dm_create(vehicleAliveName, 1, &turnsDmi.vehicleAlive, DM_MBOOL, 1, DM_ENDT); if (checkCreate(status, "Vehicle Turns Log - vehicle alive flag", FALSE) == ERROR) exit(0); status = dm_create(VEHICLE_TURNS_COUNT_DM, 1, &turnsDmi.count, DM_INT16, 1, DM_ENDT); if (checkCreate(status, "Vehicle Turns Log - turns count", FALSE) == ERROR) exit(0); status = dm_create(VEHICLE_TURNS_SUM_DM, 1, &turnsDmi.sum, DM_FLT32, 1, DM_ENDT); if (checkCreate(status, "Vehicle Turns Log - turns sum", FALSE) == ERROR) exit(0); status = dm_create(VEHICLE_TURNS_SETPOINT_DM, 1, &turnsDmi.setpoint, DM_FLT32, 1, DM_ENDT); if (checkCreate(status, "Vehicle Turns Log - turns setpoint", FALSE) == ERROR) exit(0); status = dm_create(RESET_VEHICLE_TURNS_COUNT_DM, 1, &turnsDmi.reset, DM_MBOOL, 1, DM_ENDT); if (checkCreate(status, "Vehicle Turns Log - reset turns counter", FALSE) == ERROR) exit(0); status = dm_create(VEHICLE_TURNS_LOG_STOP_DM, 1, &turnsDmi.stopLog, DM_MBOOL, 1, DM_ENDT); if (checkCreate(status, "Vehicle Turns Log - stop data logging flag", FALSE) == ERROR) exit(0); /* start consumer items and check status */ status = dm_start_consumer(turnsDmi.vehicleAlive, DM_STATIC, turnsSem); if (checkConsumer(status, "Vehicle Turns Log - vehicle alive flag", FALSE) == ERROR) exit(0); status = dm_start_consumer(turnsDmi.count, DM_STATIC, turnsSem); if (checkConsumer(status, "Vehicle Turns Log - turns count", FALSE) == ERROR) exit(0); status = dm_start_consumer(turnsDmi.sum, DM_ASYNC, turnsSem); if (checkConsumer(status, "Vehicle Turns Log - turns sum", FALSE) == ERROR) exit(0); status = dm_start_consumer(turnsDmi.setpoint, DM_STATIC, turnsSem); if (checkConsumer(status, "Vehicle Turns Log - turns setpoint", FALSE) == ERROR) exit(0); status = dm_start_consumer(turnsDmi.reset, DM_STATIC, turnsSem); if (checkConsumer(status, "Vehicle Turns Log - reset turns counter", FALSE) == ERROR) exit(0); status = dm_start_consumer(turnsDmi.stopLog, DM_STATIC, turnsSem); if (checkConsumer(status, "Vehicle Turns Log - stop data logging flag", FALSE) == ERROR) exit(0); /* create data manager group */ turnsGroup = dm_create_group(); /* add data manager items to the group */ status = dm_group_add_item(turnsGroup, turnsDmi.vehicleAlive, &turnsId.vehicleAliveBit); checkGroup(status, "Vehicle Turns Log - vehicle alive bit"); status = dm_group_add_item(turnsGroup, turnsDmi.count, &turnsId.countBit); checkGroup(status, "Vehicle Turns Log - turns count bit"); status = dm_group_add_item(turnsGroup, turnsDmi.sum, &turnsId.sumBit); checkGroup(status, "Vehicle Turns Log - turns sum bit"); status = dm_group_add_item(turnsGroup, turnsDmi.setpoint, &turnsId.setpointBit); checkGroup(status, "Vehicle Turns Log - turns setpoint bit"); status = dm_group_add_item(turnsGroup, turnsDmi.reset, &turnsId.resetBit); checkGroup(status, "Vehicle Turns Log - reset turns counter bit"); status = dm_group_add_item(turnsGroup, turnsDmi.stopLog, &turnsId.stopLogBit); checkGroup(status, "Vehicle Turns Log - stop data logging bit"); /* determine vehicle alive status */ dm_read(turnsDmi.vehicleAlive, (Void *) &vehicleAlive, sizeof(vehicleAlive), (DM_Time * ) NULL); if (vehicleAlive) { if ((turnsLogFile = fopen(TURNS_COUNTER_LOG, "r+")) != NULL) { fscanf(turnsLogFile, "%*s%*s\n"); fgetpos(turnsLogFile, &setpointPosition); fscanf(turnsLogFile, "%*s%f%*s\n", &turnsSetpoint); fgetpos(turnsLogFile, &countPosition); fscanf(turnsLogFile, "%*s%hd\n", &turnsCount); fgetpos(turnsLogFile, &sumPosition); fscanf(turnsLogFile, "%*s%f%*s\n", &turnsSum); logMsg("Vehicle Turns Log : setpoint %f count %d sum %f\n", turnsSetpoint, turnsCount, turnsSum); } else logMsg("Vehicle Turns Log : Could not open file %s\n", TURNS_COUNTER_LOG); } for (;;) { /* wait for change in vehicle turns values or vehicle alive flag */ semTake(turnsSem, WAIT_FOREVER); /* determine which value changed */ turnsGroupBits = dm_get_group_changes(turnsGroup); /* read vehicle alive status */ if (turnsGroupBits & turnsId.vehicleAliveBit) { dm_read(turnsDmi.vehicleAlive, (Void *) &vehicleAlive, sizeof(vehicleAlive), (DM_Time * ) NULL); if (vehicleAlive) /* open file for logging */ { if ((turnsLogFile = fopen(TURNS_COUNTER_LOG, "r+")) != NULL) { fscanf(turnsLogFile, "%*s%*s\n"); fgetpos(turnsLogFile, &setpointPosition); fscanf(turnsLogFile, "%*s%f%*s\n", &turnsSetpoint); fgetpos(turnsLogFile, &countPosition); fscanf(turnsLogFile, "%*s%hd\n", &turnsCount); fgetpos(turnsLogFile, &sumPosition); fscanf(turnsLogFile, "%*s%f%*s\n", &turnsSum); logMsg("Vehicle Turns Log : setpoint %f count %d sum %f\n", turnsSetpoint, turnsCount, turnsSum); } else logMsg("Vehicle Turns Log : Could not open file %s\n", TURNS_COUNTER_LOG); } else /* log last turns sum and close file */ { fsetpos(turnsLogFile, sumPositionPtr); fprintf(turnsLogFile, "sum %9.4f radians", turnsSum); fclose(turnsLogFile); } } /* read vehicle turns count */ if (turnsGroupBits & turnsId.countBit) { if (vehicleAlive) { dm_read(turnsDmi.count, (Void *) &turnsCount, sizeof(turnsCount), (DM_Time * ) NULL); fsetpos(turnsLogFile, countPositionPtr); fprintf(turnsLogFile, "count %4d\n", turnsCount); fflush(turnsLogFile); /* close file to flush stream because fflush doesn't work, then reopen file */ fclose(turnsLogFile); turnsLogFile = fopen(TURNS_COUNTER_LOG, "r+"); } } /* read vehicle turns sum */ if (turnsGroupBits & turnsId.sumBit) { if (vehicleAlive) { dm_read(turnsDmi.sum, (Void *) &turnsSum, sizeof(turnsSum), (DM_Time * ) NULL); /* vehicle turns sum updates at approximately 10 Hz, log every minute */ sumCounter++; if (sumCounter == 600) { fsetpos(turnsLogFile, sumPositionPtr); fprintf(turnsLogFile, "sum %9.4f radians", turnsSum); fflush(turnsLogFile); /* close file to flush stream because fflush doesn't work, then reopen file */ fclose(turnsLogFile); turnsLogFile = fopen(TURNS_COUNTER_LOG, "r+"); sumCounter = 0; } } } /* read vehicle turns setpoint */ if (turnsGroupBits & turnsId.setpointBit) { if (vehicleAlive) { dm_read(turnsDmi.setpoint, (Void *) &turnsSetpoint, sizeof(turnsSetpoint), (DM_Time * ) NULL); fsetpos(turnsLogFile, setpointPositionPtr); fprintf(turnsLogFile, "setpoint %7.4f radians\n", turnsSetpoint); fflush(turnsLogFile); /* close file to flush stream because fflush doesn't work, then reopen file */ fclose(turnsLogFile); turnsLogFile = fopen(TURNS_COUNTER_LOG, "r+"); } } /* read vehicle reset turns counter */ if (turnsGroupBits & turnsId.resetBit) { if (vehicleAlive) { dm_read(turnsDmi.reset, (Void *) &resetTurnsCounter, sizeof(resetTurnsCounter), (DM_Time * ) NULL); if (resetTurnsCounter) /* log last turns sum */ { dm_read(turnsDmi.sum, (Void *) &turnsSum, sizeof(turnsSum), (DM_Time * ) NULL); fsetpos(turnsLogFile, sumPositionPtr); fprintf(turnsLogFile, "sum %9.4f radians", turnsSum); fflush(turnsLogFile); /* close file to flush stream because fflush doesn't work, then reopen file */ fclose(turnsLogFile); turnsLogFile = fopen(TURNS_COUNTER_LOG, "r+"); } } } /* read stop data logging flag */ if (turnsGroupBits & turnsId.stopLogBit) { dm_read(turnsDmi.stopLog, (Void *) &stopTurnsLog, sizeof(stopTurnsLog), (DM_Time * ) NULL); if (stopTurnsLog) /* log last turns sum and close file */ { if (turnsLogFile != NULL) /* file may be already closed*/ { fsetpos(turnsLogFile, sumPositionPtr); fprintf(turnsLogFile, "sum %9.4f radians", turnsSum); fclose(turnsLogFile); } exit(0); } } } /* for (;;) */ }