/******************************************************************************/ /* Copyright 1994 MBARI */ /******************************************************************************/ /* Summary : RDI Acoustic Doppler Velocimeter communication module */ /* Filename : doppler.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 11/22/94 */ /* Modified : */ /* 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 message queue library */ #include /* VxWorks system time declarations */ #include /* VxWorks string 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 /* server and microcontroller protocol */ #include /* microcontroller command communications */ #include /* doppler data manager definitions */ #include /* Quad Serial Definitions */ #include /* VSP function prototypes */ #include "ibc_card.h" /* IBC board definitions */ #include "doppler.h" /* doppler definitions */ #define DOPPLER_PRIORITY 70 /* doppler task priority */ #define DOPPLER_PERIOD 50000 /* doppler task period (microsec) */ /* (20 Hz rate) */ #define DOPPLER_BREAK 300000 /* doppler break duration (microsec) */ #define RESPONSE_DELAY 10000 /* doppler response delay (microsec) */ #define NUM_RESPONSE_WAITS 100 /* number of times to wait for a */ /* response (1 second duration) */ #define DOPPLER_BUFFER_SIZE 94 /* ADV serial data buffer size */ /* 47 bytes = 94 hex chars */ #define STRING_LENGTH 20 /* string length */ #define NX 3 /* number of cartesian dof */ #define NBYTES 2 /* number of bytes */ #define MM_TO_METRES 0.001 /* mm to meters conversion */ #define DOPPLER_LSB 0 /* least significant byte */ #define DOPPLER_MSB 1 /* most significant byte */ #define NUM_BEAMS 4 /* number of beams */ #define BEAM_OK 0 /* bottom-referenced correlation and */ #define BEAM1_CORRELATION 1 /* echo amplitude status */ #define BEAM1_ECHO_AMPLITUDE 2 #define BEAM2_CORRELATION 4 #define BEAM2_ECHO_AMPLITUDE 8 #define BEAM3_CORRELATION 16 #define BEAM3_ECHO_AMPLITUDE 32 #define BEAM4_CORRELATION 64 #define BEAM4_ECHO_AMPLITUDE 128 #define BAD_VELOCITY -32768 /* bad velocity value returned */ #define BAD_X_VELOCITY 1 /* bad x velocity */ #define BAD_Y_VELOCITY 2 /* bad y velocity */ #define BAD_Z_VELOCITY 4 /* bad z velocity */ /******************************************************************************/ #define SERIAL_DELAY 10000 /* serial delay (microsec) */ #define NUM_SERIAL_WAITS 3 /* number of times to wait for serial */ /* port (30 millisecond duration) */ /* IBC Quad Serial Board Address */ #define ADV_BOARD_ADDRESS (Word) QUAD_SERIAL_0_ADDR /******************************************************************************/ /* cartesian degrees of freedom */ typedef enum { X_INDEX, Y_INDEX, Z_INDEX } cartDof; /* data manager item handles */ typedef struct { DM_Item velocity[NX]; /* doppler velocities */ DM_Item dataValid[NX]; /* valid velocity data flag */ DM_Item bottomTrackMode; /* bottom track mode */ DM_Item timeBetweenPings; /* min time between ping groups */ DM_Item timePerEnsemble; /* min interval between data ensembles*/ DM_Item powerDown; /* power down ADV */ } dopplerDMItems; /* adjustable parameters group id bits*/ typedef struct { DWord bottomTrackModeBit; /* bottom track mode bit */ DWord timeBetweenPingsBit; /* time between ping groups bit */ DWord timePerEnsembleBit; /* interval between data ensembles bit*/ DWord powerDownBit; /* power down ADV */ } dopplerIdBits; /* function prototypes */ #if __STDC__ Errno dopplerDMInit(dopplerDMItems *dopplerDmi, DM_Group *dopplerGroup, dopplerIdBits *dopplerId); Int16 initDoppler(sio32Chan *serialChannelPtr); Int16 wakeUpDoppler(sio32Chan *serialChannelPtr, Word boardAddress, Word channelNumber); Int16 sendCommand(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes, char *errorMsg); Int16 getResponse(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes, char *errorMsg); Byte getVelocity(sio32Chan *serialChannelPtr, Flt32 dopplerVelocity[]); Int16 mmToMetres(Int32 xVelocity[], Int32 yVelocity[], Int32 zVelocity[], Flt32 velocity[]); Void setBottomTrackMode(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr); Void setTimeBetweenPings(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr); Void setTimePerEnsemble(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr); Void powerDownDoppler(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr); STATUS initAdvSerial(sio32Chan *serialChannelPtr); Void closeAdvSerial(sio32Chan *serialChannelPtr); STATUS getAdvSerialData(sio32Chan *serialChannelPtr, Byte *buffer, Int16 numBytes); #endif /******************************************************************************/ /* Function : dopplerTask */ /* Purpose : Gets Acoustic Doppler Velocimeter (ADV) data from a serial port */ /* and updates vehicle bottom-referenced velocities. */ /* Inputs : None. */ /* Outputs : Returns ERROR on fatal error. */ /******************************************************************************/ STATUS dopplerTask(sio32Chan* serialChannel) { dopplerDMItems dopplerDmi; /* doppler data manager items */ Errno status, xStatus, /* data manager error codes */ yStatus, zStatus; Byte velocityStatus; /* velocity status bits */ Byte velocityStatusMask; /* velocity status bit mask */ WDOG_ID dopplerPeriod; /* watchdog timer period */ SEM_ID dopplerSem; /* task wakeup semaphore */ DM_Group dopplerGroup; /* doppler data manager group */ DWord dopplerGroupBits; /* doppler group changes bit vector */ dopplerIdBits dopplerId; /* doppler id bits */ MBool dopplerDataValid[NX] /* sensor valid flag */ = { FALSE, FALSE, FALSE }; MBool init; /* initialization flag */ Int16 dof; /* dof counter */ Flt32 dopplerVelocity[NX]; /* doppler velocity measurements */ DM_Time sampleTime; /* data manager item sample time */ /* set task priority */ taskPrioritySet(taskIdSelf(), DOPPLER_PRIORITY); /* initialize task wakeup semaphores */ if ( ((dopplerSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) || ((dopplerPeriod = wdCreate()) == NULL) ) { logMsg("Doppler System FAILURE :\n could not initialize semaphores\n"); return(ERROR); } /* create data manager items and start providers */ if (dopplerDMInit(&dopplerDmi, &dopplerGroup, &dopplerId) == ERROR) return(ERROR); taskSuspend(taskIdSelf()); /* Wait for Adv to powerup */ #if 0 if (initAdvSerial(serialChannel) == ERROR) { logMsg("Doppler System FAILURE : could not initialize serial port\n"); return(ERROR); } /* initialize ADV */ if (initDoppler(serialChannel) == ERROR) { logMsg("Doppler System FAILURE : could not initialize ADV\n"); return(ERROR); } #endif init = TRUE; FOREVER { /* set doppler rate with watchdog timer */ /* do not use MBARI type Int16 to typecast int here */ wdStart(dopplerPeriod, (sysClkRateGet() / (USECS_PER_SEC / DOPPLER_PERIOD)), (FUNCPTR) semGive, (int) dopplerSem); /* get doppler velocities and update data manager items */ velocityStatus = getVelocity(serialChannel, dopplerVelocity); for (dof = 0; dof < NX; dof++) { if (dof == X_INDEX) velocityStatusMask = BAD_X_VELOCITY; else if (dof == Y_INDEX) velocityStatusMask = BAD_Y_VELOCITY; else /* dof == Z_INDEX */ velocityStatusMask = BAD_Z_VELOCITY; gettimeofday(&sampleTime, (struct timezone *) NULL); if ((velocityStatus & velocityStatusMask) != velocityStatusMask) { if (!dopplerDataValid[dof]) { dopplerDataValid[dof] = TRUE; dm_write(dopplerDmi.dataValid[dof], (Void *) &dopplerDataValid[dof], sizeof(dopplerDataValid[dof]), &sampleTime); } status = dm_write(dopplerDmi.velocity[dof], (Void *) &dopplerVelocity[dof], sizeof(dopplerVelocity[dof]), &sampleTime); if (init) checkWrite(status, "Doppler System - velocity"); } else if (dopplerDataValid[dof]) { dopplerDataValid[dof] = FALSE; dm_write(dopplerDmi.dataValid[dof], (Void *) &dopplerDataValid[dof], sizeof(dopplerDataValid[dof]), &sampleTime); } } /* determine if any doppler parameters have been changed */ dopplerGroupBits = dm_get_group_changes(dopplerGroup); if (dopplerGroupBits != 0) { /* reset bottom track mode */ if (dopplerGroupBits & dopplerId.bottomTrackModeBit) setBottomTrackMode(&dopplerDmi, serialChannel); /* reset time between pings */ if (dopplerGroupBits & dopplerId.timeBetweenPingsBit) setTimeBetweenPings(&dopplerDmi, serialChannel); /* reset time per ensemble */ if (dopplerGroupBits & dopplerId.timePerEnsembleBit) setTimePerEnsemble(&dopplerDmi, serialChannel); /* power down ADV */ if (dopplerGroupBits & dopplerId.powerDownBit) powerDownDoppler(&dopplerDmi, serialChannel); } /* wait for wakeup semaphore */ semTake(dopplerSem, WAIT_FOREVER); if (init) init = FALSE; } /* FOREVER */ } /* dopplerTask */ /******************************************************************************/ /* Function : dopplerDMInit */ /* Purpose : Creates data manager items, starts providers and checks status. */ /* Inputs : Doppler data manager items structure pointer. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Errno dopplerDMInit(dopplerDMItems *dopplerDmi, DM_Group *dopplerGroup, dopplerIdBits *dopplerId) { Int16 item; /* data manager item counter */ Int16 dof; /* dof counter */ Errno status; /* error code */ MBool dopplerDataValid = /* doppler data valid */ { FALSE }; DM_Time sampleTime; /* data manager item sample time */ /* initialized items */ MBool powerDown = FALSE; /* power down flag */ Nat16 trackMode = 4; /* bottom track mode */ pingTime timeBetweenPings = /* time between pings */ { 0, 0, 5 }; ensembleTime timePerEnsemble = /* time per ensemble interval */ { 0, 0, 0, 5 }; DM_Element pingTypes[4] = /* ping time structure types */ { {DM_NAT16, 1}, {DM_NAT16, 1}, {DM_NAT16, 1}, {DM_ENDT, 0} }; DM_Element ensembleTypes[5] = /* ensemble time structure types */ { {DM_NAT16, 1}, {DM_NAT16, 1}, {DM_NAT16, 1}, {DM_NAT16, 1}, {DM_ENDT, 0} }; DM_Define dmiInit[] = { { BOTTOM_TRACK_MODE_DM, 1, DM_NAT16, DMT_NULL, &trackMode }, { TIME_BETWEEN_PINGS_DM, 1, DM_STRUCT, pingTypes, &timeBetweenPings }, { TIME_PER_ENSEMBLE_DM, 1, DM_STRUCT, ensembleTypes, &timePerEnsemble }, { DOPPLER_POWER_DOWN_DM, 1, DM_MBOOL, DMT_NULL, &powerDown }, { NULL, 0, DM_ENDT, DMT_NULL, NULL } }; /* uninitialized items */ struct { 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_VELOCITY_DM, DM_FLT32, NX, dopplerDmi->velocity }, { DOPPLER_DATA_VALID_DM, DM_MBOOL, NX, dopplerDmi->dataValid }, { 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); } status = dm_create_items(dmiInit); if (checkCreate(status, dmiInit[item].itm_name, FALSE) == ERROR) return(ERROR); /* look up data manager item handles of initialized items */ if ((dopplerDmi->bottomTrackMode = dm_lookup(BOTTOM_TRACK_MODE_DM, 0)) == NO_ITEM) { logMsg("Doppler System FAILURE : bottom track mode item does not exist\n"); return(ERROR); } if ((dopplerDmi->timeBetweenPings = dm_lookup(TIME_BETWEEN_PINGS_DM, 0)) == NO_ITEM) { logMsg("Doppler System FAILURE : time between pings item does not exist\n"); return(ERROR); } if ((dopplerDmi->timePerEnsemble = dm_lookup(TIME_PER_ENSEMBLE_DM, 0)) == NO_ITEM) { logMsg("Doppler System FAILURE : time per ensemble item does not exist\n"); return(ERROR); } if ((dopplerDmi->powerDown = dm_lookup(DOPPLER_POWER_DOWN_DM, 0)) == NO_ITEM) { logMsg("Doppler System FAILURE : power down item does not exist\n"); return(ERROR); } /* start provider items and check status */ for (dof = 0; dof < NX; dof++) { status = dm_start_provider(dopplerDmi->velocity[dof], DM_STATIC); if (checkProvider(status, "Doppler System - velocities") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->dataValid[dof], DM_STATIC); if (checkProvider(status, "Doppler System - data valid flags") == ERROR) return(ERROR); } /* initialize doppler data as invalid */ gettimeofday(&sampleTime, (struct timezone *) NULL); for (dof = 0; dof < NX; dof++) { status = dm_write(dopplerDmi->dataValid[dof], (Void *) &dopplerDataValid, sizeof(dopplerDataValid), &sampleTime); checkWrite(status, "Doppler System - data valid flags"); } /* start consumer items and check status */ status = dm_start_consumer(dopplerDmi->bottomTrackMode, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - bottom track mode") == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->timeBetweenPings, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - time between pings") == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->timePerEnsemble, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - time per ensemble") == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->powerDown, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - power down") == ERROR) return(ERROR); /* create data manager group */ *dopplerGroup = dm_create_group(); /* add data manager items to group */ status = dm_group_add_item(*dopplerGroup, dopplerDmi->bottomTrackMode, &dopplerId->bottomTrackModeBit); if (checkGroup(status, "Doppler System - bottom track mode") == ERROR) return(ERROR); status = dm_group_add_item(*dopplerGroup, dopplerDmi->timeBetweenPings, &dopplerId->timeBetweenPingsBit); if (checkGroup(status, "Doppler System - time between pings") == ERROR) return(ERROR); status = dm_group_add_item(*dopplerGroup, dopplerDmi->timePerEnsemble, &dopplerId->timePerEnsembleBit); if (checkGroup(status, "Doppler System - time per ensemble") == ERROR) return(ERROR); status = dm_group_add_item(*dopplerGroup, dopplerDmi->powerDown, &dopplerId->powerDownBit); if (checkGroup(status, "Doppler System - power down") == ERROR) return(ERROR); return(OK); } /* dopplerDMInit */ /******************************************************************************/ /* Function : initDoppler */ /* Purpose : Initializes the Acoustic Doppler Velocimeter. */ /* Inputs : Serial channel pointer. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Int16 initDoppler(sio32Chan *serialChannelPtr) { Byte buffer[MAX_PACKET_LEN] = ""; /* serial buffer */ Int16 numBytes; /* number of bytes */ /* send on break, then off break to wake up ADV */ if (wakeUpDoppler(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_WRITE_CHANNEL_NUMBER) == ERROR) { logMsg("Doppler System FAILURE : Could not wake up doppler\n"); return(ERROR); } /* reset the ADV to factory settings */ sprintf(buffer, "CR1\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not retrieve parameters") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - CR1 - retrieve parameters") != ERROR) logMsg("ADV Response : %s\n", buffer); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - CR1 - retrieve parameters") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the serial port parameters */ /* or default values sprintf(buffer, "CB%d%d%d\r", BAUD_RATE_9600, PARITY_NONE, STOP_BITS_1); if (sendCommand(serialChannelPtr, buffer, 9, "could not set serial port parameters") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - CB - set serial port parameters") != ERROR) logMsg("ADV Response : %s\n", buffer); */ /* set the number of water-track pings to average in each data ensemble */ sprintf(buffer, "WP0\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not set water-track pings") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - WP0 - retrieve parameters") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the number of bottom-track pings to average in each data ensemble */ sprintf(buffer, "BP1\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not set bottom-track pings") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - BP1 - set bottom-track pings") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the ping frequency of the water-mass reference-layer ping. */ sprintf(buffer, "BK0\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not set water-track ping frequency") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - BK0 - set water-track ping frequency") != ERROR) logMsg("ADV Response : %s\n", buffer); /* select the type of ensemble output data structure */ sprintf(buffer, "PD4\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not select output data structure") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - PD4 - select output dtat structure") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the coordinate transformation processing flags */ sprintf(buffer, "EX01000\r"); if (sendCommand(serialChannelPtr, buffer, 8, "could not set coordinate transformation") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - EX0100 - set coordinate transformation") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the bottom track mode */ sprintf(buffer, "BM4\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not set bottom track mode") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - BM4 - set bottom track mode") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the minimum interval between data collection cycles */ sprintf(buffer, "TE00000005\r"); /* 5 hundredths of a second */ if (sendCommand(serialChannelPtr, buffer, 11, "could not set interval between data collection") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - TE00000005 - set interval between data collection") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the minimum time between ping groups (1 bottom-track ping) */ sprintf(buffer, "TP000005\r"); /* 5 hundredths of a second */ if (sendCommand(serialChannelPtr, buffer, 9, "could not set interval between ping groups") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - TP000005 - set interval between ping groups") != ERROR) logMsg("ADV Response : %s\n", buffer); /* start the pinging and data collection cycle */ sprintf(buffer, "CS\r"); /* 5 hundredths of a second */ if (sendCommand(serialChannelPtr, buffer, 3, "could not start pinging") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - CS - start pinging") != ERROR) logMsg("ADV Response : %s\n", buffer); } /* initDoppler */ /******************************************************************************/ /* Function : wakeUpDoppler */ /* Purpose : Sends a 300 ms break to wake up the Acoustic Doppler */ /* Velocimeter. */ /* Inputs : Serial channel pointer, board address and channel number. */ /* Outputs : None. */ /******************************************************************************/ Int16 wakeUpDoppler(sio32Chan *serialChannelPtr, Word boardAddress, Word channelNumber) { Byte buffer[MAX_PACKET_LEN] = ""; /* serial buffer */ Int16 count; /* counter */ /* flush serial port */ if (Quad_Serial_Flush(serialChannelPtr, boardAddress, channelNumber) == ERROR) logMsg("ADV WARNING : serial write port did not flush after wake up"); if (Quad_Serial_Flush(serialChannelPtr, boardAddress, ADV_READ_CHANNEL_NUMBER) == ERROR) logMsg("ADV WARNING : serial read port did not flush after wake up"); /* send on break and then delay 300 ms */ count = 0; while (Quad_Serial_SendBreak(serialChannelPtr, boardAddress, channelNumber, ON) == ERROR) { count++; if (count > NUM_RESPONSE_WAITS) { logMsg("Doppler System WARNING : ADV not responding to on break\n"); return(ERROR); } taskDelay(sysClkRateGet() / (USECS_PER_SEC / RESPONSE_DELAY)); } taskDelay(sysClkRateGet() / (USECS_PER_SEC / DOPPLER_BREAK)); /* send off break */ count = 0; while (Quad_Serial_SendBreak(serialChannelPtr, boardAddress, channelNumber, OFF) == ERROR) { count++; if (count > NUM_RESPONSE_WAITS) { logMsg("Doppler System WARNING : ADV not responding to off break\n"); return(ERROR); } taskDelay(sysClkRateGet() / (USECS_PER_SEC / RESPONSE_DELAY)); } /* get ADV response (ignore first line) */ getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to wake-up ADV"); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to wake-up ADV") != ERROR) logMsg("ADV Response : %s\n", buffer); else return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to wake-up ADV") != ERROR) logMsg("ADV Response : %s\n", buffer); else return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to wake-up ADV") != ERROR) logMsg("ADV Response : %s\n", buffer); else return(ERROR); return(OK); } /* wakeUpDoppler */ /******************************************************************************/ /* Function : sendCommand */ /* Purpose : Sends a command to the Acoustic Doppler Velocimeter serial port.*/ /* Inputs : Serial channel pointer, serial buffer, number of bytes to send */ /* and error message. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Int16 sendCommand(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes, char *errorMsg) { if (Quad_Serial_Write(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_WRITE_CHANNEL_NUMBER, buffer, numBytes) == ERROR) { logMsg("Doppler System WARNING : %s\n", errorMsg); return(ERROR); } } /* sendCommand */ /******************************************************************************/ /* Function : getResponse */ /* Purpose : Gets a response from the Acoustic Doppler Velocimeter serial */ /* port. */ /* Inputs : Serial channel pointer, serial buffer, number of bytes to get */ /* and error message. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Int16 getResponse(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes, char *errorMsg) { Int16 numTermChars = 0; /* number of terminal chars */ Int16 count = 0; /* counter */ /* wait for a terminal character in the buffer */ while ( (numTermChars = Quad_Serial_TermChars(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER)) < 1) { count++; if (count > NUM_RESPONSE_WAITS) { logMsg("Doppler System WARNING : %s\n", errorMsg); return(ERROR); } taskDelay(sysClkRateGet() / (USECS_PER_SEC / RESPONSE_DELAY)); } if (Quad_Serial_Read(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER, buffer, numBytes) == ERROR) { logMsg("Doppler System WARNING : %s\n", errorMsg); return(ERROR); } } /* getResponse */ /******************************************************************************/ /* Function : getVelocity */ /* Purpose : Gets doppler velocities. */ /* Inputs : Serial channel pointer and doppler velocity pointer. */ /* Outputs : Returns velocity status. */ /******************************************************************************/ Byte getVelocity(sio32Chan *serialChannelPtr, Flt32 dopplerVelocity[]) { Byte buffer[MAX_PACKET_LEN]; /* data buffer */ Int32 xVelocity[NBYTES], yVelocity[NBYTES], /* velocities */ zVelocity[NBYTES]; Int32 bottomStatus; /* bottom-reference status */ Flt32 velocity[NX]; /* doppler velocities */ /* get the data buffer */ if (getAdvSerialData(serialChannelPtr, buffer, DOPPLER_BUFFER_SIZE) == ERROR) { logMsg("Doppler System WARNING : serial data error reading velocity\n"); return(BAD_X_VELOCITY | BAD_Y_VELOCITY | BAD_Z_VELOCITY); } sscanf(buffer+10, "%2x %2x %2x %2x %2x %2x", &xVelocity[DOPPLER_LSB], &xVelocity[DOPPLER_MSB], &yVelocity[DOPPLER_LSB], &yVelocity[DOPPLER_MSB], &zVelocity[DOPPLER_LSB], &zVelocity[DOPPLER_MSB]); sscanf(buffer+42, "%2x", &bottomStatus); /* for testing */ bottomStatus = BEAM_OK; if (bottomStatus == BEAM_OK) /* convert from mm/s to m/s */ return(mmToMetres(xVelocity, yVelocity, zVelocity, dopplerVelocity)); else /* beam error */ { if ((bottomStatus & BEAM1_CORRELATION) == BEAM1_CORRELATION) logMsg("Doppler System WARNING : beam 1 low correlation\n"); if ((bottomStatus & BEAM1_ECHO_AMPLITUDE) == BEAM1_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 1 low echo amplitude\n"); if ((bottomStatus & BEAM2_CORRELATION) == BEAM2_CORRELATION) logMsg("Doppler System WARNING : beam 2 low correlation\n"); if ((bottomStatus & BEAM2_ECHO_AMPLITUDE) == BEAM2_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 2 low echo amplitude\n"); if ((bottomStatus & BEAM3_CORRELATION) == BEAM3_CORRELATION) logMsg("Doppler System WARNING : beam 3 low correlation\n"); if ((bottomStatus & BEAM3_ECHO_AMPLITUDE) == BEAM3_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 3 low echo amplitude\n"); if ((bottomStatus & BEAM4_CORRELATION) == BEAM4_CORRELATION) logMsg("Doppler System WARNING : beam 4 low correlation\n"); if ((bottomStatus & BEAM4_ECHO_AMPLITUDE) == BEAM4_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 4 low echo amplitude\n"); return(BAD_X_VELOCITY | BAD_Y_VELOCITY | BAD_Z_VELOCITY); } } /* getVelocity */ /******************************************************************************/ /* Function : mmToMetres */ /* Purpose : Converts velocities in mm/s to m/s. */ /* Inputs : X, y and z velocity lsb and msb, doppler velocity. */ /* Outputs : Returns velocity status. */ /******************************************************************************/ Int16 mmToMetres(Int32 xVelocity[], Int32 yVelocity[], Int32 zVelocity[], Flt32 velocity[]) { MBool badXVelocity = FALSE; /* bad x velocity flag */ MBool badYVelocity = FALSE; /* bad y velocity flag */ MBool badZVelocity = FALSE; /* bad z velocity flag */ Int16 velocityMM; /* velocity in mm */ /* get x velocity, check if bad, convert to metres per second */ velocityMM = xVelocity[DOPPLER_LSB] | (xVelocity[DOPPLER_MSB] << 8); if (velocityMM == BAD_VELOCITY) badXVelocity = BAD_X_VELOCITY; else velocity[X_INDEX] = (Flt32) velocityMM * MM_TO_METRES; /* get y velocity, check if bad, convert to metres per second */ velocityMM = yVelocity[DOPPLER_LSB] | (yVelocity[DOPPLER_MSB] << 8); if (velocityMM == BAD_VELOCITY) badYVelocity = BAD_Y_VELOCITY; else velocity[Y_INDEX] = (Flt32) velocityMM * MM_TO_METRES; /* get z velocity, check if bad, convert to metres per second */ velocityMM = zVelocity[DOPPLER_LSB] | (zVelocity[DOPPLER_MSB] << 8); if (velocityMM == BAD_VELOCITY) badZVelocity = BAD_Z_VELOCITY; else velocity[Z_INDEX] = (Flt32) velocityMM * MM_TO_METRES; return(badXVelocity | badYVelocity | badZVelocity); } /* mmToMetres */ /******************************************************************************/ /* Function : setBottomTrackMode */ /* Purpose : Resets the bottom track mode. */ /* Inputs : Doppler data manager items pointer and serial channel pointer. */ /* Outputs : None. */ /******************************************************************************/ Void setBottomTrackMode(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr) { Byte buffer[MAX_PACKET_LEN] = ""; /* serial buffer size */ Nat16 bottomTrackMode; /* bottom track mode */ Errno status; /* data manager error code */ /* get new value */ status = dm_read(dopplerDmi->bottomTrackMode, (Void *) &bottomTrackMode, sizeof(bottomTrackMode), (DM_Time *) NULL); if (checkRead(status, "Doppler System - bottom track mode\n")) return; /* check range */ if ( (bottomTrackMode != 0) && (bottomTrackMode != 4) && (bottomTrackMode != 5) ) { logMsg("Doppler System WARNING : bottom track mode out of range (4,5)\n"); return; } /* set the bottom track mode */ sprintf(buffer, "BM%d\r", bottomTrackMode); sendCommand(serialChannelPtr, buffer, 4, "could not set bottom track mode"); } /* setBottomTrackMode */ /******************************************************************************/ /* Function : setTimeBetweenPings */ /* Purpose : Resets the minimum time between ping groups. */ /* Inputs : Doppler data manager items pointer and serial channel pointer. */ /* Outputs : None. */ /******************************************************************************/ Void setTimeBetweenPings(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr) { Byte buffer[MAX_PACKET_LEN]; /* serial buffer size */ char temp1[STRING_LENGTH], /* temporary strings */ temp2[STRING_LENGTH], temp3[STRING_LENGTH]; Errno status; /* data manager error code */ pingTime timeBetweenPings; /* time between pings */ /* get new value */ status = dm_read(dopplerDmi->timeBetweenPings, (Void *) &timeBetweenPings, sizeof(timeBetweenPings), (DM_Time *) NULL); if (checkRead(status, "Doppler System - time between pings\n")) return; /* check range */ if (timeBetweenPings.minutes > 59) { logMsg("Doppler System WARNING : time between pings (minutes) out of range (0..59)\n"); return; } if (timeBetweenPings.seconds > 59) { logMsg("Doppler System WARNING : time between pings (seconds) out of range (0..59)\n"); return; } if (timeBetweenPings.hundredthSeconds > 99) { logMsg("Doppler System WARNING : time between pings (hundredthSeconds) out of range (0..99)\n"); return; } /* set the time between pings */ if (timeBetweenPings.minutes < 10) sprintf(temp1, "TP0%d", timeBetweenPings.minutes); else sprintf(temp1, "TP%d", timeBetweenPings.minutes); if (timeBetweenPings.seconds < 10) sprintf(temp2, "0%d", timeBetweenPings.seconds); else sprintf(temp2, "%d", timeBetweenPings.seconds); if (timeBetweenPings.hundredthSeconds < 10) sprintf(temp3, "0%d", timeBetweenPings.hundredthSeconds); else sprintf(temp3, "%d", timeBetweenPings.hundredthSeconds); sprintf(buffer, "%s%s%s\r", temp1, temp2, temp3); sendCommand(serialChannelPtr, buffer, 9, "could not set time between pings"); } /* setTimeBetweenPings */ /******************************************************************************/ /* Function : setTimePerEnsemble */ /* Purpose : Resets the minimum interval between data ensembles. */ /* Inputs : Doppler data manager items pointer and serial channel pointer. */ /* Outputs : None. */ /******************************************************************************/ Void setTimePerEnsemble(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr) { Byte buffer[MAX_PACKET_LEN]; /* serial buffer size */ char temp1[STRING_LENGTH], /* temporary strings */ temp2[STRING_LENGTH], temp3[STRING_LENGTH]; Errno status; /* data manager error code */ ensembleTime timePerEnsemble; /* time per ensemble interval */ /* get new value */ status = dm_read(dopplerDmi->timePerEnsemble, (Void *) &timePerEnsemble, sizeof(timePerEnsemble), (DM_Time *) NULL); if (checkRead(status, "Doppler System - time per interval\n")) return; /* check range */ if (timePerEnsemble.hours > 23) { logMsg("Doppler System WARNING : time per ensemble (hours) out of range (0..23)\n"); return; } if (timePerEnsemble.minutes > 59) { logMsg("Doppler System WARNING : time per ensemble (minutes) out of range (0..59)\n"); return; } if (timePerEnsemble.seconds > 59) { logMsg("Doppler System WARNING : time per ensemble (seconds) out of range (0..59)\n"); return; } if (timePerEnsemble.hundredthSeconds > 99) { logMsg("Doppler System WARNING : time per ensemble (hundredthSeconds) out of range (0..99)\n"); return; } /* set the time between pings */ if (timePerEnsemble.minutes < 10) sprintf(temp1, "TE0%d", timePerEnsemble.minutes); else sprintf(temp1, "TE%d", timePerEnsemble.minutes); if (timePerEnsemble.seconds < 10) sprintf(temp2, "0%d", timePerEnsemble.seconds); else sprintf(temp2, "%d", timePerEnsemble.seconds); if (timePerEnsemble.hundredthSeconds < 10) sprintf(temp3, "0%d", timePerEnsemble.hundredthSeconds); else sprintf(temp3, "%d", timePerEnsemble.hundredthSeconds); sprintf(buffer, "%s%s%s\r", temp1, temp2, temp3); sendCommand(serialChannelPtr, buffer, 9, "could not set time per ensemble"); } /* setTimePerEnsemble */ /******************************************************************************/ /* Function : powerDownDoppler */ /* Purpose : Powers down the ADV. */ /* Inputs : Doppler data manager items pointer and serial channel pointer. */ /* Outputs : None. */ /******************************************************************************/ Void powerDownDoppler(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr) { Byte buffer[MAX_PACKET_LEN]; /* serial buffer size */ MBool powerDown; /* power down flag */ Errno status; /* data manager error code */ /* get new value */ status = dm_read(dopplerDmi->powerDown, (Void *) &powerDown, sizeof(powerDown), (DM_Time *) NULL); if (checkRead(status, "Doppler System - power down\n")) return; /* check value, can not do a software power up */ if (powerDown == FALSE) { logMsg("Doppler System WARNING : can not use this command to power up\n"); return; } sprintf(buffer, "CZ\r"); if (sendCommand(serialChannelPtr, buffer, 3, "could not power down ADV") == ERROR) { logMsg("Doppler System WARNING : could not use power down ADV\n"); return; } memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - CZ - power down") != ERROR) logMsg("ADV Response : %s\n", buffer); } /* powerDownDoppler */ /******************************************************************************/ /******************************************************************************/ /* Function : initAdvSerial */ /* Purpose : Initializes the virtual serial port. */ /* Inputs : Serial channel pointer. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ STATUS initAdvSerial(sio32Chan *serialChannelPtr) { Byte buffer[MAX_PACKET_LEN]; /* serial buffer */ MBool handshake = OFF; /* no handshaking */ #if 0 /* serial channel number */ Nat16 serialChannelNum = SONAR_IBC_SERIAL_CHAN; #endif Nat16 baudRate = 9600; /* baud rate */ Nat16 dataBits = 8; /* data bits */ Nat16 stopBits = 1; /* stop bits */ Nat16 parity = 2; /* parity */ Int16 numBytes; /* number of bytes */ Int16 protocol = 1; /* RS485 protocol */ Int16 trys; /* Number of tries for Cmd Enable */ #if 0 /* initialize the serial channel */ if (sio32ChanInit(serialChannelNum, "/sio32/adv", serialChannelPtr) == ERROR) { logMsg("Doppler System WARNING : could not initialize serial channel\n"); return(ERROR); } /* send command enable to IBC micro */ for (trys = 0; ((microCmdEnable(serialChannelPtr) == ERROR) && trys < MICRO_ENABLE_TRYS); trys++) taskDelay(sysClkRateGet() / 5); if (trys == MICRO_ENABLE_TRYS) { logMsg("Doppler System WARNING : error enabling microcontroller\n"); return(ERROR); } #endif /* open quad serial Read port */ if (Quad_Serial_Open(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER) == ERROR) { logMsg("Doppler System WARNING : could not open read serial port\n"); return(ERROR); } /* open quad serial Write port */ if (Quad_Serial_Open(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_WRITE_CHANNEL_NUMBER) == ERROR) { logMsg("Doppler System WARNING : could not open write serial port\n"); return(ERROR); } /* set serial port parameters */ if (Quad_Serial_SetLineParm(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER, baudRate, dataBits, stopBits, parity, handshake, protocol) == ERROR) { logMsg("Doppler System WARNING : could not set read port parameters\n"); return(ERROR); } if (Quad_Serial_SetLineParm(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_WRITE_CHANNEL_NUMBER, baudRate, dataBits, stopBits, parity, handshake, protocol) == ERROR) { logMsg("Doppler System WARNING :could not set write port parameters\n"); return(ERROR); } return (OK); } /* initAdvSerial */ /******************************************************************************/ /* Function : closeAdvSerial */ /* Purpose : Closes the virtual serial port. */ /* Inputs : Serial channel pointer. */ /* Outputs : None. */ /******************************************************************************/ Void closeAdvSerial(sio32Chan *serialChannelPtr) { Quad_Serial_Close(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER); Quad_Serial_Close(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_WRITE_CHANNEL_NUMBER); } /* closeAdvSerial */ /******************************************************************************/ /* Function : getAdvSerialData */ /* Purpose : Gets data from the virtual serial port. */ /* Inputs : Serial data structure, buffer and number of bytes. */ /* Outputs : Returns number of bytes received. */ /******************************************************************************/ STATUS getAdvSerialData(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes) { Int16 numTermChars; /* number of terminal chars */ Int16 count = 0; /* counter */ Int16 byte; /* byte counter */ Int32 byteVal; /* byte decimal value */ Int32 checksum = 0; /* calculated checksum */ Int32 advChecksum; /* data ensemble checksum */ Int32 checkSumLsb, checkSumMsb; /* checksum lsb and msb */ /* wait for a terminal character in the buffer */ while ( (numTermChars = Quad_Serial_TermChars(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER)) < 1) { if (count > NUM_SERIAL_WAITS) { logMsg("Doppler System WARNING : No response from doppler\n"); return(ERROR); } taskDelay(sysClkRateGet() / (USECS_PER_SEC / SERIAL_DELAY)); } /* get the data ensemble (must flush after each read) */ if (Quad_Serial_Read(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER, buffer, numBytes) == ERROR) { logMsg("Doppler System WARNING : problem reading serial port\n"); return(ERROR); } Quad_Serial_Flush(serialChannelPtr, (Word) ADV_BOARD_ADDRESS, (Word) ADV_READ_CHANNEL_NUMBER); /* calculate ensemble checksum */ for (byte = 0; byte < (numBytes-4); byte+=2) { sscanf(buffer+byte, "%2x", &byteVal); checksum += byteVal; } /* verify ensemble checksum */ sscanf(buffer+numBytes-4, "%2x %2x", &checkSumLsb, &checkSumMsb); advChecksum = checkSumLsb | (checkSumMsb << 8); if (checksum == advChecksum) return(OK); else { logMsg("Doppler System WARNING : ensemble checksum incorrect\n"); return(ERROR); } } /* getAdvSerialData */