/****************************************************************************/ /* Copyright 1995 to 1996 MBARI */ /****************************************************************************/ /* Summary : Sonar Dcon Interface Module for VxWorks */ /* Filename : sonarDcon.c */ /* Author : Douglas Au, Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 10/30/95 */ /* Modified : 08/14/96 */ /* : 00/11/27; rsm. */ /* 1. Renamed getVelocity() to getDvlData() */ /* 2. Moved the velocityStatus flag into getDvlData's argument */ /* list. Renamed it to dvlDataStatus */ /* 3. Added BAD_COMMS flag to the dvlDataStatus byte. */ /* 4. Moved bottomStatus and waterStatus into getDvlData()'s */ /* argument list so that they can be passed on as Data */ /* Manager items. */ /* 5. Added dvlRange and dvlTemp calculations to getDvlData(). */ /* Also added these as arguments. */ /* 6. Added a calculation of dvlAltitude from dvlRange, and */ /* vehicleRoll and vehiclePitch. These two items are read */ /* from Data Manager, and are inclinometer measurements. */ /* 7. Provided 6 new Data Manager items: */ /* DOPPLER_ALTITUDE_DM "TIBURON.SENSOR.DOPPLER.ALTITUDE.METERS" */ /* DOPPLER_TEMP_DM "TIBURON.SENSOR.DOPPLER.TEMP.CENTIG" */ /* DOPPLER_BOTTOM_TRACK_STATUS_DM "TIBURON.SENSOR.DOPPLER.BOTTOMTRACK_STATUS"*/ /* DOPPLER_WATER_TRACK_STATUS_DM "TIBURON.SENSOR.DOPPLER.WATERTRACK_STATUS"*/ /* DOPPLER_COMMS_VALID_DM "TIBURON.SENSOR.DOPPLER.COMMS_VALID" */ /* "TIBURON.SENSOR.DOPPLER.BOTTOM_TRACK_VELOCITY_ERROR" */ /* 8. Changed the return type of getAdvSerialData() to Byte. */ /* The Byte contains 3 flags pertaining to the serial comms. */ /* These flags are also now set in dvlDataStatus. */ /* 9. Added logic so that the DVL has a chance to respond (at */ /* 3-4 Hz) before this loop, at 10 Hz, sets the COMMS_INVALID*/ /* flag. This is in the FOREVER loop in sonarDconSensorInTask*/ /* 10. Changed BM4 to BM5 in the doppler initialization routine. */ /* Arthur Johnson of RDI says that this is now the reccommended*/ /* setting and will give superior performance to BM4. */ /* */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: sonarDcon.c,v 1.0 95/10/30 audo Exp $ * $Log: sonarDcon.c,v $ * Revision 1.0 * Initial revision * */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* VxWorks standard I/O library */ #include /* VxWorks string library */ #include /* vxWorks semaphore functions */ #include /* vxWorks task library functions */ #include /* vxWorks time and date functions */ #include /* vxWorks Message Queue Library */ #include /* vxWorks watch dog timer functions */ #include /* vxWorks math library */ #include /* MBARI style guide type declarations */ #include /* Miscellaneous constants */ #include /* MBARI Rov Application Priorities */ #include /* MBARI time and date functions */ #include /* Data Manager declarations */ #include /* Data Manager error declarations */ #include "powerAlloc.h" /* Device Power Requirements */ #include "powerDM.h" /* Power System Data Manager Items */ #include "powerTypes.h" /* Power management Data Types */ #include "console.h" /* Topside Console Data Manager Items */ #include "sio32Drv.h" /* Sio32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #include "sonarDcon.h" /* Sonar Dcon Can Definitions */ #include "sensorsDm.h" /* Sensor Dm Prefix */ #include "medianFilter.h" /* median filter data structers */ #include "applic.h" /* Microcontroller applications */ #include "microcmd.h" /* Microcontroller commands */ #include "microDm.h" /* Micro Data Manager Items & Functions */ #define Boolean MBool /* Map microcontroller Boolean to MBool */ #include "timer.h" /* IBC Micro Timer Definitions */ #include "ibc_card.h" /* IBC Card Types */ #include "ibc_cmd.h" /* IBC Serial Commands */ #include "filter.h" /* IBC General IIR Filter Routines */ #include "gf_board.h" /* GF/5V Controller Board Definitions */ #include "low_pwr.h" /* Low Power Switch Board Definitions */ #include "sonar.h" /* Sonar Dcon Micro definitions */ #include "ibc.h" /* Function Prototypes for ibc.c */ #include "switch.h" /* IBC Function Libarary Definitions */ #include "ibcDm.h" /* IBC Data Manager Interface Functions */ #include "lpsDm.h" /* IBC LPS Board Interface Functions */ #include "gf_5vDm.h" /* IBC Ground Fault Board DMgr Functions*/ #include "gf_5v.h" /* IBC Ground Fault Board Definitions */ #include "quadSerDm.h" /* Quad Serial Board Data Manager Funcs */ #include "quadSerCmd.h" /* VSP Function Prototypes */ #include "vspDm.h" /* VSP Data Manager Interface Functions */ #include "vspCmd.h" /* VSP Function Prototypes */ #include "doppler.h" /* ADV Funcs */ #include "dopplerDM.h" /* Doppler Data Manager definitions */ #include "motionpak.h" /* motion pak definitions */ #include "kalman.h" /* kalman filter definitions */ #include "microTask.h" /* Microcontroller Task IO definitions */ /* Sonar Dcon Device Name */ MLocal char* const sonarDconName = "Sonar Dcon"; /* Sonar Dcon Data Mgr Item Name Prefix */ MLocal char* const sonarIbcDmPrefix = SONAR_IBC_DM_PREFIX; MLocal char* const sensorDmPrefix = SENSOR_DM_PREFIX; #define PI 3.14159265358979323846 /* pi (not in vxWorks math library) */ #define CM_TO_METRES 0.01 /* cm to meters conversion */ #define cos30 0.86602540378444 /* Doppler Constants */ #define DOPPLER_PRIORITY 70 /* doppler task priority */ #define POWER_DELAY 1000000 /* Power on Delay for Doppler LPS */ #define DOPPLER_BREAK 300000 /* doppler break duration (microsec) */ /* 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 NXE 4 /* number of cartesian dof & error */ #define KALMAN_STATES 6 /* number of Kalman filter states */ #define NBYTES 2 /* number of bytes */ #define BUTTER_ORDER2 2 /* Butterworth filter order */ #define MM_TO_METRES 0.001 /* mm to meters conversion */ #define RAD_TO_DEG 180.0 / PI /* radians to degrees converion */ #define EPSILON 0.00001 /* floating point zero */ #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 /* beam status ok */ #define BOTTOM_BEAM1_CORRELATION 1 /* bottom-referenced correlation and */ #define BOTTOM_BEAM1_ECHO_AMPLITUDE 2 /* echo amplitude status */ #define BOTTOM_BEAM2_CORRELATION 4 #define BOTTOM_BEAM2_ECHO_AMPLITUDE 8 #define BOTTOM_BEAM3_CORRELATION 16 #define BOTTOM_BEAM3_ECHO_AMPLITUDE 32 #define BOTTOM_BEAM4_CORRELATION 64 #define BOTTOM_BEAM4_ECHO_AMPLITUDE 128 #define WATER_BEAM1_CORRELATION 1 /* water-referenced depth and */ #define WATER_BEAM2_CORRELATION 4 /* correlation status */ #define WATER_BEAM3_CORRELATION 8 #define WATER_BEAM4_CORRELATION 16 #define ALTITUDE_TOO_SHALLOW 32 /* bad velocity value returned */ #define BAD_VELOCITY -32768 /* dvlDataStatus & SerialStatus flags */ #define DVL_DATA_GOOD 0 /* Good data received from DVL */ #define BAD_BOTTOM_TRACK_VELOCITY 1 /* bad bottom track velocity */ #define BAD_WATER_MASS_VELOCITY 2 /* bad water track velocity */ #define NO_DVL_RESPONSE 4 /* No response from DVL serial line */ #define BAD_SERIAL_READ 8 /* Problem reading serial line */ #define CHECKSUM_WRONG 16 /* Incorrect checksum from serial */ enum beamIndx {BM1, BM2, BM3, BM4}; /* for indexing the 4 beams in rawRange*/ enum timeIndx {HRS, MIN, SEC, CENTISEC};/* For indexing the time bytes */ /* Doppler Serial Constants */ #define SERIAL_DELAY 10000 /* serial delay (microsec) */ #define NUM_SERIAL_WAITS 3 /* number of times to wait for serial */ /* port (30 millisecond duration) */ #define NUM_DVL_REPLIES 10 /* number of times to wait for DVL */ /* response */ #define REPLACE_ALT_W_DVL /******************************************************************************/ #define GF5V_BOARD_ADDR (Word) GROUND_FAULT_0_ADDR #define SERIAL_BOARD_ADDR (Word) QUAD_SERIAL_0_ADDR #define LPS_BOARDS 2 /* Number of LPS Board in Sonar Dcon */ MLocal Word lpsBoardAddr[LPS_BOARDS] = /* LPS Board Assignments */ { LOW_PWR_SWITCH_48V_0_ADDR, LOW_PWR_SWITCH_24V_0_ADDR }; MLocal ibcCardConfig ibcCardSet = { 5, /* Number of IBC Cards Expected */ { CPU196, GF5V_BOARD_ADDR, LOW_PWR_SWITCH_48V_0_ADDR, LOW_PWR_SWITCH_24V_0_ADDR, SERIAL_BOARD_ADDR } }; #define VSP_CHAN 1 /* LPS 48V 0 Board Channel Assignments*/ #define DOPPLER_SONAR_SWITCH_CHAN 0 /* Doppler Power */ /* LPS 24V 0 Board Channel Assignments*/ #define ALTIMETER_SWITCH_CHAN 0 /* Altimeter Power */ #define HOMER_SWITCH_CHAN 1 /* Homer Power */ #define RESPONDER_SWITCH_CHAN 2 /* Responder Power */ #define SONAR_DM_PERIOD 100000 /* Sonar Data period 10 Hz */ #define HUMIDITY_DM_PERIOD 2000000 /* Humidity sonar period .5Hz */ #define SONAR_IBC_DM_PERIOD 500000 /* Sonar Dcon Data period 2 Hz */ #define SONAR_IBC_IN_STACKSIZE 3000 /* Sonar Dcon Input Task size & TCB */ #define SONAR_IBC_OUT_STACKSIZE 4000 /* Sonar Dcon Output Task size & TCB */ #define SONAR_TASK_STACKSIZE 10000 /* Sonar Dcon Sensor Task size & TCB */ #define DOPPLER_SWITCH_DELAY 1000 /* Delay before reading LPS Voltages */ #define NUM_TASKS 4 /* Number of co-operating tasks */ typedef struct /* Sonar Dcon Data Manager Item Handles*/ { DM_Item powerSwitch; /* Data Concentrator Power Switch */ DM_Item canWaterAlarm; /* Housing Water Alarm Detected */ DM_Item jboxWaterAlarm; /* J Box Water Alarm Detected */ DM_Item waterAlarmThresh; /* Water Alarm Threshold */ DM_Item altimeterRange; /* Altimeter range setting */ DM_Item altimeterGain; /* Altimeter gain setting */ ibcCpuDmItems ibcDm; /* IBC CPU Data Manager Items */ ibcGf5vDmItems gf5vDm; /* GF/5V Board Data Manager Items */ ibcLpsDmItems lpsDm[LPS_BOARDS]; /* LPS Board Data Manager Items */ ibcQuadSerDmItems quadSerDm; /* Quad Serial Board Data Manager Items */ ibcVspDmItems vspDm; /* VSP Data Manager Items */ } sonarDconDmItems; typedef struct /* Sonar Dcon Data Manager Item Handles */ { DM_Item altitude; DM_Item rawAltitude; /* Raw Vehicle altitude in meters */ DM_Item altimeterSig; /* Altimeter signal level */ DM_Item altitudeDataValid; /* Altitude data valid flag */ } sonarDconSensorItems; /* Doppler typedefs */ /* cartesian degrees of freedom */ typedef enum { X_INDEX, Y_INDEX, Z_INDEX, E_INDEX } cartDof; /* data manager item handles */ typedef struct { DM_Item startPinging; /* start pinging command */ DM_Item interrupt; /* interrupt command */ DM_Item command; /* user command */ DM_Item response; /* response to user command */ DM_Item dopplerPower; /* power switch status */ DM_Item bottomTrackVelocity[NX]; /* bottom track velocity */ DM_Item bottomTrackVelocityError; /* bottom track velocity error comp. */ DM_Item filtBottomTrackVelocity[NX];/* filtered bottom track velocity */ DM_Item bottomTrackCurrent; /* bottom track current (planer) */ DM_Item bottomTrackDirection; /* bottom track direction (planar) */ DM_Item bottomTrackDataValid; /* bottom track data valid flag */ DM_Item waterMassVelocity[NX]; /* water-mass layer velocity */ DM_Item filtWaterMassVelocity[NX]; /*filtered water-mass layer velocity */ DM_Item waterMassCurrent; /* water-mass layer current (planer) */ DM_Item waterMassDirection; /* water-mass layer direction (planar)*/ DM_Item waterMassDataValid; /* water-mass layer data valid flag */ DM_Item kalmanAttitude; /* (In) Kalman filter attitude */ DM_Item vehicleRoll; /* (In) Roll angle from inclinometer */ DM_Item vehiclePitch; /* (In) Pitch angle from inclinometer */ DM_Item altitude; /* Altitude computed from DVL, meters */ DM_Item temp; /* Temp meas. by DVL, Centigrade */ DM_Item bottomStatus; /* DVL bottom-track status */ DM_Item waterStatus; /* DVL water-track status */ DM_Item commsValid; /* Serial link to DVL is working */ } dopplerDMItems; /* command group id bits */ typedef struct { DWord startPinging; /* start pinging command */ DWord interrupt; /* interrupt command */ DWord command; /* user command */ } dopplerIdBits; /******************************************************************************/ Void sonarDconInTask(Reg sonarDconDmItems *dmItems, microIOControl *microIOCtrl); Void sonarDconOutTask(Reg sonarDconDmItems* dmItems, microIOControl *microIOCtrl); Void sonarDconSensorInTask(microIOControl *microIOCtrl); MLocal Word processSonarDconSRQ( Reg sonarDconDmItems* dmItems, microIOControl *microIOCtrl); MLocal Void initSonarDconDmItems(sonarDconDmItems* dmItems, sio32Chan* sonarDconDataChan); /* function prototypes */ #if __STDC__ Errno dopplerDMInit(dopplerDMItems *dopplerDmi, DM_Group *dopplerGroup, dopplerIdBits *dopplerId); Int16 initDoppler(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr); Int16 wakeUpDoppler(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr, Word boardAddress, Word channelNumber); Int16 startPinging(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr); Int16 sendCommand(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes, char *errorMsg); Int16 getResponse(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes, char *errorMsg); Void getDvlData(sio32Chan *serialChannelPtr, Flt32 bottomTrackVelocity[], Int32 *bottomStatus, Flt32 waterMassVelocity[], Int32 *waterStatus, Flt32 *dvlRange, Flt32 *dvlTemp, Byte *dvlDataStatus ); MBool mmToMetres(Int32 xVelocity[], Int32 yVelocity[], Int32 zVelocity[], Int32 eVelocity[], Flt32 velocity[]); Flt64 velocityFilter(Flt64 raw, Flt64 rawPrev[], Flt64 filtPrev[]); Void commandDoppler(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr); STATUS initAdvSerial(sio32Chan *serialChannelPtr); Void closeAdvSerial(sio32Chan *serialChannelPtr); Byte getAdvSerialData(sio32Chan *serialChannelPtr, Byte *buffer, Int16 numBytes); MBool cvtRange(Int32 rawRange[NUM_BEAMS][NBYTES], Flt32 *dvlAltitude); #endif /****************************************************************************/ /* Function : sonarCanReadSensors */ /* Purpose : Reads altimeter sensor from Sonar Can IBC */ /* Inputs : Sio32 channel, pointers to altitude, sig Level, pressure */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS sonarCanReadSensors( sio32Chan *IBC_SerialChan, Int16 *altitude, Nat16 *sigLevel) { Int16 integer, fraction; Int16 cmdStatus; /* Command Status */ Word reply[3]; /* Command Result from microcontroller */ /* Send command to VME IBC micro */ if ( sendMicroCommand( IBC_SerialChan, &reply, sizeof(Word) * 3, 1, SONAR_IBC_APP | GET_SENSOR_DATA) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update thresholds */ wordsFromBuf(reply, 3, &cmdStatus, altitude, sigLevel); return (OK); /* Return OK */ } /* sonarCanReadSensors() */ /****************************************************************************/ /* Function : setAltimeterRange */ /* Purpose : Get altimeter range from Data Manager and send to IBC */ /* Inputs : Sio32 channel, altimeter range dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS setAltimeterRange( sio32Chan *sio32DataChan, DM_Item altimeterRangeDm ) { Errno status; /* Result of dm_read */ Int16 altimeterRange; /* Altimeter Range Value */ /* Read altimeter range from dmgr */ status = dm_read( altimeterRangeDm, (char *) &altimeterRange, sizeof(altimeterRange), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, SONAR_IBC_APP | SET_ALTIMETER_RANGE, altimeterRange ) != ERROR ) /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); return(ERROR); } /* setAltimeterRange() */ /****************************************************************************/ /* Function : setAltimeterGain */ /* Purpose : Get altimeter gain from Data Manager and send to IBC */ /* Inputs : Sio32 channel, altimeter range dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS setAltimeterGain( sio32Chan *sio32DataChan, DM_Item altimeterGainDm ) { Errno status; /* Result of dm_read */ Int16 altimeterGain; /* Altimeter Gain Value */ /* Read altimeter range from dmgr */ status = dm_read( altimeterGainDm, (char *) &altimeterGain, sizeof(altimeterGain), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, SONAR_IBC_APP | SET_ALTIMETER_GAIN, altimeterGain ) != ERROR ) /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); return(ERROR); } /* setAltimeterGain() */ /******************************************************************************/ /* Function : sonarCanGetWaterAlarm */ /* Purpose : Read water alarm status from Sonar Dcon microcontroller */ /* Inputs : Sio32 socket and pointer to water alarm status */ /* Outputs : Returns OK or ERROR */ /******************************************************************************/ LOCAL STATUS sonarCanGetWaterAlarm( sio32Chan *IBC_SerialChan, Word *waterAlarmStatus ) { /* Send command to microcontroller */ return( microCommand(IBC_SerialChan, SONAR_IBC_APP | GET_WATER_ALARM, waterAlarmStatus) ); } /* sonarCanGetWaterAlarm() */ /****************************************************************************/ /* Function : sonarGetWaterAlarmThresh */ /* Purpose : Reads water alarm threshold value from IBC CPU */ /* Inputs : Sio32 channel, pointer to water alarm threshold */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS sonarGetWaterAlarmThresh( sio32Chan *IBC_SerialChan, Int16 *waterAlarmThresh ) { /* Send command to microcontroller */ return (microCommand( IBC_SerialChan, SONAR_IBC_APP | GET_WATER_ALARM_THRESH, waterAlarmThresh) ); } /* sonarGetWaterAlarmThresh() */ /****************************************************************************/ /* Function : initWaterAlarmThreshDm */ /* Purpose : Read Water Alarm Threshold and update data manager item */ /* Inputs : Sio32 channel, water alarm threshold dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS initWaterAlarmThreshDm( sio32Chan *IBC_SerialChan, DM_Item waterAlarmThreshDm ) { Int16 waterAlarmThresh; /* Water Alarm Threshold from IBC */ /* Get Water Alarm Threshold */ if (sonarGetWaterAlarmThresh( IBC_SerialChan, &waterAlarmThresh ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return (urgentWriteDmItem(waterAlarmThreshDm, &waterAlarmThresh, sizeof(waterAlarmThresh)) ); } /* initWaterAlarmThreshDm() */ /****************************************************************************/ /* Function : writeWaterAlarmThresh */ /* Purpose : Get water alarm threshold value from dm and send to IBC */ /* Inputs : Sio32 channel, water threshold dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writeWaterAlarmThresh( sio32Chan *sio32DataChan, DM_Item waterAlarmThreshDm ) { Errno status; /* Result of dm_read */ Int16 waterAlarmThresh; /* Water Alarm Threshold */ /* Read Water Alarm Thresh from dmgr*/ status = dm_read( waterAlarmThreshDm, (char *) &waterAlarmThresh, sizeof(waterAlarmThresh), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, SONAR_IBC_APP | SET_WATER_ALARM_THRESH, waterAlarmThresh ) != ERROR ) /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); return(ERROR); } /* writeWaterAlarmThresh() */ /****************************************************************************/ /* Function : updateWaterAlarmDm */ /* Purpose : Reads micro Water Alarm status & updates dmgr items */ /* Inputs : Sio32 chan & water alarm status data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateWaterAlarmDm( sio32Chan *sio32DataChan, DM_Item canWaterAlarmDm, DM_Item jboxWaterAlarmDm ) { Word waterAlarmStatus; /* Microcontroller Water Alarm Status */ MBool status; /* Boolean state of Water Alarm */ /* Read Water Alarm Status */ if (sonarCanGetWaterAlarm(sio32DataChan, &waterAlarmStatus) != OK) return(ERROR); status = ( (waterAlarmStatus & HOUSING_WATER_ALARM_BIT) ? TRUE : FALSE); writeDmItem(canWaterAlarmDm, &status, sizeof(status)); status = ( (waterAlarmStatus & JBOX_WATER_ALARM_BIT) ? TRUE : FALSE); /* Write water alarm status to Dmgr */ return ( writeDmItem(jboxWaterAlarmDm, &status, sizeof(status)) ); } /* updateWaterAlarmDm() */ /****************************************************************************/ /* Function : createSonarDconAppDmItems */ /* Purpose : Create Data Mgr Items specific to Sonar Dcon Application */ /* Inputs : Pointer to structure of DM item handles, Dcon DM prefix, */ /* Sonar DM prefix, Sonar Data Concentrator Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS createSonarDconAppDmItems( sonarDconDmItems *sonarDmItems, const char* sonarIbcDmPrefix, const char* sensorDmPrefix, const char* sonarDconName ) { dmItemList sonarDconDmInit[] = { { &sonarDmItems->canWaterAlarm, "Housing Water Alarm", HOUSING_WATER_ALARM_DM, DM_MBOOL, 1 }, { &sonarDmItems->jboxWaterAlarm, "J Box Water Alarm", JBOX_WATER_ALARM_DM, DM_MBOOL, 1 }, { &sonarDmItems->waterAlarmThresh, "Water Alarm Threshold", WATER_ALARM_THRESH_DM, DM_NAT16, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; dmItemList sensorDmInit[] = { { &sonarDmItems->altimeterRange, "Altimeter Range", ALTIMETER_RANGE_DM, DM_INT16, 1 }, { &sonarDmItems->altimeterGain, "Altimeter Gain", ALTIMETER_GAIN_DM, DM_INT16, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Initialize Data Manager Items from table*/ if (initMicroDmItemList( sonarDconDmInit, sonarIbcDmPrefix, "Sonar Dcon I/F" ) != OK) return(ERROR); /* Initialize Data Manager Items from table*/ if (initMicroDmItemList( sensorDmInit, sensorDmPrefix, "Sonar Dcon I/F" ) != OK) return(ERROR); if ( (sonarDmItems->powerSwitch = initMicroDmItem( "", SONAR_DCON_SWITCH_STATUS_DM, DM_ENUM, 1)) == ERROR) { logMsg("Sonar Dcon I/F: Error initializeing Dcon Power Switch Item\n"); return (ERROR); } /* if */ /* Declare this task an item provider */ dm_start_provider(sonarDmItems->canWaterAlarm, DM_STATIC); dm_start_provider(sonarDmItems->jboxWaterAlarm, DM_STATIC); dm_start_consumer(sonarDmItems->powerSwitch, DM_STATIC, SEM_NULL); return(OK); } /* createSonarDconAppDmItems() */ /****************************************************************************/ /* Function : createSonarDconSensorDmItems */ /* Purpose : Create Data Mgr Items specific to Sonar Dcon Application */ /* Inputs : Pointer to structure of DM item handles, Dcon DM prefix, */ /* Sonar DM prefix, Sonar Data Concentrator Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS createSonarDconSensorDmItems( sonarDconSensorItems *sonarDmItems, const char* sensorDmPrefix, const char* sonarDconName ) { dmItemList sonarDconDmInit[] = { { &sonarDmItems->altitude, "Vehicle Altitude", ALTITUDE_METERS_DM, DM_FLT32, 1 }, { &sonarDmItems->rawAltitude, "Altitude Sensor Reading", ALTIMETER_RAW_METERS_DM, DM_FLT32, 1 }, { &sonarDmItems->altimeterSig, "Altimeter Signal Level", ALTIMETER_SIGNAL_DM, DM_NAT16, 1 }, { &sonarDmItems->altitudeDataValid, "Altitude Data Valid", ALTITUDE_DATA_VALID_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Initialize Data Manager Items from table*/ if (initMicroDmItemList( sonarDconDmInit, sensorDmPrefix, "Sonar Dcon I/F" ) != OK) return(ERROR); /* Declare this task an item provider */ dm_start_provider(sonarDmItems->rawAltitude, SONAR_DM_PERIOD); dm_start_provider(sonarDmItems->altimeterSig, SONAR_DM_PERIOD); dm_start_provider(sonarDmItems->altitude, SONAR_DM_PERIOD); dm_start_provider(sonarDmItems->altitudeDataValid, DM_STATIC); return(OK); } /* createSonarDconSensorDmItems() */ /****************************************************************************/ /* Function : initAltimeterDm */ /* Purpose : Reads altimeter settings and update Data Manager items */ /* Inputs : Sio32 chan & sonar dcon item handles */ /* Outputs : None */ /****************************************************************************/ LOCAL Void initAltimeterDm( sio32Chan *IBC_SerialChan, sonarDconDmItems *dmItems) { Word altimeterRange, altimeterGain; /* Altimeter settings */ /* Send command to microcontroller */ if (microCommand( IBC_SerialChan, SONAR_IBC_APP | GET_ALTIMETER_RANGE, &altimeterRange) == OK) if (urgentWriteDmItem(dmItems->altimeterRange, &altimeterRange, sizeof(altimeterRange) ) == ERROR) logMsg("Sonar Dcon: Error initializing Altimeter Range\n"); /* Send command to microcontroller */ if (microCommand( IBC_SerialChan, SONAR_IBC_APP | GET_ALTIMETER_GAIN, &altimeterGain) == OK) if (urgentWriteDmItem(dmItems->altimeterGain, &altimeterGain, sizeof(altimeterGain)) == ERROR) logMsg("Sonar Dcon: Error initializing Altimeter Gain\n"); } /* initAltimeterDm() */ /******************************************************************************/ /* 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 bottomTrackDataValid = /* bottom track data valid */ FALSE; MBool waterMassDataValid = /* water-mass layer data valid */ FALSE; switchStatus dopplerPower = /* doppler power switch status */ SWITCH_OFF; DM_Time sampleTime; /* data manager item sample time */ DM_Define dmiInit[] = /* initilaized items */ { { DOPPLER_SONAR_SWITCH_STATUS_DM, 1, DM_ENUM, DMT_NULL, &dopplerPower }, { NULL, 0, DM_ENDT, DMT_NULL, NULL } }; struct /* uninitialized items */ { char *name; /* data manager array name */ DM_Type type; /* data manager item type */ DM_Num numItems; /* number of data manager items */ DM_Num numElements; /* number of elements of item type */ DM_Item *item; /* data manager item handle */ } dmiCreate[] = { { DOPPLER_START_PINGING_DM, DM_MBOOL, 1, 1, &dopplerDmi->startPinging }, { DOPPLER_INTERRUPT_DM, DM_EMPTY, 1, 1, &dopplerDmi->interrupt }, { DOPPLER_COMMAND_DM, DM_CHAR, 1, MAX_PACKET_LEN, &dopplerDmi->command }, { DOPPLER_RESPONSE_DM, DM_CHAR, 1, MAX_PACKET_LEN, &dopplerDmi->response }, { DOPPLER_BOTTOM_TRACK_VELOCITY_DM, DM_FLT32, NX, 1, dopplerDmi->bottomTrackVelocity }, { DOPPLER_BOTTOM_TRACK_VELOCITY_E_DM, DM_FLT32, 1, 1, &dopplerDmi->bottomTrackVelocityError }, { DOPPLER_FILT_BOTTOM_TRACK_VELOCITY_DM, DM_FLT32, NX, 1, dopplerDmi->filtBottomTrackVelocity }, { DOPPLER_BOTTOM_TRACK_CURRENT_DM, DM_FLT32, 1, 1, &dopplerDmi->bottomTrackCurrent }, { DOPPLER_BOTTOM_TRACK_DIRECTION_DM, DM_FLT32, 1, 1, &dopplerDmi->bottomTrackDirection }, { DOPPLER_BOTTOM_TRACK_DATA_VALID_DM, DM_MBOOL, 1, 1, &dopplerDmi->bottomTrackDataValid }, { DOPPLER_WATER_MASS_VELOCITY_DM, DM_FLT32, NX, 1, dopplerDmi->waterMassVelocity }, { DOPPLER_FILT_WATER_MASS_VELOCITY_DM, DM_FLT32, NX, 1, dopplerDmi->filtWaterMassVelocity }, { DOPPLER_WATER_MASS_CURRENT_DM, DM_FLT32, 1, 1, &dopplerDmi->waterMassCurrent }, { DOPPLER_WATER_MASS_DIRECTION_DM, DM_FLT32, 1, 1, &dopplerDmi->waterMassDirection }, { DOPPLER_WATER_MASS_DATA_VALID_DM, DM_MBOOL, 1, 1, &dopplerDmi->waterMassDataValid }, { ATTITUDE_DM, DM_FLT32, 1, KALMAN_STATES, &dopplerDmi->kalmanAttitude }, { VEHICLE_ROLL_DM, DM_FLT32, 1, 1, &dopplerDmi->vehicleRoll }, { VEHICLE_PITCH_DM, DM_FLT32, 1, 1, &dopplerDmi->vehiclePitch }, { DOPPLER_ALTITUDE_DM, DM_FLT32, 1, 1, &dopplerDmi->altitude }, { DOPPLER_TEMP_DM, DM_FLT32, 1, 1, &dopplerDmi->temp }, { DOPPLER_BOTTOM_TRACK_STATUS_DM, DM_INT32, 1, 1, &dopplerDmi->bottomStatus }, { DOPPLER_WATER_TRACK_STATUS_DM, DM_INT32, 1, 1, &dopplerDmi->waterStatus }, { DOPPLER_COMMS_VALID_DM, DM_MBOOL, 1, 1, &dopplerDmi->commsValid }, { NULL, DM_ENDT, 0, 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].numItems, dmiCreate[item].item, dmiCreate[item].type, dmiCreate[item].numElements, 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); if ( (dopplerDmi->dopplerPower = initMicroDmItem( "", DOPPLER_SONAR_SWITCH_STATUS_DM, DM_ENUM, 1)) == ERROR) { logMsg("Sonar Dcon I/F: Error initializeing Dcon Power Switch Item\n"); return (ERROR); } /* if */ for (dof = 0; dof < NX; dof++) { status = dm_start_provider(dopplerDmi->bottomTrackVelocity[dof], DM_ASYNC); if (checkProvider(status, "Doppler System - bottom track velocity") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->filtBottomTrackVelocity[dof], DM_ASYNC); if (checkProvider(status, "Doppler System - filtered bottom track velocity") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->waterMassVelocity[dof], DM_ASYNC); if (checkProvider(status, "Doppler System - water-mass layer velocity") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->filtWaterMassVelocity[dof], DM_ASYNC); if (checkProvider(status, "Doppler System - filtered water-mass layer velocity") == ERROR) return(ERROR); } status = dm_start_provider(dopplerDmi->bottomTrackVelocityError, DM_ASYNC); if (checkProvider(status, "Doppler System - Bottom Track Vel Error") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->altitude, DM_ASYNC); if (checkProvider(status, "Doppler System - Altitude, meters") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->temp, DM_ASYNC); if (checkProvider(status, "Doppler System - DVL Temperature, centigrade") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->bottomStatus, DM_STATIC); if (checkProvider(status, "Doppler System - bottom-track data status flags") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->waterStatus, DM_STATIC); if (checkProvider(status, "Doppler System - water-track data status flags") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->commsValid, DM_STATIC); if (checkProvider(status, "Doppler System - DVL serial comms valid flag") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->bottomTrackDataValid, DM_STATIC); if (checkProvider(status, "Doppler System - bottom-track data valid flag") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->waterMassDataValid, DM_STATIC); if (checkProvider(status, "Doppler System - water-mass layer data valid flag") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->bottomTrackCurrent, DM_ASYNC); if (checkProvider(status, "Doppler System - bottom track current") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->bottomTrackDirection, DM_ASYNC); if (checkProvider(status, "Doppler System - bottom track direction") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->waterMassCurrent, DM_ASYNC); if (checkProvider(status, "Doppler System - water-mass layer current") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->waterMassDirection, DM_ASYNC); if (checkProvider(status, "Doppler System - water-mass layer direction") == ERROR) return(ERROR); status = dm_start_provider(dopplerDmi->response, DM_STATIC); if (checkProvider(status, "Doppler System - command response") == ERROR) return(ERROR); /* start multiple provider items and check status */ status = dm_start_multiple(dopplerDmi->startPinging); if (checkProvider(status, "Doppler System - start pinging") == ERROR) return(ERROR); /* initialize doppler data as invalid */ gettimeofday(&sampleTime, (struct timezone *) NULL); status = dm_write(dopplerDmi->bottomTrackDataValid, (Void *) &bottomTrackDataValid, sizeof(bottomTrackDataValid), &sampleTime); checkWrite(status, "Doppler System - bottom track data valid flags"); status = dm_write(dopplerDmi->waterMassDataValid, (Void *) &waterMassDataValid, sizeof(waterMassDataValid), &sampleTime); checkWrite(status, "Doppler System - water-mass layer data valid flags"); /* start consumer items and check status */ status = dm_start_consumer(dopplerDmi->dopplerPower, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - doppler power") == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->startPinging, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - start pinging command", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->interrupt, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - interrupt ADV", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->command, DM_STATIC, SEM_NULL); if (checkConsumer(status, "Doppler System - user command", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->kalmanAttitude, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Doppler System - Kalman filter attitude", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->vehicleRoll, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Doppler System Input - Inclinometer Roll", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(dopplerDmi->vehiclePitch, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Doppler System Input - Inclinometer Pitch", FALSE) == ERROR) return(ERROR); /* create data manager group */ *dopplerGroup = dm_create_group(); /* add data manager items to group */ status = dm_group_add_item(*dopplerGroup, dopplerDmi->startPinging, &dopplerId->startPinging); if (checkGroup(status, "Doppler System - start pinging command") == ERROR) return(ERROR); status = dm_group_add_item(*dopplerGroup, dopplerDmi->interrupt, &dopplerId->interrupt); if (checkGroup(status, "Doppler System - interrupt ADV") == ERROR) return(ERROR); status = dm_group_add_item(*dopplerGroup, dopplerDmi->command, &dopplerId->command); if (checkGroup(status, "Doppler System - user command") == ERROR) return(ERROR); return(OK); } /* dopplerDMInit */ /****************************************************************************/ /* Function : sonarDconTask */ /* Purpose : Initializes communication with Sonar Data Concentrator. */ /* Provides interface between Sonar Data Concentrator and */ /* Data Manager items. */ /* Inputs : SIO32 serial channel number for Sonar Data Concentrator */ /* Physically maps the Data Concentrators to a serial channel */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS sonarDconTask( Nat16 sonarDconSerialChan ) { sonarDconDmItems sonarDmItems; /* Sonar Application Data Mgr handles */ microIOControl microIOCtrl; /* Microcontroller IO Task control */ Int16 lps; /* Low Power Switch Board Index */ /* Initialize serial channel and task */ /* synchronization resources */ if (initMicroTaskIO( µIOCtrl, NUM_TASKS, sonarDconSerialChan, sonarIbcDmPrefix, "/sio32/sonarDcon", SONAR_DCON_SWITCH_STATUS_DM ) == ERROR) { logMsg("Error initializing IO for Sonar DCon"); return(ERROR); } /* if */ /* Clear Sonar DCon Data Manager handles */ bzero((char *) &sonarDmItems, sizeof(sonarDmItems)); /* Create IBC CPU Data Manager Items */ if (createIbcCpuDmItems(&(sonarDmItems.ibcDm), sonarIbcDmPrefix, sonarDconName) == ERROR) { /* Error occurred so print message & exit */ sio32ChanDestroy( sonarDconSerialChan, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ /* Create Sonar Dcon Application Items */ if (createSonarDconAppDmItems(&sonarDmItems, sonarIbcDmPrefix, sensorDmPrefix, sonarDconName) == ERROR) { /* Error occurred so print message & exit */ sio32ChanDestroy( sonarDconSerialChan, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ /* Create Virtual Serial Port Data Manager Items */ if (createIbcVspDmItems(&sonarDmItems.vspDm, sonarIbcDmPrefix, sonarDconName, 1 )== ERROR) { /* Error occured so print message & exit */ sio32ChanDestroy( sonarDconSerialChan, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ initIbcVspProvider (&sonarDmItems.vspDm); /* Create GF/5V Board Data Manager Items */ createIbcGf5vDmItems( &sonarDmItems.gf5vDm, sonarIbcDmPrefix, sonarDconName); initIbcGf5vAlarmProvider( &sonarDmItems.gf5vDm ); for (lps = 0; lps < LPS_BOARDS; lps++) { /* Create IBC LPS Data Manager Items */ createIbcLpsDmItems( &sonarDmItems.lpsDm[lps], sonarIbcDmPrefix, sonarDconName, lps ); initIbcLpsAlarmProvider( &sonarDmItems.lpsDm[lps] ); } /* for */ /* Creat Quad Serial Board Data Mgr Items*/ createIbcQuadSerDmItems( &sonarDmItems.quadSerDm, sonarIbcDmPrefix, sonarDconName, 0); initIbcQuadSerAlarmProvider( &sonarDmItems.quadSerDm ); /* Initialize Sonar Dcon Data Out Task */ if ((microIOCtrl.childTasks[OUT_TASK].taskId = taskSpawn("sonarDconOut", SONAR_IBC_OUT_PRIORITY, 0, SONAR_IBC_OUT_STACKSIZE, (FUNCPTR) sonarDconOutTask, (int) &sonarDmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { /* Close serial communication channel */ sio32ChanDestroy( sonarDconSerialChan, µIOCtrl.sio32DataChan ); logMsg("Sonar Dcon: Error spawning data output task\n"); return(ERROR); } /* if */ /* Initialize Sonar Dcon Data In Task */ if (( microIOCtrl.childTasks[IN_TASK].taskId = taskSpawn( "sonarDconIn", SONAR_IBC_IN_PRIORITY, 0, SONAR_IBC_IN_STACKSIZE, (FUNCPTR) sonarDconInTask, (int) &sonarDmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0) ) == ERROR) { /* Delete Sonar Dcon Out Task */ taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); sio32ChanDestroy( sonarDconSerialChan, µIOCtrl.sio32DataChan ); logMsg("Sonar Dcon: Error spawning data input task\n"); return(ERROR); } /* if */ /* Initialize Sonar Dcon Sensor In Task */ if (( microIOCtrl.childTasks[AUX_IN_TASK].taskId = taskSpawn( "sonarDconSensorIn", SONAR_IBC_SENSOR_PRIORITY, VX_FP_TASK, SONAR_TASK_STACKSIZE, (FUNCPTR) sonarDconSensorInTask, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0, 0) ) == ERROR) { /* Delete Sonar Dcon Out and IN Tasks */ taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); taskDelete(microIOCtrl.childTasks[IN_TASK].taskId); sio32ChanDestroy( sonarDconSerialChan, µIOCtrl.sio32DataChan ); logMsg("Sonar Dcon: Error spawning sensor input task\n"); return(ERROR); } /* if */ FOREVER { /* Check if Dcon is switched on */ if ((microDevPowerSwitchState(µIOCtrl) != SWITCH_ON) || ((microDevPowerSwitchState(µIOCtrl) == SWITCH_ON) && (microIOCtrl.resetEventCount != 0))) /* Process SRQ's from micro */ while(processSonarDconSRQ(&sonarDmItems, µIOCtrl) != MICRO_RESET_SRQ); microIOCtrl.resetEventCount++; /* Count reset events ffrom micro */ /* Reset event received from Micro so */ /* Mark serial coms link as up */ setSerialLinkState(µIOCtrl, LINK_UP); /* Initialize Static IBC dm items */ if (initIbcCpuDmItems( &sonarDmItems.ibcDm, µIOCtrl.sio32DataChan, sonarDconName, ibcCardSet ) == OK) { /* Initialize Sonar Dcon Applic items */ initSonarDconDmItems( &sonarDmItems, µIOCtrl.sio32DataChan ); updateIbcGf5vAlarmsDm( µIOCtrl.sio32DataChan, GF5V_BOARD_ADDR, &sonarDmItems.gf5vDm ); for (lps = 0; lps < LPS_BOARDS; lps++) updateIbcLpsAlarmsDm( µIOCtrl.sio32DataChan, lpsBoardAddr[lps], &sonarDmItems.lpsDm[lps] ); updateIbcQuadSerAlarmsDm( µIOCtrl.sio32DataChan, SERIAL_BOARD_ADDR, &sonarDmItems.quadSerDm ); updateIbcVspDmItems( µIOCtrl.sio32DataChan, SERIAL_BOARD_ADDR, VSP_CHAN, &sonarDmItems.vspDm, sonarDconName ); /* Wake up output task to configure micro*/ semGive(microIOCtrl.childTasks[OUT_TASK].taskSyncSem); } /* if */ else setSerialLinkState(µIOCtrl, LINK_DOWN); } /* FOREVER */ /* Delete Child Tasks */ taskDelete(microIOCtrl.childTasks[IN_TASK].taskId); taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); taskDelete(microIOCtrl.childTasks[AUX_IN_TASK].taskId); /* Close serial communication channel */ sio32ChanDestroy( sonarDconSerialChan, µIOCtrl.sio32DataChan ); /* dm_stop_provider, dm_stop_consumer, etc are NOT */ /* required when the task exits as the Data Manager */ /* does this automatically when the task is deleted */ /* via taskDeleteHookAdd() and dm_task_exit functions*/ return(OK); /* That's all folks */ } /* sonarDconTask() */ /*****************************************************************************/ /* Function : initSonarDconDmItems */ /* Purpose : Reads parameters from Sonar Dcon & update Data Mgr items */ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : None, but Data Manager Items change */ /*****************************************************************************/ LOCAL Void initSonarDconDmItems( sonarDconDmItems* dmItems, sio32Chan* sonarDconDataChan ) { /* Set Micro Water Alarms initial value */ if (updateWaterAlarmDm(sonarDconDataChan, dmItems->canWaterAlarm, dmItems->jboxWaterAlarm) == ERROR) logMsg("Sonar Dcon: Error checking water alarm status\n"); /* Set Altimeter initial values */ initAltimeterDm(sonarDconDataChan, dmItems); } /* initSonarDconDmItems() */ /*****************************************************************************/ /* Function : sonarDconOutTask */ /* Purpose : Sonar Dcon Out Task. Read DM items & write to Sonar Dcon */ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void sonarDconOutTask(Reg sonarDconDmItems *dmItems, microIOControl *microIOCtrl) { DM_Group cpuGroup; /* CPU & Misc Item Data Manager Group */ DM_Group gf5vGroup; /* GF/5V Board Data Manager Group */ DM_Group lps48VGroup; /* LPS 48V Board Data Manager Group */ DM_Group lps24VGroup; /* LPS 24V Board Data Manager Group */ DM_Group vspGroup; /* VSP Data Manager Group */ SEM_ID dmUpdateSem; /* Data Manager Item Change wakeup sem */ SEM_ID powerReqSem; /* Power Request Update Semaphore */ cpuUpdateBits ibcCpuBits; /* CPU Board Parameter Update Bits */ gf5vUpdateBits gf5vBits; /* GF/5V Board Paramater Update Bits */ lpsUpdateBits lps48VBits; /* LPS 48V Board Paramater Update Bits */ lpsUpdateBits lps24VBits; /* LPS 24V Board Paramater Update Bits */ vspUpdateBits vspBits; /* VSP Parameter Update Bits */ DWord sonarDconGroupBits; /* Holds dm_group_changes bit vector */ DWord waterAlarmThreshBit; /* Water alarm threshold item change bit */ DWord lps48VEnableBit; /* LPS 48 V Enable Request changed bit */ DWord lps24VEnableBit; /* LPS 24 V Enable Request changed bit */ DWord dopplerSwitchBit; /* Doppler Switch Request changed bit */ DWord altimeterSwitchBit; /* Altimeter Switch Request changed bit */ DWord responderSwitchBit; /* Responder Switch Request changed bit */ DWord homerSwitchBit; /* Homer Pro Switch Request changed bit */ DWord altimeterRangeBit; /* Altimeter Range param changed bit */ DWord altimeterGainBit; /* Altimeter Gain param changed bit */ DWord sonarDconSwitchBit; /* Sonar Dcon Switch Request changed bit */ /* Sonar Dcon LPS Output Enable switches */ switchEntry sonarLps48VEnableSwitch; switchEntry sonarLps24VEnableSwitch; switchEntry sonarDconSwitch; /* Sonar DCon Switch */ switchEntry altimeterSwitch; /* Altimeter Power Switch */ switchEntry dopplerSwitch; /* Doppler Power Switch */ switchEntry responderSwitch; /* Responder Power Switch */ switchEntry homerSwitch; /* Homer Pro Power Switch */ /******************************************************************************/ /* The code is this task implements the following interconnected switches for the sonar data concentrator, altimeter and Doppler. Sonar Dcon 48V LPS Enable LPS 0-4 48V Bus _____ \_____________ \______________ \____ Doppler 48V LPS _____| | | | | |_ \_____ spare 48V LPS ______| | | | | |_ \_____ spare 48V LPS ______| | | | | |_ \_____ spare 48V LPS ______| | | | 24V LPS Enable LPS 0-4 | |________ \______________ \________ altimeter ________| | | |_ \_____ Homer Pro ________| | | |_ \________ responder _ ______| | | |_ \_____ spare 24V LPS ______| */ /******************************************************************************/ /* Create switch & power sync semaphores */ if ( ((dmUpdateSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) || ((powerReqSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) ) { logMsg("Sonar Dcon: Error initializing Input Task Resources\n"); return; } /* if */ duplicateIbcSwitchEntry( &sonarDconSwitch, SONAR_DCON_SWITCH_REQ_DM, SONAR_DCON_SWITCH_STATUS_DM, dmUpdateSem, sonarDconName, "Sonar DCon Switch"); initIbcSwitchEntry( &sonarLps48VEnableSwitch, SERIAL_CHAN, VICOR_STANDBY_POWER, B48, HOTEL, LOW_POWER_SWITCH, lpsBoardAddr[0], LPS_VICOR_ENABLE, SONAR_LPS0_48V_ENABLE_SWITCH_REQ_DM, SONAR_LPS0_48V_ENABLE_STATUS_DM, dmUpdateSem, SONAR_LPS0_48V_ENABLE_PWR_REQ_DM, SONAR_LPS0_48V_ENABLE_PWR_OK_DM, powerReqSem, sonarDconName, "LPS 48V Enable"); initIbcSwitchEntry( &dopplerSwitch, SERIAL_CHAN, DOPPLER_SONAR_POWER, B48, HOTEL, LOW_POWER_SWITCH, lpsBoardAddr[0], DOPPLER_SONAR_SWITCH_CHAN, DOPPLER_SONAR_SWITCH_REQ_DM, DOPPLER_SONAR_SWITCH_STATUS_DM, dmUpdateSem, DOPPLER_SONAR_PWR_REQ_DM, DOPPLER_SONAR_PWR_OK_DM, powerReqSem, sonarDconName, "Doppler Switch"); initIbcSwitchEntry( &sonarLps24VEnableSwitch, SERIAL_CHAN, VICOR_STANDBY_POWER, B48, HOTEL, LOW_POWER_SWITCH, lpsBoardAddr[1], LPS_VICOR_ENABLE, SONAR_LPS1_24V_ENABLE_SWITCH_REQ_DM, SONAR_LPS1_24V_ENABLE_STATUS_DM, dmUpdateSem, SONAR_LPS1_24V_ENABLE_PWR_REQ_DM, SONAR_LPS1_24V_ENABLE_PWR_OK_DM, powerReqSem, sonarDconName, "LPS 24V Enable"); initIbcSwitchEntry( &altimeterSwitch, SERIAL_CHAN, ALTIMETER_POWER, B48, HOTEL, LOW_POWER_SWITCH, lpsBoardAddr[1], ALTIMETER_SWITCH_CHAN, ALTIMETER_SWITCH_REQ_DM, ALTIMETER_SWITCH_STATUS_DM, dmUpdateSem, ALTIMETER_PWR_REQ_DM, ALTIMETER_PWR_OK_DM, powerReqSem, sonarDconName, "Altimeter Switch"); initIbcSwitchEntry( &homerSwitch, SERIAL_CHAN, HOMER_POWER, B48, HOTEL, LOW_POWER_SWITCH, lpsBoardAddr[1], HOMER_SWITCH_CHAN, HOMER_SWITCH_REQ_DM, HOMER_SWITCH_STATUS_DM, dmUpdateSem, HOMER_PWR_REQ_DM, HOMER_PWR_OK_DM, powerReqSem, sonarDconName, "Homer Switch"); initIbcSwitchEntry( &responderSwitch, SERIAL_CHAN, RESPONDER_POWER, B48, HOTEL, LOW_POWER_SWITCH, lpsBoardAddr[1], RESPONDER_SWITCH_CHAN, RESPONDER_SWITCH_REQ_DM, RESPONDER_SWITCH_STATUS_DM, dmUpdateSem, RESPONDER_PWR_REQ_DM, RESPONDER_PWR_OK_DM, powerReqSem, sonarDconName, "Responder Switch"); ibcAddSwitchInSeries( &sonarDconSwitch, &sonarLps48VEnableSwitch ); ibcAddSwitchInSeries( &sonarDconSwitch, &sonarLps24VEnableSwitch ); /* Devices on Sonar Can Low Power Switches*/ ibcAddSwitchInSeries( &sonarLps48VEnableSwitch, &dopplerSwitch ); ibcAddSwitchInSeries( &sonarLps24VEnableSwitch, &altimeterSwitch ); ibcAddSwitchInSeries( &sonarLps24VEnableSwitch, &homerSwitch ); ibcAddSwitchInSeries( &sonarLps24VEnableSwitch, &responderSwitch ); ibcConnectLpsDmItems( &dmItems->lpsDm[0], &dopplerSwitch, sonarDconName, "Doppler", DOPPLER_SONAR_SWITCH_CHAN, DOPPLER_SONAR_CURRENT_DM, DOPPLER_SONAR_CURRENT_ALARM_DM, DOPPLER_SONAR_CURRENT_WARN_THRESH_DM, DOPPLER_SONAR_CURRENT_ALARM_THRESH_DM); ibcConnectLpsDmItems( &dmItems->lpsDm[1], &altimeterSwitch, sonarDconName, "Altimeter", ALTIMETER_SWITCH_CHAN, ALTIMETER_CURRENT_DM, ALTIMETER_CURRENT_ALARM_DM, ALTIMETER_CURRENT_WARN_THRESH_DM, ALTIMETER_CURRENT_ALARM_THRESH_DM ); ibcConnectLpsDmItems( &dmItems->lpsDm[1], &homerSwitch, sonarDconName, "Homer Pro", HOMER_SWITCH_CHAN, HOMER_CURRENT_DM, HOMER_CURRENT_ALARM_DM, HOMER_CURRENT_WARN_THRESH_DM, HOMER_CURRENT_ALARM_THRESH_DM ); ibcConnectLpsDmItems( &dmItems->lpsDm[1], &responderSwitch, sonarDconName, "Responder", RESPONDER_SWITCH_CHAN, RESPONDER_CURRENT_DM, RESPONDER_CURRENT_ALARM_DM, RESPONDER_CURRENT_WARN_THRESH_DM, RESPONDER_CURRENT_ALARM_THRESH_DM ); /* Create Data Manager Group for items */ cpuGroup = initIbcCpuConsumer(&dmItems->ibcDm, &ibcCpuBits, dmUpdateSem); /* Use CPU Group for misc parameters */ dm_start_consumer(dmItems->waterAlarmThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->altimeterRange, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->altimeterGain, DM_STATIC, dmUpdateSem); dm_group_add_item(cpuGroup, sonarDconSwitch.switchStatusDm, &sonarDconSwitchBit); dm_group_add_item(cpuGroup, dmItems->waterAlarmThresh, &waterAlarmThreshBit); dm_group_add_item(cpuGroup, dmItems->altimeterRange, &altimeterRangeBit); dm_group_add_item(cpuGroup, dmItems->altimeterGain, &altimeterGainBit); /* Create GF/5V Data Manager Group */ gf5vGroup = initIbcGf5vConsumer( &dmItems->gf5vDm, &gf5vBits, dmUpdateSem ); /* Create Virtual Serial Port Data Manager Group */ vspGroup = initIbcVspConsumer( &dmItems->vspDm, &vspBits, dmUpdateSem ); /* Create LPS Data Manager Group */ lps48VGroup = initIbcLpsConsumer( &dmItems->lpsDm[0], &lps48VBits, dmUpdateSem ); lps24VGroup = initIbcLpsConsumer( &dmItems->lpsDm[1], &lps24VBits, dmUpdateSem ); /* Add Switch Requests to DM Group */ dm_group_add_item(cpuGroup, sonarLps48VEnableSwitch.switchRequestDm, &lps48VEnableBit); dm_group_add_item(cpuGroup, sonarLps24VEnableSwitch.switchRequestDm, &lps24VEnableBit); dm_group_add_item(cpuGroup, dopplerSwitch.switchRequestDm, &dopplerSwitchBit); dm_group_add_item(cpuGroup, altimeterSwitch.switchRequestDm, &altimeterSwitchBit); dm_group_add_item(cpuGroup, homerSwitch.switchRequestDm, &homerSwitchBit); dm_group_add_item(cpuGroup, responderSwitch.switchRequestDm, &responderSwitchBit); /* Clear changed bits before we start */ sonarDconGroupBits = dm_get_group_changes(cpuGroup); FOREVER { /* Mark micro configuration as invalid */ microIOCtrl->microConfigured = FALSE; /* Wait for micro reset via SRQ Task */ semTake(microIOCtrl->childTasks[OUT_TASK].taskSyncSem, WAIT_FOREVER); /* We need to initialize default state of Data manager Items here */ /* Initialize DM item values and default */ /* IBC Switch states */ ibcSetSwitchState (&dopplerSwitch, SWITCH_OFF); ibcSetSwitchState (&altimeterSwitch, SWITCH_OFF); ibcSetSwitchState (&homerSwitch, SWITCH_OFF); ibcSetSwitchState (&responderSwitch, SWITCH_ON ); ibcSetSwitchState (&sonarLps48VEnableSwitch, SWITCH_ON ); ibcSetSwitchState (&sonarLps24VEnableSwitch, SWITCH_ON ); /* Setup Switch Delay for Doppler LPS */ if (IBC_LPS_SetSwitchDelay( SERIAL_CHAN, lpsBoardAddr[0], DOPPLER_SWITCH_DELAY) == ERROR) logMsg("Sonar Dcon: Error setting switch delay\n"); initIbcHumidityThreshDm(SERIAL_CHAN, dmItems->ibcDm.humidityWarnThresh, dmItems->ibcDm.humidityAlarmThresh); initWaterAlarmThreshDm(SERIAL_CHAN, dmItems->waterAlarmThresh); updateIbcGf5vDmItems( SERIAL_CHAN, GF5V_BOARD_ADDR, &dmItems->gf5vDm, sonarDconName ); setGf5vChargeTimes( &dmItems->gf5vDm, LOW_VOLTAGE_GF_CHARGE_TIME, LOW_VOLTAGE_GF_CHARGE_TIME, LOW_VOLTAGE_GF_CHARGE_TIME, LOW_VOLTAGE_GF_CHARGE_TIME ); enableGf5vMonitoring( SERIAL_CHAN, GF5V_BOARD_ADDR, &dmItems->gf5vDm, TRUE ); runGf5vBoardSelftest( SERIAL_CHAN, GF5V_BOARD_ADDR, &dmItems->gf5vDm ); updateIbcVspDmItems( SERIAL_CHAN, SERIAL_BOARD_ADDR, VSP_CHAN, &dmItems->vspDm, sonarDconName ); updateIbcLpsDmItems( SERIAL_CHAN, lpsBoardAddr[0], &dmItems->lpsDm[0], sonarDconName ); updateIbcLpsDmItems( SERIAL_CHAN, lpsBoardAddr[1], &dmItems->lpsDm[1], sonarDconName ); /* Clear changed item bits in DM */ sonarDconGroupBits = dm_get_group_changes(cpuGroup); /* Flag that micro has been configured */ microIOCtrl->microConfigured = TRUE; /* Wake up input task to read from micro */ semGive(microIOCtrl->childTasks[IN_TASK].taskSyncSem); /* Wake up task to check Aux In status */ semGive(microIOCtrl->childTasks[AUX_IN_TASK].taskSyncSem); while (microIOCtrl->serialLinkState == LINK_UP) { /* Wait for any data items to change */ semTake(dmUpdateSem, WAIT_FOREVER); /* Ask Data Manager which items changed */ sonarDconGroupBits = dm_get_group_changes(cpuGroup); /* Check if micro Reset Event occurred */ if (sonarDconGroupBits & ibcCpuBits.resetEventBit) break; /* Exit loop and re-initialize DM items */ taskSafe(); /* Prevent task from being deleted during*/ /* serial IO and Data Manager updates */ /* Sonar Dcon Power Switch Request */ if (sonarDconGroupBits & sonarDconSwitchBit) { ibcProcessDupSwitchRequest( &sonarDconSwitch ); if (microDevPowerSwitchState(microIOCtrl) != SWITCH_ON) { /* DCon is switched off so link is down */ setSerialLinkState(microIOCtrl, LINK_DOWN); taskSafe(); break; } /* if */ } /* if */ /* Sonar Can LPS Output Enable Request */ if (sonarDconGroupBits & lps48VEnableBit) ibcProcessSwitchRequest( &sonarLps48VEnableSwitch ); if (sonarDconGroupBits & lps24VEnableBit) ibcProcessSwitchRequest( &sonarLps24VEnableSwitch ); /* Doppler Power Switch Request */ if (sonarDconGroupBits & dopplerSwitchBit) { ibcSetSwitchState (&sonarLps48VEnableSwitch, SWITCH_ON ); ibcProcessSwitchRequest( &dopplerSwitch ); /* Wait 100 milliseconds for ADV Power */ taskDelay(sysClkRateGet() / (USECS_PER_SEC / POWER_DELAY)); } /* if */ /* Altimeter Power Switch Request */ if (sonarDconGroupBits & altimeterSwitchBit) { ibcSetSwitchState (&sonarLps24VEnableSwitch, SWITCH_ON ); ibcProcessSwitchRequest( &altimeterSwitch ); setAltimeterRange(SERIAL_CHAN, dmItems->altimeterRange); setAltimeterGain(SERIAL_CHAN, dmItems->altimeterGain); } /* if */ /* Homer Pro Power Switch Request */ if (sonarDconGroupBits & homerSwitchBit) { ibcSetSwitchState (&sonarLps24VEnableSwitch, SWITCH_ON ); ibcProcessSwitchRequest( &homerSwitch ); } /* if */ /* Responder Power Switch Request */ if (sonarDconGroupBits & responderSwitchBit) { ibcSetSwitchState (&sonarLps24VEnableSwitch, SWITCH_ON ); ibcProcessSwitchRequest( &responderSwitch ); } /* if */ /* Check for GF/5V Board Parameter Update*/ ibcGf5vUpdateCheck( SERIAL_CHAN, GF5V_BOARD_ADDR, &dmItems->gf5vDm, &gf5vBits, gf5vGroup ); /* Check for Virtual Serial Port Parameter Update*/ ibcVspUpdateCheck( SERIAL_CHAN, SERIAL_BOARD_ADDR, VSP_CHAN, &dmItems->vspDm, &vspBits, vspGroup ); /* Check for LPS Board Parameter Update */ ibcLpsUpdateCheck( SERIAL_CHAN, lpsBoardAddr[0], &dmItems->lpsDm[0], &lps48VBits, lps48VGroup ); /* Check for LPS Board Parameter Update */ ibcLpsUpdateCheck( SERIAL_CHAN, lpsBoardAddr[1], &dmItems->lpsDm[1], &lps24VBits, lps24VGroup ); /* Send Set Humidity Alarm Threshold cmd*/ if ((sonarDconGroupBits & ibcCpuBits.humidityWarnThreshBit) || (sonarDconGroupBits & ibcCpuBits.humidityAlarmThreshBit)) writeIbcHumidityThresh(SERIAL_CHAN, dmItems->ibcDm.humidityWarnThresh, dmItems->ibcDm.humidityAlarmThresh); /* Send Set Water Alarm Threshold cmd */ if (sonarDconGroupBits & waterAlarmThreshBit) writeWaterAlarmThresh(SERIAL_CHAN, dmItems->waterAlarmThresh); /* Send Set Altimeter Range cmd */ if (sonarDconGroupBits & altimeterRangeBit) setAltimeterRange(SERIAL_CHAN, dmItems->altimeterRange); /* Send Set Altimeter Gain cmd */ if (sonarDconGroupBits & altimeterGainBit) setAltimeterGain(SERIAL_CHAN, dmItems->altimeterGain); /* Check if Reset Command request occurred*/ if (sonarDconGroupBits & ibcCpuBits.resetCmdBit) { /* Mark serial link as down */ setSerialLinkState(microIOCtrl, LINK_DOWN); /* Manual reset so announce it */ microIOCtrl->announceResets = TRUE; /* Send Reset Command to Microcontroller */ microResetCmd(SERIAL_CHAN); break; /* Exit loop and wait for RESET */ } /* if */ taskUnsafe(); /* Task may now be safely deleted */ } /* while (LINK_UP) */ } /* FOREVER */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are NOT required when the task exits as the */ /* Data Manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ /* That's all folks ! */ } /* sonarDconOutTask() */ /*****************************************************************************/ /* Function : sonarDconInTask */ /* Purpose : Sonar Dcon Input Task. Wakes up periodically, read data */ /* from the Sonar Dcon and write Data Manager Items */ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void sonarDconInTask(Reg sonarDconDmItems* dmItems, microIOControl *microIOCtrl) { SEM_ID wakeupSem; /* Used by watch dog to wake up this task*/ Int32 updates = 0; /* Counts dm updates for rate calculation*/ /* Create wakeup semaphore for provider */ /* rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Sonar Dcon: Error initializing Input Task Resources\n"); return; } /* if */ /* Provider Task for non-static data items*/ initIbcCpuProvider (&dmItems->ibcDm, HUMIDITY_DM_PERIOD); initIbcGf5vProvider (&dmItems->gf5vDm, SONAR_IBC_DM_PERIOD); initIbcLpsProvider (&dmItems->lpsDm[0], SONAR_IBC_DM_PERIOD); initIbcLpsProvider (&dmItems->lpsDm[1], SONAR_IBC_DM_PERIOD); FOREVER { /* Wait for IBC Coms to be initialized */ semTake(microIOCtrl->childTasks[IN_TASK].taskSyncSem, WAIT_FOREVER); while (microIOCtrl->serialLinkState == LINK_UP) { taskSafe(); /* Prevent task from being deleted during */ /* serial IO and Data Manager updates */ if ((updates % (HUMIDITY_DM_PERIOD / SONAR_IBC_DM_PERIOD)) == 0) updateIbcHumidityDm( SERIAL_CHAN, dmItems->ibcDm.microHumidity ); updateNewIbcGf5vDm(SERIAL_CHAN, GF5V_BOARD_ADDR, &dmItems->gf5vDm ); updateIbcLpsDm(SERIAL_CHAN, lpsBoardAddr[0], &dmItems->lpsDm[0] ); updateIbcLpsDm(SERIAL_CHAN, lpsBoardAddr[1], &dmItems->lpsDm[1] ); taskUnsafe(); /* task may now be deleted safely */ /* increment & range check update counter */ if (++updates == 10000) updates = 0; semTake(wakeupSem, /* pend until sem timeout wakes us up */ sysClkRateGet() / ( USECS_PER_SEC / SONAR_IBC_DM_PERIOD)); } /* while */ } /* FOREVER */ semDelete(wakeupSem); /* reclaim semaphore resource */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are not required when the task exits as the */ /* data manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ /* that's all folks ! */ } /* sonarDConInTask() */ /*****************************************************************************/ /* Function : sonarDconSensorInTask */ /* Purpose : Sonar Dcon Sensor Task. Wakes up periodically, read data */ /* from the Sonar Dcon and write Data Manager Items */ /* Inputs : Array of Data Manager Item handles & Micro IO Ctrl structure*/ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void sonarDconSensorInTask(microIOControl *microIOCtrl) { SEM_ID wakeupSem; /* Used wake up this task periodically */ Nat32 ticks; Int16 rawAltitude; /* Raw altitude reading from sensor */ Nat16 sigLevel; /* Altimeter signal level */ Flt32 altitude; /* Sensor values from Sonar IBC */ MedianFilter altitudeFilter; /* Filter for altimeter sensor dropout */ dopplerDMItems dopplerDmi; /* doppler data manager items */ Errno status, xStatus, /* data manager error codes */ yStatus, zStatus; Byte dvlDataStatus; /* velocity & dvl data status bits */ Byte dvlDataStatusMask; /* velocity status bit mask */ Int32 bottomStatus; /* More bottom velocity status bits */ Int32 waterStatus; /* Water-referenced velocity status */ Int32 lastBottomStatus= 0;/* bottom velocity status bits state */ Int32 lastWaterStatus = 0;/* Water-ref velocity status state */ Int32 dvlReplyCounter=0;/* Counts no-responses from the DVL */ WDOG_ID dopplerPeriod; /* watchdog timer period */ DM_Group dopplerGroup; /* doppler data manager group */ DWord dopplerGroupBits; /* doppler group changes bit vector */ dopplerIdBits dopplerId; /* doppler id bits */ /* data valid flags */ MBool altitudeDataValid = FALSE; MBool bottomTrackDataValid = FALSE; MBool waterMassDataValid = FALSE; MBool dvlCommsValid = FALSE; MBool init = FALSE; /* initialization flag */ MBool pinging = FALSE; /* pinging activated flag */ MBool prevPinging = FALSE;/* previous pinging activated flag */ Int16 dof; /* dof counter */ Int16 order; /* order counter */ Flt32 dvlTemp; /* dvl temp in Deg. C */ Flt32 dvlRange; /* Range to bottom in DVL coord's, m. */ Flt32 dvlAltitude; /* Altitude from DVL, m. */ /* bottom track velocity measurements */ Flt32 bottomTrackVelocity[NXE]; Flt32 filtBottomTrackVelocity[NX]; Flt64 bottomTrackVelocityPrev[NX][BUTTER_ORDER2]; Flt64 filtBottomTrackVelocityPrev[NX][BUTTER_ORDER2]; /* water-mass velocity measurements */ Flt32 waterMassVelocity[NX]; Flt32 filtWaterMassVelocity[NX]; Flt64 waterMassVelocityPrev[NX][BUTTER_ORDER2]; Flt64 filtWaterMassVelocityPrev[NX][BUTTER_ORDER2]; /* vehicle velocity */ Flt32 vehicleBottomTrackVelocity[NX]; Flt32 vehicleWaterMassVelocity[NX]; Flt32 bottomTrackCurrent; /* water current (magnitude) */ Flt32 waterMassCurrent; /* water current (magnitude) */ Flt32 bottomTrackAngle; /* water current angle */ Flt32 waterMassAngle; /* water current angle */ Flt32 bottomTrackDirection;/* water current direction */ Flt32 waterMassDirection; /* water current direction */ Flt32 kalmanHeadingDegrees;/* Kalman filter heading */ Flt32 vehicleRoll; /* Roll from the inclinometer */ Flt32 vehiclePitch; /* Pitch from the inclinometer */ VehicleAttitude kalmanAttitude; /* Kalman filter attitude */ DM_Time sampleTime; /* data manager item sample time */ switchStatus dopplerPower; /* power switch status */ sonarDconSensorItems dmItems; /* Sonar Application Data Mgr handles */ /* Clear Sonar DCon Data Manager handles */ bzero((char *) &dmItems, sizeof(dmItems)); /* Create Sonar Dcon Application Items */ if (createSonarDconSensorDmItems(&dmItems, sensorDmPrefix, sonarDconName) == ERROR) { /* Error occurred so print message & exit */ logMsg("Sonar Dcon: Error initializing Sonar Data Manager Items\n"); return; } /* if */ /* create data manager items and start */ /* providers for ADV Doppler */ if (dopplerDMInit(&dopplerDmi, &dopplerGroup, &dopplerId) == ERROR) { /* Error occurred so print message & exit */ logMsg("Sonar Dcon: Error initializing ADV Data Manager Items\n"); return; } /* if */ /* Create wakeup semaphore and watch dog */ /* timer for provider rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Sonar Dcon: Error initializing Input Task Resources\n"); return; } /* if */ /* initialize altimeter data valid */ writeDmItem(dmItems.altitudeDataValid, &altitudeDataValid, sizeof(altitudeDataValid)); /* initialize altimeter median filter */ initMedianFilter(&altitudeFilter, ALTIMETER_FILTER_SAMPLES); /* initialize velocity filter values */ for (order = 0; order < BUTTER_ORDER2; order++) for (dof = 0; dof < NX; dof++) { bottomTrackVelocityPrev[dof][order] = 0.0; filtBottomTrackVelocityPrev[dof][order] = 0.0; waterMassVelocityPrev[dof][order] = 0.0; filtWaterMassVelocityPrev[dof][order] = 0.0; } FOREVER { /* Wait for Dcon Coms to be initialized */ semTake(microIOCtrl->childTasks[AUX_IN_TASK].taskSyncSem, WAIT_FOREVER); init = FALSE; /* Doppler is uninitialized */ while (microIOCtrl->serialLinkState == LINK_UP) { taskSafe(); /* Prevent task from being deleted during */ /* serial IO and Data Manager updates */ #ifndef REPLACE_ALT_W_DVL if (sonarCanReadSensors(SERIAL_CHAN, &rawAltitude, &sigLevel ) == OK) { if (rawAltitude <= ALTIMETER_MAX_RANGE) { altitude = (Flt32) rawAltitude * ALTIMETER_RANGE_UNITS * ALTIMETER_SOS / ALTIMETER_SENSOR_SOS; writeDmItem(dmItems.rawAltitude, &altitude, sizeof(altitude)); /* filter to remove altimeter data values */ /* that dropout to zero */ altitude = medianFilter(&altitudeFilter, altitude); writeDmItem(dmItems.altimeterSig, &sigLevel, sizeof(sigLevel)); writeDmItem(dmItems.altitude, &altitude, sizeof(altitude)); /* flag any zero altimeter data values */ if (fabs(altitude) < 0.00001) /* data invalid */ { if (altitudeDataValid) /* set data valid flag FALSE, */ { /* and update DM item */ altitudeDataValid = FALSE; writeDmItem(dmItems.altitudeDataValid, &altitudeDataValid, sizeof(altitudeDataValid)); } } else /* data valid */ { if (!altitudeDataValid) /* set data valid flag TRUE, */ { /* and update DM item */ altitudeDataValid = TRUE; writeDmItem(dmItems.altitudeDataValid, &altitudeDataValid, sizeof(altitudeDataValid)); } } } /* if */ } /* if */ #endif status = dm_read(dopplerDmi.dopplerPower, (char *) &dopplerPower, sizeof(dopplerPower), (DM_Time *) NULL); if ((dopplerPower == SWITCH_ON) && (init == FALSE)) { if (initAdvSerial(SERIAL_CHAN) == ERROR) { logMsg("Doppler System FAILURE : could not initialize serial port\n"); init = FALSE; if( dvlCommsValid ) /* If still TRUE, flip it to FALSE.*/ { dvlCommsValid = FALSE; status = dm_write(dopplerDmi.commsValid, (Void *) &dvlCommsValid, sizeof(dvlCommsValid), &sampleTime); checkWrite(status, "Doppler System - Serial Comms Valid"); } /* if( dvlCommsValid ) */ } /* if */ /* Update Rate is not guaranteed due to */ /* the possibility of task preemption */ /* initialize ADV */ if (initDoppler(&dopplerDmi, SERIAL_CHAN) == ERROR) { logMsg("Doppler System FAILURE : could not initialize ADV\n"); init = FALSE; } /* if */ else { init = TRUE; if( !dvlCommsValid ) /* If still FALSE, flip it to TRUE.*/ { dvlCommsValid = TRUE; status = dm_write(dopplerDmi.commsValid, (Void *) &dvlCommsValid, sizeof(dvlCommsValid), &sampleTime); checkWrite(status, "Doppler System - Serial Comms Valid"); }/* if( !dvlCommsValid ) */ } } /* if */ else if (dopplerPower == SWITCH_OFF) { init = FALSE; pinging = FALSE; if (pinging != prevPinging) { gettimeofday(&sampleTime, (struct timezone *) NULL); status = dm_write(dopplerDmi.startPinging, (Void *) &pinging, sizeof(pinging), &sampleTime); checkWrite(status, "Doppler System - start pinging"); } if( dvlCommsValid ) /* If still TRUE, flip it to FALSE.*/ { dvlCommsValid = FALSE; status = dm_write(dopplerDmi.commsValid, (Void *) &dvlCommsValid, sizeof(dvlCommsValid), &sampleTime); checkWrite(status, "Doppler System - Serial Comms Valid"); } /* if( dvlCommsValid ) */ } if (init) { /* get ADV commands */ dopplerGroupBits = dm_get_group_changes(dopplerGroup); if (dopplerGroupBits != 0) { /* check for start pinging command */ if (dopplerGroupBits & dopplerId.startPinging) { dm_read(dopplerDmi.startPinging, (Void *) &pinging, sizeof(pinging), (DM_Time *) NULL); if (pinging) { if (startPinging(&dopplerDmi, SERIAL_CHAN) == ERROR) pinging = FALSE; } } /* check for interrupt command */ if (dopplerGroupBits & dopplerId.interrupt) if (wakeUpDoppler(&dopplerDmi, SERIAL_CHAN, (Word) SERIAL_BOARD_ADDR, (Word) ADV_WRITE_CHANNEL_NUMBER) == ERROR) logMsg("Doppler System FAILURE : Could not interrupt doppler\n"); else pinging = FALSE; /* check for user input commands */ if (dopplerGroupBits & dopplerId.command) commandDoppler(&dopplerDmi, SERIAL_CHAN); } /* if (dopplerGroupBits != 0) */ if (pinging) { /* ** Read DVL data, including velocity. Read other necessary ** variables, such as Kalman filter heading, from Data Manager. */ getDvlData(SERIAL_CHAN, bottomTrackVelocity, &bottomStatus, waterMassVelocity, &waterStatus, &dvlRange, &dvlTemp, &dvlDataStatus); dm_read(dopplerDmi.kalmanAttitude, (Void *) &kalmanAttitude, sizeof(kalmanAttitude), (DM_Time * ) NULL); if (kalmanAttitude.heading != NAN) kalmanHeadingDegrees = kalmanAttitude.heading * 180.0 / PI; else kalmanHeadingDegrees = 0.0; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_read(dopplerDmi.vehicleRoll, (Void *) &vehicleRoll, sizeof(vehicleRoll), (DM_Time * ) NULL); dm_read(dopplerDmi.vehiclePitch, (Void *) &vehiclePitch, sizeof(vehiclePitch), (DM_Time * ) NULL); /* ** Write the DVL Serial Comms Valid flag to Data Mangler. */ if ((dvlDataStatus & NO_DVL_RESPONSE) != NO_DVL_RESPONSE) { /* ** Serial comms are valid. Only write to Data Manager ** when the value changes. */ dvlReplyCounter = 0;/* Reset the no-response counter. */ if( !dvlCommsValid ) /* If still FALSE, flip it to TRUE.*/ { dvlCommsValid = TRUE; status = dm_write(dopplerDmi.commsValid, (Void *) &dvlCommsValid, sizeof(dvlCommsValid), &sampleTime); checkWrite(status, "Doppler System - Serial Comms Valid"); }/* if( !dvlCommsValid ) */ /* ** Write out Bottom-Track status and Water-Track status. ** The "if" statements prevent constant updating when ** the value hasn't changed. */ if( bottomStatus != lastBottomStatus ) { status = dm_write(dopplerDmi.bottomStatus, (Void *) &bottomStatus, sizeof(bottomStatus), &sampleTime); checkWrite(status, "Doppler System - Bottom-Track Status"); lastBottomStatus = bottomStatus; } if( waterStatus != lastWaterStatus ) { status = dm_write(dopplerDmi.waterStatus, (Void *) &waterStatus, sizeof(waterStatus), &sampleTime); checkWrite(status, "Doppler System - Water-Track Status"); lastWaterStatus = waterStatus; } status = dm_write(dopplerDmi.temp, (Void *) &dvlTemp, sizeof(dvlTemp), &sampleTime); checkWrite(status, "Doppler System - Temperature"); /* ** Extract bottom-track velocity and update data ** manager items. */ if ((dvlDataStatus & BAD_BOTTOM_TRACK_VELOCITY) != BAD_BOTTOM_TRACK_VELOCITY) { /* ** This is the case when the bottom-track data is VALID. ** ** If necessary, flip the Data Valid flag to TRUE. This ** "if" statement prevents constant updating when the flag ** hasn't changed value. */ if (!bottomTrackDataValid) { bottomTrackDataValid = TRUE; dm_write(dopplerDmi.bottomTrackDataValid, (Void *) &bottomTrackDataValid, sizeof(bottomTrackDataValid), &sampleTime); } /* if */ /* ** Convert bottom-track velocity to vehicle coordinates. */ vehicleBottomTrackVelocity[X_INDEX] = -bottomTrackVelocity[Y_INDEX]; vehicleBottomTrackVelocity[Y_INDEX] = -bottomTrackVelocity[X_INDEX]; vehicleBottomTrackVelocity[Z_INDEX] = -bottomTrackVelocity[Z_INDEX]; for (dof = 0; dof < NX; dof++) { status = dm_write(dopplerDmi.bottomTrackVelocity[dof], (Void *) &vehicleBottomTrackVelocity[dof], sizeof(vehicleBottomTrackVelocity[dof]), &sampleTime); checkWrite(status, "Doppler System - bottom track velocity"); } /* for */ /* ** Explicity write out the error component to Data Mangler. */ status = dm_write(dopplerDmi.bottomTrackVelocityError, (Void *) &bottomTrackVelocity[3], sizeof(bottomTrackVelocity[3]), &sampleTime); checkWrite(status, "Doppler System - bottom track velocity 4th component."); /* ** Compute the altitude from the range measurement. Beware ** that the roll and pitch measurements from the ** inclinometer ARE NOT Euler angles (formed by successive ** rotations). They are space-fixed angles. */ dvlAltitude = dvlRange/ sqrt( 1. + pow( tan( vehicleRoll ), 2. ) + pow( tan( vehiclePitch ), 2. ) ); /* ** Write out altitude to Data Mangler HERE. */ status = dm_write(dopplerDmi.altitude, (Void *) &dvlAltitude, sizeof(dvlAltitude), &sampleTime); checkWrite(status, "Doppler System - Altitude"); #ifdef REPLACE_ALT_W_DVL writeDmItem(dmItems.rawAltitude, &dvlAltitude, sizeof(dvlAltitude)); /* filter to remove altimeter data values */ /* that dropout to zero */ dvlAltitude = medianFilter(&altitudeFilter, dvlAltitude); writeDmItem(dmItems.altitude, &dvlAltitude, sizeof(dvlAltitude)); if (!altitudeDataValid) /* set data valid flag TRUE, */ { /* and update DM item */ altitudeDataValid = TRUE; writeDmItem(dmItems.altitudeDataValid, &altitudeDataValid, sizeof(altitudeDataValid)); } #endif /* filter bottom-track velocity */ for (dof = 0; dof < NX; dof++) { filtBottomTrackVelocity[dof] = velocityFilter((Flt64)vehicleBottomTrackVelocity[dof], bottomTrackVelocityPrev[dof], filtBottomTrackVelocityPrev[dof]); status = dm_write(dopplerDmi.filtBottomTrackVelocity[dof], (Void *) &filtBottomTrackVelocity[dof], sizeof(filtBottomTrackVelocity[dof]), &sampleTime); checkWrite(status, "Doppler System - filtered bottom track velocity"); } /* for */ /* update previous values */ for (dof = 0; dof < NX; dof++) { bottomTrackVelocityPrev[dof][1] = bottomTrackVelocityPrev[dof][0]; bottomTrackVelocityPrev[dof][0] = (Flt64) vehicleBottomTrackVelocity[dof]; filtBottomTrackVelocityPrev[dof][1] = filtBottomTrackVelocityPrev[dof][0]; filtBottomTrackVelocityPrev[dof][0] = (Flt64) filtBottomTrackVelocity[dof]; } /* calculate bottom-track current and direction from the x and y velocity */ bottomTrackCurrent = sqrt(filtBottomTrackVelocity[X_INDEX] * filtBottomTrackVelocity[X_INDEX] + filtBottomTrackVelocity[Y_INDEX] * filtBottomTrackVelocity[Y_INDEX]); if (fabs(filtBottomTrackVelocity[X_INDEX]) < EPSILON) { if (filtBottomTrackVelocity[Y_INDEX] >= 0.0) bottomTrackAngle = PI / 2.0; else bottomTrackAngle = -PI / 2.0; bottomTrackDirection = bottomTrackAngle * RAD_TO_DEG; } else { bottomTrackAngle = atan(fabs(filtBottomTrackVelocity[Y_INDEX]) / fabs(filtBottomTrackVelocity[X_INDEX])); if (filtBottomTrackVelocity[X_INDEX] >= 0.0) { if (filtBottomTrackVelocity[Y_INDEX] >= 0.0) bottomTrackDirection = bottomTrackAngle * RAD_TO_DEG; else bottomTrackDirection = 360.0 - (bottomTrackAngle * RAD_TO_DEG); } else { if (filtBottomTrackVelocity[Y_INDEX] >= 0.0) bottomTrackDirection = 180.0 - (bottomTrackAngle * RAD_TO_DEG); else bottomTrackDirection = 180.0 + (bottomTrackAngle * RAD_TO_DEG); } } /* determine direction relative to current vehicle heading */ bottomTrackDirection += kalmanHeadingDegrees; if (bottomTrackDirection >= 360.0) bottomTrackDirection -= 360.0; dm_write(dopplerDmi.bottomTrackCurrent, (Void *) &bottomTrackCurrent, sizeof(bottomTrackCurrent), &sampleTime); dm_write(dopplerDmi.bottomTrackDirection, (Void *) &bottomTrackDirection, sizeof(bottomTrackDirection), &sampleTime); } else { /* ** This is the case when the bottom-track data is INVALID. ** ** If necessary, flip the Data Valid flag to FALSE. This ** "if" statement prevents constant updating when the flag ** hasn't changed value. */ if (bottomTrackDataValid) { bottomTrackDataValid = FALSE; dm_write(dopplerDmi.bottomTrackDataValid, (Void *) &bottomTrackDataValid, sizeof(bottomTrackDataValid), &sampleTime); } #ifdef REPLACE_ALT_W_DVL if (altitudeDataValid) /* set data valid flag FALSE, */ { /* and update DM item */ altitudeDataValid = FALSE; writeDmItem(dmItems.altitudeDataValid, &altitudeDataValid, sizeof(altitudeDataValid)); dvlAltitude = 0.; writeDmItem(dmItems.altitude, &dvlAltitude, sizeof(dvlAltitude)); } #endif } /* if (dvlDataStatus != BAD_BOTTOM_TRACK_VELOCITY) */ /* get water-track velocity and update data manager items */ if ((dvlDataStatus & BAD_WATER_MASS_VELOCITY) != BAD_WATER_MASS_VELOCITY) { if (!waterMassDataValid) { waterMassDataValid = TRUE; dm_write(dopplerDmi.waterMassDataValid, (Void *) &waterMassDataValid, sizeof(waterMassDataValid), &sampleTime); } /* if */ /* convert water-mass layer velocity to vehicle coordinates */ vehicleWaterMassVelocity[X_INDEX] = -waterMassVelocity[Y_INDEX]; vehicleWaterMassVelocity[Y_INDEX] = -waterMassVelocity[X_INDEX]; vehicleWaterMassVelocity[Z_INDEX] = -waterMassVelocity[Z_INDEX]; for (dof = 0; dof < NX; dof++) { status = dm_write(dopplerDmi.waterMassVelocity[dof], (Void *) &vehicleWaterMassVelocity[dof], sizeof(vehicleWaterMassVelocity[dof]), &sampleTime); checkWrite(status, "Doppler System - water-mass layer velocity"); } /* for */ /* filter water-mass layer velocity */ for (dof = 0; dof < NX; dof++) { filtWaterMassVelocity[dof] = velocityFilter((Flt64)vehicleWaterMassVelocity[dof], waterMassVelocityPrev[dof], filtWaterMassVelocityPrev[dof]); status = dm_write(dopplerDmi.filtWaterMassVelocity[dof], (Void *) &filtWaterMassVelocity[dof], sizeof(filtWaterMassVelocity[dof]), &sampleTime); checkWrite(status, "Doppler System - filtered water-mass velocity"); } /* for */ /* update previous values */ for (dof = 0; dof < NX; dof++) { waterMassVelocityPrev[dof][1] = waterMassVelocityPrev[dof][0]; waterMassVelocityPrev[dof][0] = (Flt64) vehicleWaterMassVelocity[dof]; filtWaterMassVelocityPrev[dof][1] = filtWaterMassVelocityPrev[dof][0]; filtWaterMassVelocityPrev[dof][0] = (Flt64) filtWaterMassVelocity[dof]; } /* calculate water-mass current and direction from the x and y velocity */ waterMassCurrent = sqrt(filtWaterMassVelocity[X_INDEX] * filtWaterMassVelocity[X_INDEX] + filtWaterMassVelocity[Y_INDEX] * filtWaterMassVelocity[Y_INDEX]); if (fabs(filtWaterMassVelocity[X_INDEX]) < EPSILON) { if (filtWaterMassVelocity[Y_INDEX] >= 0.0) waterMassAngle = PI / 2.0; else waterMassAngle = -PI / 2.0; waterMassDirection = bottomTrackAngle * RAD_TO_DEG; } else { waterMassAngle = atan(fabs(filtWaterMassVelocity[Y_INDEX]) / fabs(filtWaterMassVelocity[X_INDEX])); if (filtWaterMassVelocity[X_INDEX] >= 0.0) { if (filtWaterMassVelocity[Y_INDEX] >= 0.0) waterMassDirection = waterMassAngle * RAD_TO_DEG; else waterMassDirection = 360.0 - (waterMassAngle * RAD_TO_DEG); } else { if (filtWaterMassVelocity[Y_INDEX] >= 0.0) waterMassDirection = 180.0 - (waterMassAngle * RAD_TO_DEG); else waterMassDirection = 180.0 + (waterMassAngle * RAD_TO_DEG); } } /* determine direction relative to current vehicle heading */ waterMassDirection += kalmanHeadingDegrees; if (waterMassDirection >= 360.0) waterMassDirection -= 360.0; dm_write(dopplerDmi.waterMassCurrent, (Void *) &waterMassCurrent, sizeof(waterMassCurrent), &sampleTime); dm_write(dopplerDmi.waterMassDirection, (Void *) &waterMassDirection, sizeof(waterMassDirection), &sampleTime); } else if (waterMassDataValid) { waterMassDataValid = FALSE; dm_write(dopplerDmi.waterMassDataValid, (Void *) &waterMassDataValid, sizeof(waterMassDataValid), &sampleTime); } /* if (dvlDataStatus != BAD_WATER_MASS_VELOCITY) */ } else /* Serial comms are invalid */ { /* ** The DVL asynchronously updates between 3 and 4 Hz, ** depending on the altitude. This loop is running faster ** (10Hz), so we expect most serial reads to return a ** NO_DVL_RESPONSE. So, we'll wait NUM_DVL_REPLIES before ** declaring the DVL to actually not be responding. */ dvlReplyCounter++; if( ( (dvlReplyCounter % NUM_DVL_REPLIES) == 0 ) || ( (dvlDataStatus & BAD_SERIAL_READ) == BAD_SERIAL_READ ) || ( (dvlDataStatus & CHECKSUM_WRONG) == CHECKSUM_WRONG ) ) { if( dvlCommsValid ) /* If still TRUE, flip it to FALSE.*/ { dvlCommsValid = FALSE; status = dm_write(dopplerDmi.commsValid, (Void *) &dvlCommsValid, sizeof(dvlCommsValid), &sampleTime); checkWrite(status, "Doppler System - Serial Comms Valid"); } /* if( dvlCommsValid ) */ #ifdef REPLACE_ALT_W_DVL if (altitudeDataValid) /* set data valid flag FALSE, */ { /* and update DM item */ altitudeDataValid = FALSE; writeDmItem(dmItems.altitudeDataValid, &altitudeDataValid, sizeof(altitudeDataValid)); dvlAltitude = 0.; writeDmItem(dmItems.altitude, &dvlAltitude, sizeof(dvlAltitude)); } #endif if ( (dvlReplyCounter % NUM_DVL_REPLIES) == 0 ) { logMsg("Doppler System WARNING : " "No response from doppler: 30 tries.\n"); dvlReplyCounter = 0; } } /* if( dvlReplyCounter ... ) */ }/*if ((dvlDataStatus & NO_DVL_RESPONSE) != NO_DVL_RESPONSE)*/ } /* if (pinging) */ } /* if (init) */ prevPinging = pinging; taskUnsafe(); /* Task may now be deleted safely */ /* Pend until timer wakes us up */ /* Update Rate is not guaranteed due to */ /* the possibility of task preemption */ semTake(wakeupSem, sysClkRateGet()/ (USECS_PER_SEC / SONAR_DM_PERIOD)); } /* while */ if( dvlCommsValid ) /* If still TRUE, flip it to FALSE.*/ { dvlCommsValid = FALSE; status = dm_write(dopplerDmi.commsValid, (Void *) &dvlCommsValid, sizeof(dvlCommsValid), &sampleTime); checkWrite(status, "Doppler System - Serial Comms Valid"); } /* if( dvlCommsValid ) */ } /* FOREVER */ semDelete(wakeupSem); /* semaphore resources */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are NOT required when the task exits as the */ /* Data Manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ /* That's all folks ! */ } /* sonarDconSensorInTask() */ /******************************************************************************/ /* Function : processSonarDconSRQ */ /* Purpose : Decode Service Request and take appropriate action */ /* Inputs : SRQ code, array of DM items, serial channel */ /* Outputs : Returns SRQ Recieved Status & many DM items are changed */ /******************************************************************************/ LOCAL Word processSonarDconSRQ( Reg sonarDconDmItems* dmItems, microIOControl *microIOCtrl ) { Word srqRequest; /* Service Request Value */ Byte buffer[SRQ_PACKET_LEN]; /* Buffer for micro communications data */ /* Block on Reading Service Request */ if (readSRQPacket(SERIAL_CHAN, buffer, SRQ_PACKET_LEN, WAIT_FOREVER) != ERROR) /* Service Request received so process it */ srqRequest = wordFromBuf(buffer, 0); switch (srqRequest) /* Decode Service Request from micro */ { case MICRO_RESET_SRQ: /* Microcontroller Reset Occurred */ /* Post Reset Event DM Item */ if (microIOCtrl->announceResets) microResetEventDm(dmItems->ibcDm.microDm.resetEvent); break; case CAN_WATER_ALARM_ON_SRQ: /* Housing Water Alarm Active */ writeBooleanDmItem(dmItems->canWaterAlarm, TRUE); break; case CAN_WATER_ALARM_OFF_SRQ: /* Housing Water Alarm Inactive */ writeBooleanDmItem(dmItems->canWaterAlarm, FALSE); break; case JBOX_WATER_ALARM_ON_SRQ: /* J Box Water Alarm Active */ writeBooleanDmItem(dmItems->jboxWaterAlarm, TRUE); break; case JBOX_WATER_ALARM_OFF_SRQ: /* J Box Water Alarm Inactive */ writeBooleanDmItem(dmItems->jboxWaterAlarm, FALSE); break; default: if (processIbcCpuSRQ(srqRequest, buffer, &dmItems->ibcDm ) != TRUE) if (processIbcLpsSRQ(srqRequest, buffer, lpsBoardAddr[0], &dmItems->lpsDm[0]) != TRUE) if (processIbcLpsSRQ(srqRequest, buffer, lpsBoardAddr[1], &dmItems->lpsDm[1]) != TRUE) if (processIbcGf5vSRQ(srqRequest, buffer, GF5V_BOARD_ADDR, &dmItems->gf5vDm) != TRUE) if (processIbcVspSRQ(srqRequest, buffer, SERIAL_BOARD_ADDR, VSP_CHAN, &dmItems->vspDm ) != TRUE) if (processIbcQuadSerSRQ(srqRequest, buffer, SERIAL_BOARD_ADDR, &dmItems->quadSerDm ) != TRUE) printf("Sonar Dcon: Unrecognized SRQ 0x%x\n", srqRequest); } /* switch */ return (srqRequest); /* Return SRQ Received to Caller */ } /* processSonarDconSRQ() */ /* Doppler Routines*/ /******************************************************************************/ /* Function : initDoppler */ /* Purpose : Initializes the Acoustic Doppler Velocimeter. */ /* Inputs : Serial channel pointer. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Int16 initDoppler(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr) { char response[MAX_PACKET_LEN]; /* response dm item */ Byte buffer[MAX_PACKET_LEN] = ""; /* serial buffer */ MBool startPinging = TRUE; /* start pinging flag */ Int16 numBytes; /* number of bytes */ Errno status; /* data manager error code */ DM_Time updateTime; /* data manager update time */ /* send on break, then off break to wake up ADV */ if (wakeUpDoppler(dopplerDmi, serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (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); while (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); } /* select the frequency of the water-mass layer ping */ sprintf(buffer, "BK1\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not select the water-mass layer ping frequency") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - BK1 - water-mass layer mode") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the bottom-track mode */ /* ** 00/12/7, rsm. Changed BM4 to BM5 on recommendation of Arthur ** Johnson of RDI. */ sprintf(buffer, "BM5\r"); if (sendCommand(serialChannelPtr, buffer, 4, "could not set the profiling mode") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - BM5 - bottom-track mode") != 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 - pings per ensemble") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the minimum time between pings to 0.25 seconds */ sprintf(buffer, "TP000025\r"); if (sendCommand(serialChannelPtr, buffer, 9, "could not set time between pings") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - TP000025 - time between pings") != ERROR) logMsg("ADV Response : %s\n", buffer); /* set the number of water-track pings to zero to turn off water-track data */ /* collection */ 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 - water-track pings per ensemble") != 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 steam") == ERROR) return(ERROR); memset(buffer, 0, (size_t) MAX_PACKET_LEN); if (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to - PD4 - data stream select") != 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 - EX01000 - coordinate transformation") != ERROR) logMsg("ADV Response : %s\n", buffer); /* update data manager ADV response item */ memset(response, 0, (size_t) MAX_PACKET_LEN); strcpy(response, "ADV powered up"); gettimeofday(&updateTime, (struct timezone *) NULL); status = dm_write(dopplerDmi->response, (Void *) response, MAX_PACKET_LEN * sizeof(char), &updateTime); checkWrite(status, "Doppler System - command response"); /* start the pinging and data collection cycle */ status = dm_write(dopplerDmi->startPinging, (Void *) &startPinging, sizeof(startPinging), &updateTime); checkWrite(status, "Doppler System - start pinging"); } /* 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(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr, Word boardAddress, Word channelNumber) { char response[MAX_PACKET_LEN]; /* response to input command */ Byte buffer[MAX_PACKET_LEN] = ""; /* serial buffer */ Int16 count; /* counter */ Int16 numBytes; /* number of bytes in command */ /* number of responses to wait*/ Int16 numResponseWaits; /* for, to total 1 sec wait */ Errno status; /* data manager error code */ DM_Time updateTime; /* data manager update time */ numResponseWaits = sysClkRateGet(); /* 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 > numResponseWaits) { logMsg("Doppler System WARNING : ADV not responding to on break\n"); return(ERROR); } taskDelay(1); } taskDelay(sysClkRateGet() / (USECS_PER_SEC / DOPPLER_BREAK)); /* send off break */ count = 0; while (Quad_Serial_SendBreak(serialChannelPtr, boardAddress, channelNumber, OFF) == ERROR) { count++; if (count > numResponseWaits) { logMsg("Doppler System WARNING : ADV not responding to off break\n"); return(ERROR); } taskDelay(1); } /* update ADV response */ memset(buffer, 0, (size_t) MAX_PACKET_LEN); memset(response, 0, (size_t) MAX_PACKET_LEN); while (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to wake-up ADV") != ERROR) { logMsg("ADV Response : %s\n", buffer); numBytes = strlen(buffer); strncpy(response, buffer, numBytes-2); gettimeofday(&updateTime, (struct timezone *) NULL); status = dm_write(dopplerDmi->response, (Void *) response, MAX_PACKET_LEN * sizeof(char), &updateTime); checkWrite(status, "Doppler System - command response"); memset(buffer, 0, (size_t) MAX_PACKET_LEN); memset(response, 0, (size_t) MAX_PACKET_LEN); } 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) SERIAL_BOARD_ADDR, (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 */ /* number of responses to wait*/ Int16 numResponseWaits; /* for, to total 1 sec wait */ numResponseWaits = sysClkRateGet(); /* wait for a terminal character in the buffer */ while ( (numTermChars = Quad_Serial_TermChars(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (Word) ADV_READ_CHANNEL_NUMBER)) < 1) { count++; if (count > numResponseWaits) return(ERROR); taskDelay(1); } if (Quad_Serial_Read(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (Word) ADV_READ_CHANNEL_NUMBER, buffer, numBytes) == ERROR) { logMsg("Doppler System WARNING : %s\n", errorMsg); return(ERROR); } /* ** BEWARE: Adding this flush made the "garbling" problem worse. The ** other function that talks serial, getAdvSerialData(), has this flush. ** Quad_Serial_Flush(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (Word) ADV_READ_CHANNEL_NUMBER); */ } /* getResponse */ /******************************************************************************/ /* Function : getDvlData */ /* Purpose : Gets doppler data. */ /* Inputs : Serial channel pointer. */ /* Outputs : Bottom-track and water-track velocity, bottom status byte, */ /* water status byte, dvlRange, dvlTemp, and dvlDataStatus byte. */ /* Returns : Void. /* NOTE: : bottomTrackVelocity has dimension 4 because the error component */ /* is now included. waterMassVelocity still has dim 3. */ /******************************************************************************/ Void getDvlData(sio32Chan *serialChannelPtr, Flt32 bottomTrackVelocity[], Int32 *bottomStatus, Flt32 waterMassVelocity[], Int32 *waterStatus, Flt32 *dvlRange, Flt32 *dvlTemp, Byte *dvlDataStatus ) { Byte buffer[MAX_PACKET_LEN]; /* data buffer */ Byte bottomTrackVelocityStatus = 0; /* velocity status */ Byte waterMassVelocityStatus = 0; Int32 xBottomTrackVelocity[NBYTES]; /* velocity */ Int32 yBottomTrackVelocity[NBYTES]; Int32 zBottomTrackVelocity[NBYTES]; Int32 eBottomTrackVelocity[NBYTES]; Int32 xWaterMassVelocity[NBYTES]; Int32 yWaterMassVelocity[NBYTES]; Int32 zWaterMassVelocity[NBYTES]; Int32 eWaterMassVelocity[NBYTES]; Flt32 tempWaterMassVelocity[NXE]; Int32 rawRange[NUM_BEAMS][NBYTES]; /* raw DVL range in centimetrs*/ Int32 rawTemp[NBYTES]; /* temperature, .01 Deg C/bit */ Int16 dof; /* dof counter */ MBool status; /* General function call status*/ Byte dvlSerialStatus; /* Status of serial comms to DVL*/ /* get the data buffer */ dvlSerialStatus = getAdvSerialData(serialChannelPtr, buffer, DOPPLER_BUFFER_SIZE ); if ((dvlSerialStatus != DVL_DATA_GOOD)) { /* ** There was no response from the DVL, or a serial line problem. */ *dvlDataStatus = (BAD_BOTTOM_TRACK_VELOCITY | BAD_WATER_MASS_VELOCITY | dvlSerialStatus ); return; } sscanf( (char *) buffer+10, "%2x %2x %2x %2x %2x %2x %2x %2x", &xBottomTrackVelocity[DOPPLER_LSB], &xBottomTrackVelocity[DOPPLER_MSB], &yBottomTrackVelocity[DOPPLER_LSB], &yBottomTrackVelocity[DOPPLER_MSB], &zBottomTrackVelocity[DOPPLER_LSB], &zBottomTrackVelocity[DOPPLER_MSB], &eBottomTrackVelocity[DOPPLER_LSB], &eBottomTrackVelocity[DOPPLER_MSB]); sscanf( (char *) buffer+26, "%2x %2x %2x %2x %2x %2x %2x %2x", &rawRange[BM1][DOPPLER_LSB], &rawRange[BM1][DOPPLER_MSB], &rawRange[BM2][DOPPLER_LSB], &rawRange[BM2][DOPPLER_MSB], &rawRange[BM3][DOPPLER_LSB], &rawRange[BM3][DOPPLER_MSB], &rawRange[BM4][DOPPLER_LSB], &rawRange[BM4][DOPPLER_MSB]); sscanf( (char *) buffer+42, "%2x", bottomStatus); sscanf( (char *) buffer+44, "%2x %2x %2x %2x %2x %2x %2x %2x", &xWaterMassVelocity[DOPPLER_LSB], &xWaterMassVelocity[DOPPLER_MSB], &yWaterMassVelocity[DOPPLER_LSB], &yWaterMassVelocity[DOPPLER_MSB], &zWaterMassVelocity[DOPPLER_LSB], &zWaterMassVelocity[DOPPLER_MSB], &eWaterMassVelocity[DOPPLER_LSB], &eWaterMassVelocity[DOPPLER_MSB]); sscanf( (char *) buffer+68, "%2x", waterStatus); sscanf( (char *) buffer+86, "%2x %2x", &rawTemp[DOPPLER_LSB], &rawTemp[DOPPLER_MSB]); /* check the bottom-track velocity status */ if (*bottomStatus == BEAM_OK) /* convert from mm/s to m/s */ { if (mmToMetres(xBottomTrackVelocity, yBottomTrackVelocity, zBottomTrackVelocity, eBottomTrackVelocity, bottomTrackVelocity) == ERROR) bottomTrackVelocityStatus = BAD_BOTTOM_TRACK_VELOCITY; } else /* beam error */ { #if 0 if ((*bottomStatus & BOTTOM_BEAM1_CORRELATION) == BOTTOM_BEAM1_CORRELATION) logMsg("Doppler System WARNING : beam 1 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM1_ECHO_AMPLITUDE) == BOTTOM_BEAM1_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 1 low echo amplitude\n"); if ((*bottomStatus & BOTTOM_BEAM2_CORRELATION) == BOTTOM_BEAM2_CORRELATION) logMsg("Doppler System WARNING : beam 2 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM2_ECHO_AMPLITUDE) == BOTTOM_BEAM2_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 2 low echo amplitude\n"); if ((*bottomStatus & BOTTOM_BEAM3_CORRELATION) == BOTTOM_BEAM3_CORRELATION) logMsg("Doppler System WARNING : beam 3 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM3_ECHO_AMPLITUDE) == BOTTOM_BEAM3_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 3 low echo amplitude\n"); if ((*bottomStatus & BOTTOM_BEAM4_CORRELATION) == BOTTOM_BEAM4_CORRELATION) logMsg("Doppler System WARNING : beam 4 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM4_ECHO_AMPLITUDE) == BOTTOM_BEAM4_ECHO_AMPLITUDE) logMsg("Doppler System WARNING : beam 4 low echo amplitude\n"); #endif bottomTrackVelocityStatus = BAD_BOTTOM_TRACK_VELOCITY; } /* check the water-track velocity status */ if (*waterStatus == BEAM_OK) /* convert from mm/s to m/s */ { if (mmToMetres(xWaterMassVelocity, yWaterMassVelocity, zWaterMassVelocity, eWaterMassVelocity, tempWaterMassVelocity) == ERROR) waterMassVelocityStatus = BAD_WATER_MASS_VELOCITY; /* ** Here, we'll drop the error component of the water-mass velocity. */ for (dof = 0; dof < NX; dof++) { waterMassVelocity[dof] = tempWaterMassVelocity[dof]; } } else /* beam error */ { #if 0 if ((*waterStatus & ALTITUDE_TOO_SHALLOW) == ALTITUDE_TOO_SHALLOW) logMsg("Doppler System WARNING : altitude too shallow\n"); if ((*waterStatus & WATER_BEAM1_CORRELATION) == WATER_BEAM1_CORRELATION) logMsg("Doppler System WARNING : beam 1 low correlation\n"); if ((*waterStatus & WATER_BEAM2_CORRELATION) == WATER_BEAM2_CORRELATION) logMsg("Doppler System WARNING : beam 2 low correlation\n"); if ((*waterStatus & WATER_BEAM3_CORRELATION) == WATER_BEAM3_CORRELATION) logMsg("Doppler System WARNING : beam 3 low correlation\n"); if ((*waterStatus & WATER_BEAM4_CORRELATION) == WATER_BEAM4_CORRELATION) logMsg("Doppler System WARNING : beam 4 low correlation\n"); #endif waterMassVelocityStatus = BAD_WATER_MASS_VELOCITY; } *dvlDataStatus = (bottomTrackVelocityStatus | waterMassVelocityStatus); /* ** Convert the temperature into Degrees centigrade. */ *dvlTemp = (Flt32) ( rawTemp[DOPPLER_LSB] | (rawTemp[DOPPLER_MSB] << 8) ); *dvlTemp /= 100.; /* 100 counts = 1 Degree C */ /* ** Compute range: */ status = cvtRange( rawRange, dvlRange ); if( status == ERROR ) { logMsg(" Doppler System - Error in range computation.\n"); } return; } /* getDvlData */ /******************************************************************************/ /* Function : cvtRange */ /* Purpose : Converts raw DVL range measurement into Eng. units of meters. */ /* Inputs : 4 beam range-to-bottoms, lsb and msb. */ /* Outputs : Returns OK or ERROR on bad velocity status. */ /******************************************************************************/ MBool cvtRange(Int32 rawRange[NUM_BEAMS][NBYTES], Flt32 *dvlAltitude) { Int16 rangeCM; /* range in centimeters */ Flt32 range = 0.; Int16 i; for( i=0; iresponse, (Void *) response, MAX_PACKET_LEN * sizeof(char), &updateTime); checkWrite(status, "Doppler System - command response"); } return(OK); } /* startPinging */ /******************************************************************************/ /* Function : commandDoppler */ /* Purpose : Sends a command to the ADV and receives its response. */ /* Inputs : Doppler data manager items pointer and serial channel pointer. */ /* Outputs : None. */ /******************************************************************************/ Void commandDoppler(dopplerDMItems *dopplerDmi, sio32Chan *serialChannelPtr) { char command[MAX_PACKET_LEN]; /* user input command string */ char response[MAX_PACKET_LEN]; /* response to input command */ Byte buffer[MAX_PACKET_LEN]; /* serial buffer size */ Int16 numBytes; /* number of bytes in command */ Errno status; /* data manager error code */ DM_Time updateTime; /* data manager update time */ /* get user input command */ status = dm_read(dopplerDmi->command, (Void *) command, MAX_PACKET_LEN * sizeof(char), (DM_Time *) NULL); if (checkRead(status, "Doppler System - user command\n")) return; /* send command to ADV */ numBytes = strlen(command) + 1; sprintf(buffer, "%s\r", command); if (sendCommand(serialChannelPtr, buffer, numBytes, "could not command ADV") == ERROR) { logMsg("Doppler System WARNING : ADV command %s failed\n", command); return; } /* update ADV response */ memset(buffer, 0, (size_t) MAX_PACKET_LEN); memset(response, 0, (size_t) MAX_PACKET_LEN); while (getResponse(serialChannelPtr, buffer, MAX_PACKET_LEN, "no response to command") != ERROR) { logMsg("ADV Response : %s\n", buffer); numBytes = strlen(buffer); strncpy(response, buffer, numBytes-2); gettimeofday(&updateTime, (struct timezone *) NULL); status = dm_write(dopplerDmi->response, (Void *) response, MAX_PACKET_LEN * sizeof(char), &updateTime); checkWrite(status, "Doppler System - command response"); memset(buffer, 0, (size_t) MAX_PACKET_LEN); memset(response, 0, (size_t) MAX_PACKET_LEN); } } /* commandDoppler */ /******************************************************************************/ /* 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 */ 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 */ /* open quad serial Read port */ if (Quad_Serial_Open(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (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) SERIAL_BOARD_ADDR, (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) SERIAL_BOARD_ADDR, (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) SERIAL_BOARD_ADDR, (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) SERIAL_BOARD_ADDR, (Word) ADV_READ_CHANNEL_NUMBER); Quad_Serial_Close(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (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. */ /******************************************************************************/ Byte getAdvSerialData(sio32Chan *serialChannelPtr, Byte buffer[], Int16 numBytes) { static Int16 noDataCount = 0; /* no data counter */ Int16 numTermChars; /* number of terminal chars */ 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 */ /* ** It REALLY looks like the logMsg() and subsequent return should both be ** INSIDE braces, so that it only returns after NUM_SERIAL_WAITS times. */ while ( (numTermChars = Quad_Serial_TermChars(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (Word) ADV_READ_CHANNEL_NUMBER)) < 1) { noDataCount++; if ((noDataCount % NUM_SERIAL_WAITS) == 0) { /* logMsg("Doppler System WARNING : No response from doppler.\n"); */ return( NO_DVL_RESPONSE ); } /* if ((noDataCount % NUM_SERIAL_WAITS) == 0) */ } noDataCount = 0; /* get the data ensemble (must flush after each read) */ if (Quad_Serial_Read(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (Word) ADV_READ_CHANNEL_NUMBER, buffer, numBytes) == ERROR) { logMsg("Doppler System WARNING : problem reading serial port\n"); return( BAD_SERIAL_READ | NO_DVL_RESPONSE ); } Quad_Serial_Flush(serialChannelPtr, (Word) SERIAL_BOARD_ADDR, (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(DVL_DATA_GOOD); else { logMsg("Doppler System WARNING : ensemble checksum incorrect\n" "Computed checksum = %d\n" "DVL checksum = %d\n", checksum, advChecksum); return( CHECKSUM_WRONG | NO_DVL_RESPONSE ); } } /* getAdvSerialData */