/****************************************************************************/ /* Copyright 1992 to 1996 MBARI */ /****************************************************************************/ /* Summary : VME IBC Interface Module for VxWorks */ /* Filename : vmeIbc.c */ /* Author : Andrew Pearce */ /* Project : Tiburon ROV */ /* Version : Version 1.0 */ /* Created : 10/01/92 */ /* Modified : 06/02/98 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: vmeIbc.c,v 1.1 93/07/01 14:36:35 pean Exp $ * $Log: vmeIbc.c,v $ * Revision 1.1 93/07/01 14:36:35 14:36:35 pean (Andrew Pearce) * Initial revision * * */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks semaphore functions */ #include /* vxWorks task library functions */ #include /* vxWorks time and date functions */ #include /* vxWorks string function library */ #include /* vxWorks Message Queue Library */ #include /* vxWorks Standard IO library */ #include /* MBARI style guide type declarations */ #include /* Miscellaneous constants */ #include /* Trigonometric Functions & 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 management Data Types */ #include "powerTypes.h" /* Power management Data Types */ #include "console.h" /* Topside Console Data Manager Items */ #include "sio32.h" /* Sio32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #include "vmeIbc.h" /* VME Can IBC Definitions */ #include "sensorsDm.h" /* Sensor Dm Prefix */ #include "applic.h" /* Microcontroller applications */ #include "microcmd.h" /* Microcontroller commands */ #include "microCmd.h" /* Microcontroller function interface */ #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 "nav_if.h" /* IBC Nav Interface Board Definitions */ #include "vme_ibc.h" /* VME IBC microcontroller 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 "vspDm.h" /* VSP Data Manager Interface Functions */ #include "vspCmd.h" /* VSP Function Prototypes */ #include "microTask.h" /* Microcontroller Task IO definitions */ /* VME IBC Device Name */ MLocal char* const vmeDataConName = "VME IBC"; /* VME IBC Data Manager Item Name Prefix*/ MLocal char* const vmeDmPrefix = VMEIBC_DM_PREFIX; MLocal char* const sensorDmPrefix = SENSOR_DM_PREFIX; /* LPS Output is 28V (Non Standard) */ #define LPS_28V_BOARD_ADDR (Word) LOW_PWR_SWITCH_XXV_0_ADDR #define GF5V_BOARD_ADDR (Word) GROUND_FAULT_0_ADDR #define SERIAL_BOARD_ADDR (Word) QUAD_SERIAL_0_ADDR #define A_TO_D_BOARD_ADDR (Word) QUAD_A_TO_D_0_ADDR #define PARALLEL_IO_BOARD_ADDR (Word) PARALLEL_IO_0_ADDR #define LPS_BOARDS 1 /* Number of LPS Boards in VME IBC */ MLocal Word lpsBoardAddr[LPS_BOARDS] = /* LPS Board Assignments */ { LPS_28V_BOARD_ADDR, }; MLocal ibcCardConfig ibcCardSet = { LPS_BOARDS + 7, /* Number of IBC Cards Expected */ { CPU196, NAV_INTERFACE_ADDR, A_TO_D_BOARD_ADDR, GF5V_BOARD_ADDR, LPS_28V_BOARD_ADDR, PARALLEL_IO_BOARD_ADDR, SERIAL_BOARD_ADDR, HYDROPHONE_PSU_ADDR } }; /* 28V LPS Board Voltage Alarm Threshold*/ #define VME_LPS_VOLT_WARN_THRESH 27000 #define VME_LPS_VOLT_ALARM_THRESH 26000 /* 28V LPS Board Channel Assignments */ #define IMAG_SONAR_SWITCH_CHAN 2 /* Imaging Sonar Power */ #define GYRO_SWITCH_CHAN 3 /* Gyro Power */ #define NAV_SENSOR_DM_PERIOD 50000 /* Nav sensor update peroid usecs 20 Hz */ #define HUMIDITY_DM_PERIOD 200000 /* Humidity sensor provider period .5Hz */ #define VME_IBC_DM_PERIOD 200000 /* VME IBC Data provider period 1 Hz */ #define VME_IBC_IN_STACKSIZE 2000 /* VME IBC Input Task size & TCB */ #define VME_IBC_OUT_STACKSIZE 4000 /* VME IBC Output Task size & TCB */ #define VME_SENSOR_STACKSIZE 3000 /* VME IBC Sensor Task size & TCB */ #define STARTUP_COUNT 2040 /* startup period (100 secs at 20 Hz) */ #define STARTUP_COUNT_IGNORE 1200 /* ignore period at startup (60 secs) */ #define NUM_TASKS 4 /* Number of co-operating tasks */ typedef struct /* VME IBC Data Manager Item Handles */ { DM_Item waterAlarm0; /* Water Alarm 0 Detected TRUE/FALSE */ DM_Item waterAlarm1; /* Water Alarm 1 Detected TRUE/FALSE */ DM_Item waterAlarmThresh; /* Water Alarm Threshold */ DM_Item vmePsuStatus; /* VME Can Power Supply Status */ DM_Item navIfPsuStatus; /* Nav Interface Board PSU Status */ DM_Item videoSwitchCtrl; /* VME Video switcher control word */ DM_Item kvhCompassString; /* KVH Compass String */ DM_Item kvhCompassCal; /* Starts KVH Compass Calibration */ DM_Item kvhCalPosition; /* KVH Compass Calibration */ DM_Item kvhCalScore; /* KVH Compass Calibration Score */ DM_Item kvhCalCount; /* KVH Compass Calibration Count */ 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 */ } vmeIbcDmItems; typedef struct /* VME IBC Sensor Data Manager Items */ { DM_Item compassValid; /* Compass Data Valid flag */ DM_Item compassRads; /* Compass Reading in radians */ DM_Item compassDegs; /* Compass Reading in degrees */ DM_Item gyroValid; /* Gyro Data Valid flag */ DM_Item gyroRads; /* Gyro Reading in radians */ DM_Item gyroDegs; /* Gyro Reading in degrees */ DM_Item gyroRateRads; /* Gyro Rate Reading in radians */ DM_Item gyroRateDegs; /* Gyro Rate Reading in degrees */ DM_Item gyroSwitchState; /* Gyro switch state */ DM_Item pitchRads; /* Pitch Sensor reading in radians */ DM_Item pitchDegs; /* Pitch Sensor reading in degrees */ DM_Item rollRads; /* Roll Sensor reading in radians */ DM_Item rollDegs; /* Roll Sensor reading in degrees */ } vmeIbcSensorDmItems; /****************************************************************************/ Void VME_IBC_InTask(Reg vmeIbcDmItems* dmItems, microIOControl *microIOCtrl); Void VME_IBC_OutTask(Reg vmeIbcDmItems* dmItems, microIOControl *microIOCtrl); Void VME_IBC_SensorTask(microIOControl *microIOCtrl); MLocal Word processVmeIbcSRQ(Reg vmeIbcDmItems *dmItems, microIOControl *microIOCtrl); MLocal STATUS updateNavIfPsuStatusDm( sio32Chan *dataChan, DM_Item navIfStatusDm ); MLocal Void initVmeIbcDmItems(vmeIbcDmItems* dmItems, sio32Chan* dataChan); /****************************************************************************/ /* Function : vmeCanGetPsuStatus */ /* Purpose : Reads status of VME Can Power Supply */ /* Inputs : Sio32 channel, pointer to VME PSU status */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS vmeCanGetPsuStatus( sio32Chan *IBC_SerialChan, Word *vmePsuStatus ) { return (microCommand(IBC_SerialChan, VME_IBC_APP | GET_VME_CAN_PSU_STATUS, vmePsuStatus) ); } /* vmeCanGetPsuStatus() */ /****************************************************************************/ /* Function : navIfGetPsuStatus */ /* Purpose : Reads status of Nav I/F Power Supply */ /* Inputs : Sio32 channel, pointer to VME PSU status */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS navIfGetPsuStatus( sio32Chan *IBC_SerialChan, Word *navIfPsuStatus ) { return (microCommand(IBC_SerialChan, IBC_CPU_APP | NAV_IF_READ_STATUS, navIfPsuStatus) ); } /* navIfGetPsuStatus() */ /****************************************************************************/ /* Function : vmeCanReadNavSensors */ /* Purpose : Reads pitch, roll, gyro sensor mesurements from VME Can IBC*/ /* Inputs : Sio32 channel, pointers to pitch, roll, gyro, gyro rate */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS vmeCanReadNavSensors( sio32Chan *IBC_SerialChan, Int16 *pitch, Int16 *roll, Nat16 *compass, Nat16 *gyro, Int16 *gyroRate ) { Int16 cmdStatus; /* Command Status */ Word reply[6]; /* Command Result from microcontroller */ /* Send command to VME IBC micro */ if ( sendMicroCommand( IBC_SerialChan, &reply, sizeof(Word) * 6, 1, VME_IBC_APP | GET_VME_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, 6, &cmdStatus, pitch, roll, compass, gyro, gyroRate); return (OK); /* Return OK */ } /* vmeCanReadNavSensors() */ /******************************************************************************/ /* Function : vmeCanGetWaterAlarm */ /* Purpose : Read water alarm status from VME IBC microcontroller */ /* Inputs : Sio32 socket and pointer to water alarm status */ /* Outputs : Returns OK or ERROR */ /******************************************************************************/ LOCAL STATUS vmeCanGetWaterAlarm( sio32Chan *IBC_SerialChan, Word *waterAlarmStatus ) { /* Send command to microcontroller */ return( microCommand(IBC_SerialChan, VME_IBC_APP | GET_VME_WATER_ALARM, waterAlarmStatus) ); } /* vmeCanGetWaterAlarm() */ /****************************************************************************/ /* Function : vmeSetWaterAlarmThresh */ /* Purpose : Sets water alarm threshold value from IBC CPU */ /* Inputs : Sio32 channel, water alarm threshold value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ LOCAL STATUS vmeSetWaterAlarmThresh( Reg sio32Chan *IBC_SerialChan, Int16 waterAlarmThresh ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( IBC_SerialChan, &status, sizeof(status), 2, VME_IBC_APP | SET_VME_WATER_ALARM_THRESH, waterAlarmThresh ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* vmeSetWaterAlarmThresh() */ /****************************************************************************/ /* Function : vmeGetWaterAlarmThresh */ /* Purpose : Reads water alarm threshold value from IBC CPU */ /* Inputs : Sio32 channel, pointer to water alarm threshold */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS vmeGetWaterAlarmThresh( sio32Chan *IBC_SerialChan, Int16 *waterAlarmThresh ) { /* Send command to microcontroller */ return (microCommand( IBC_SerialChan, VME_IBC_APP | GET_VME_WATER_ALARM_THRESH, waterAlarmThresh) ); } /* vmeGetWaterAlarmThresh() */ /****************************************************************************/ /* 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 (vmeGetWaterAlarmThresh( 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 value to IBC CPU */ if (vmeSetWaterAlarmThresh( sio32DataChan, waterAlarmThresh) == OK) return (OK); return(ERROR); } /* writeWaterAlarmThresh() */ /****************************************************************************/ /* Function : updateWaterAlarmsDm */ /* Purpose : Reads micro Water Alarm status & updates dmgr item */ /* Inputs : Sio32 chan & water alarm status data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateWaterAlarmsDm( sio32Chan *sio32DataChan, DM_Item waterAlarm0Dm, DM_Item waterAlarm1Dm ) { Word waterAlarmStatus; /* Microcontroller Water Alarm Status */ MBool status; /* Boolean state of Water Alarm */ /* Read Water Alarm Status */ if (vmeCanGetWaterAlarm(sio32DataChan, &waterAlarmStatus) != OK) return(ERROR); status = (waterAlarmStatus & WATER_ALARM0_BIT ? TRUE : FALSE); /* Write water alarm status to Dmgr */ if (writeDmItem(waterAlarm0Dm, &status, sizeof(status)) ) return(ERROR); status = (waterAlarmStatus & WATER_ALARM1_BIT ? TRUE : FALSE); /* Write water alarm status to Dmgr */ return ( writeDmItem(waterAlarm1Dm, &status, sizeof(status)) ); } /* updateWaterAlarmsDm() */ /****************************************************************************/ /* Function : updateVmeGyroDm */ /* Purpose : Reads VME Can Gyro & update dmgr item */ /* Inputs : Sio32 chan & Gyro data manager item */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateVmeNavSensorsDm(sio32Chan *sio32DataChan, vmeIbcSensorDmItems *dmItems) { static switchStatus prevGyroSwitchState = /* previous switch state */ SWITCH_OFF; static MBool startUp = FALSE; /* startup sensor flag */ static Int16 startupCount = 0; /* startup counter */ static Flt32 gyroRateOffset = 0.0; /* Gyro rate offset */ static Flt32 gyroRateSum = 0.0; /* Gyro rate sum */ static MBool initialize = TRUE; /* Initialize Flag */ static MBool lastGyroValid; /* Last Gyro Data Valid */ static MBool lastCompassValid; /* Last Compass Data Valid */ MBool gyroValid = FALSE; /* Assume Gyro Data is Bad */ MBool compassValid = FALSE; /* Assume Compass Data Bad */ Nat16 rawGyro; /* VME Can Raw Gyro */ Nat16 rawCompass; /* VME Can Raw Compass */ Int16 rawGyroRate; /* VME Can Gyro rate */ Int16 rawPitch, rawRoll; /* VME Can Raw Pitch, roll */ Flt32 compassRads, compassDegs; /* Compass radians/degrees */ Flt32 gyroRadians, gyroDegrees; /* Gyro radians/degrees */ Flt32 gyroRateRads, gyroRateDegs; /* Gyro rate radians/degrees*/ Flt32 pitchRads, rollRads; /* Pitch/Roll radians */ Flt32 pitchDegs, rollDegs; /* Pitch/Roll degrees */ switchStatus gyroSwitchState; /* gyro switch state */ if (initialize) { initialize = FALSE; lastCompassValid = compassValid; writeDmItem(dmItems->compassValid, &compassValid, sizeof(compassValid)); lastGyroValid = gyroValid; writeDmItem(dmItems->gyroValid, &gyroValid, sizeof(gyroValid)); } /* if */ if (vmeCanReadNavSensors(sio32DataChan, &rawPitch, &rawRoll, &rawCompass, &rawGyro, &rawGyroRate) != OK) return(ERROR); if (rawCompass != (Nat16) ERROR) { compassValid = TRUE; /* Set Compass Data Valid Flag TRUE */ compassDegs = rawCompass * 0.1; compassRads = DEGS_TO_RADS(compassDegs); } /* if */ if (rawGyro != (Nat16) ERROR) { gyroValid = TRUE; /* Set Gyro Data Valid Flag TRUE */ /* Convert gyro from bits to degrees */ gyroDegrees = (360.0 * rawGyro) / 16384.0; gyroRadians = DEGS_TO_RADS(gyroDegrees); } /* else */ /* Convert gyro rate from bits to degrees*/ gyroRateDegs = (1000.0 * 5.0 * rawGyroRate) / (32768.0 * 1.25); gyroRateDegs -= gyroRateOffset; gyroRateRads = DEGS_TO_RADS(gyroRateDegs); /* read the gyro switch status */ dm_read(dmItems->gyroSwitchState, &gyroSwitchState, sizeof(gyroSwitchState), (DM_Time *) NULL); if (gyroSwitchState != SWITCH_ON) gyroValid = FALSE; /* Set Gyro Data Valid Flag FALSE */ /* on power up of sensor, determine rate */ /* offset */ if (gyroSwitchState != prevGyroSwitchState) { if (gyroSwitchState == SWITCH_ON) { startUp = TRUE; gyroRateDegs += gyroRateOffset; gyroRateOffset = 0.0; gyroRateSum = 0.0; } /* if */ else if (gyroValid) { gyroValid = FALSE; if (startUp) { startUp = FALSE; startupCount = 0; } /* if */ } /* else */ /* Force dm_write of gyro valid flag */ lastGyroValid = (gyroValid == TRUE ? FALSE : TRUE); } /* if */ prevGyroSwitchState = gyroSwitchState; if (startUp) { startupCount++; if (startupCount > STARTUP_COUNT_IGNORE) { if (startupCount <= STARTUP_COUNT) gyroRateSum += gyroRateDegs; else { startupCount = 0; startUp = FALSE; gyroRateOffset = gyroRateSum / (STARTUP_COUNT - STARTUP_COUNT_IGNORE); } } } /* Convert pitch/roll from bits to degs */ pitchDegs = ((Flt32) rawPitch) * (30.0 / 32768.0); rollDegs = ((Flt32) rawRoll) * (30.0 / 32768.0); pitchRads = DEGS_TO_RADS(pitchDegs); rollRads = DEGS_TO_RADS(rollDegs); if (compassValid) { /* Write Compass Data Manager Items */ writeDmItem(dmItems->compassRads, &compassRads, sizeof(compassRads)); writeDmItem(dmItems->compassDegs, &compassDegs, sizeof(compassDegs)); } /* if */ if (compassValid != lastCompassValid) { /* Write Compass Data Valid Item */ lastCompassValid = compassValid; writeDmItem(dmItems->compassValid, &compassValid, sizeof(compassValid)); } /* if */ if (gyroValid) { /* Write Gyro Data Manager Items */ writeDmItem(dmItems->gyroRads, &gyroRadians, sizeof(gyroRadians)); writeDmItem(dmItems->gyroDegs, &gyroDegrees, sizeof(gyroDegrees)); } /* if */ if (gyroValid != lastGyroValid) /* Check for Data valid state changes */ { /* Write Gyro Data Valid Item */ lastGyroValid = gyroValid; writeDmItem(dmItems->gyroValid, &gyroValid, sizeof(gyroValid)); } /* if */ /* Write Gyro Rate Data Manager Items */ writeDmItem(dmItems->gyroRateRads, &gyroRateRads, sizeof(gyroRateRads)); writeDmItem(dmItems->gyroRateDegs, &gyroRateDegs, sizeof(gyroRateDegs)); /* Write Pitch Data Manager Items */ writeDmItem(dmItems->pitchRads, &pitchRads, sizeof(pitchRads)); writeDmItem(dmItems->pitchDegs, &pitchDegs, sizeof(pitchDegs)); /* Write Roll Data Manager Items */ writeDmItem(dmItems->rollRads, &rollRads, sizeof(rollRads)); writeDmItem(dmItems->rollDegs, &rollDegs, sizeof(rollDegs)); return(OK); } /* updateVmeNavSensorsDm() */ /****************************************************************************/ /* Function : updateVmePsuStatusDm */ /* Purpose : Reads VME Can PSU status & update dmgr items */ /* Inputs : Sio32 chan & VME Can PSU Status data manager items */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateVmePsuStatusDm( sio32Chan *sio32DataChan, DM_Item vmeCanPsuStatus ) { Word psuStatus; /* VME Can Power Supply Status */ if (vmeCanGetPsuStatus(sio32DataChan, &psuStatus) != OK) return(ERROR); psuStatus &= ~NAV_SYNCRO_OK; /* Mask off Syncro Status Bit */ /* Write VME Can PSU status to Dmgr */ return (writeDmItem(vmeCanPsuStatus, &psuStatus, sizeof(psuStatus)) ); } /* updateVmePsuStatusDm() */ /****************************************************************************/ /* Function : updateNavIfPsuStatusDm */ /* Purpose : Reads Nav Interface PSU status & update dmgr items */ /* Inputs : Sio32 chan & Nav I/F PSU Status data manager items */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateNavIfPsuStatusDm( sio32Chan *sio32DataChan, DM_Item navIfStatusDm ) { Word psuStatus; /* Nav I/F Power Supply Status */ if (navIfGetPsuStatus(sio32DataChan, &psuStatus) != OK) return(ERROR); psuStatus &= ~NAV_SYNCRO_OK; /* Mask off Syncro Status Bit */ /* Write Nav I/F PSU status to Dmgr */ return (writeDmItem(navIfStatusDm, &psuStatus, sizeof(psuStatus)) ); } /* updateNavIfPsuStatusDm() */ /****************************************************************************/ /* Function : checkVmeCanPsuStatus */ /* Purpose : Process VME Can PSU Status SRQ & update data mgr item */ /* Inputs : Array of data manager item handles & SRQ data buffer */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL Void checkVmeCanPsuStatus( Reg vmeIbcDmItems* dmItems, Byte *SRQBuffer ) { Word vmePsuStatus; vmePsuStatus = wordFromBuf( SRQBuffer, sizeof(Word) ); vmePsuStatus &= ~NAV_SYNCRO_OK; /* Mask off Syncro Status Bit */ /* Write VME PSU status to Data Mgr */ writeDmItem(dmItems->vmePsuStatus, &vmePsuStatus, sizeof(vmePsuStatus)); } /* checkVmeCanPsuStatus() */ /****************************************************************************/ /* Function : checkNavIfPsuStatus */ /* Purpose : Process VME Can Nav IF PSU status SRQ & update data mgr */ /* Inputs : Array of data manager item handles & SRQ data buffer */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL Void checkNavIfPsuStatus( Reg vmeIbcDmItems* dmItems, Byte *SRQBuffer ) { Word navIfPsuStatus; navIfPsuStatus = wordFromBuf( SRQBuffer, sizeof(Word) ); /* Mask off Syncro Status Bit */ navIfPsuStatus &= ~NAV_SYNCRO_OK; /* Write VME PSU status to Data Mgr */ writeDmItem(dmItems->navIfPsuStatus, &navIfPsuStatus, sizeof(navIfPsuStatus) ) ; } /* checkNavIfPsuStatus() */ MLocal STATUS writeVideoSwitcher(sio32Chan *sio32DataChan, Word videoSwitchCtrl) { Int16 bit; /* Video Switch Relay check bit */ Word cmdStatus; /* Command Result from micro */ /* Ensure that no 2 relays are on */ /* at the same time ie (A1 & A2) */ for (bit = 0; bit < NUM_VME_VIDEO_RELAYS; bit += 2) if ( (videoSwitchCtrl & (0x03 << bit)) == (0x03 << bit)) videoSwitchCtrl &= ~(0x02 << bit); /* Send value to VME IBC CPU */ if ( sendMicroCommand( sio32DataChan, &cmdStatus, sizeof(cmdStatus), 2, VME_IBC_APP | SET_VIDEO_SWITCH, videoSwitchCtrl ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* writeVideoSwitcher() */ /****************************************************************************/ /* Function : initVideoSwitchCtrlDm */ /* Purpose : Read Video Switch Control word & update data manager item */ /* Inputs : Sio32 channel, video switch control dm item handle */ /* Outputs : Returns OK or ERROR */ /***************************************************************************/ LOCAL STATUS initVideoSwitchCtrlDm( sio32Chan *sio32DataChan, DM_Item videoSwitchDm, Word videoSwitch ) { /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem(videoSwitchDm, &videoSwitch, sizeof(videoSwitch)); return (writeVideoSwitcher(sio32DataChan, videoSwitch)); } /* initVideoSwitchCtrlDm() */ /****************************************************************************/ /* Function : writeVideoSwitchCtrl */ /* Purpose : Get video switch ctrl word from dm and send to IBC */ /* Inputs : Sio32 channel, video switch ctrl word dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writeVideoSwitchCtrl( sio32Chan *sio32DataChan, DM_Item videoSwitchCtrlDm ) { Errno status; /* Result of dm_read */ Nat16 videoSwitchCtrl; /* Video Switch Control Word */ /* Read Water Alarm Thresh from dmgr*/ status = dm_read( videoSwitchCtrlDm, (char *) &videoSwitchCtrl, sizeof(videoSwitchCtrl), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) return (writeVideoSwitcher(sio32DataChan, videoSwitchCtrl)); return(ERROR); } /* writeVideoSwitchCtrl() */ /****************************************************************************/ /* Function : kvhCompassCalibrate */ /* Purpose : Calibrate KVH Compass */ /* Inputs : Sio32 channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS kvhCompassCalibrate( sio32Chan *sio32DataChan ) { Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from micro */ /* Send value to VME IBC CPU */ if ( sendMicroCommand( sio32DataChan, &cmdStatus, sizeof(cmdStatus), 1, VME_IBC_APP | CALIBRATE_KVH_COMPASS ) == ERROR ) return(ERROR); /* Check command result */ if (wordFromBuf(&cmdStatus, 0) != CMD_OK) return (ERROR); return(OK); } /* kvhCompassCalibrate() */ MLocal STATUS kvhCompassStringCmd(sio32Chan *sio32DataChan, DM_Item kvhStringDm) { Errno status; /* Result of dm_read */ Nat16 nBytes; char kvhString[KVH_STRING_LEN]; Byte packet[MAX_PACKET_LEN]; /* Command Result from microcontroller */ /* Read String from dm */ status = dm_read( kvhStringDm, kvhString, KVH_STRING_LEN, (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { nBytes = strlen(kvhString) + 1; wordsToBuf(packet, 2, VME_IBC_APP | SEND_KVH_COMPASS_STRING, nBytes); bcopy( kvhString, packet + sizeof(Word) * 2, nBytes); return (microWriteBufReply( sio32DataChan, nBytes + sizeof(Word) * 2, packet)); } /* if */ return (ERROR); } /* kvhCompassString() */ /****************************************************************************/ /* Function : createVmeAppDmItems */ /* Purpose : Create Data Manager Items specific to VME IBC Application */ /* Inputs : Pointer to structure of DM item handles, VME DM prefix, */ /* VME Data Concentrator Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS createVmeAppDmItems( vmeIbcDmItems *vmeDmItems, const char* vmeDmPrefix, const char* vmeDataConName ) { dmItemList vmeIbcDmInit[] = { { &vmeDmItems->waterAlarm0, "Water Alarm Chan 0", WATER_ALARM0_DM, DM_MBOOL, 1 }, { &vmeDmItems->waterAlarm1, "Water Alarm Chan 1", WATER_ALARM1_DM, DM_MBOOL, 1 }, { &vmeDmItems->waterAlarmThresh, "Water Alarm Threshold", WATER_ALARM_THRESH_DM, DM_NAT16, 1 }, { &vmeDmItems->vmePsuStatus, "VME Can PSU Status", VME_CAN_PSU_STATUS_DM, DM_NAT16, 1 }, { &vmeDmItems->navIfPsuStatus, "Navigation Sensor PSU Status", NAV_IF_PSU_STATUS_DM, DM_NAT16, 1 }, { &vmeDmItems->videoSwitchCtrl, "VME Video Switch Control", VME_VIDEO_SWITCH_DM, DM_NAT16, 1 }, { &vmeDmItems->kvhCompassString, "KVH Compass String Cmd", VME_KVH_STRING_CMD_DM, DM_CHAR, KVH_STRING_LEN}, { &vmeDmItems->kvhCompassCal, "KVH Compass Calibration Cmd", VME_KVH_CALIBRATE_DM, DM_EMPTY, 1 }, { &vmeDmItems->kvhCalPosition, "KVH Compass Calibration Position", VME_KVH_CAL_POSITION_DM, DM_INT16, 1 }, { &vmeDmItems->kvhCalScore, "KVH Compass Calibration Score", VME_KVH_CAL_SCORE_DM, DM_NAT16, 1 }, { &vmeDmItems->kvhCalCount, "KVH Compass Calibration Count", VME_KVH_CAL_COUNT_DM, DM_NAT16, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; return (initMicroDmItemList(vmeIbcDmInit, vmeDmPrefix, vmeDataConName)); } /* createVmeAppDmItems() */ /****************************************************************************/ /* Function : createVmeSensorDmItems */ /* Purpose : Create VME IBC Sensor Application Data Manager Items */ /* Inputs : Pointer to structure of DM item handles, VME DM prefix, */ /* Sensor DM prefix, VME Data Concentrator Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS createVmeSensorDmItems( vmeIbcSensorDmItems *vmeSensorDmItems, const char* sensorDmPrefix, const char* vmeDataConName ) { dmItemList sensorDmInit[] = { { &vmeSensorDmItems->compassValid, "Compass Data Valid", COMPASS_DATA_VALID_DM, DM_MBOOL, 1 }, { &vmeSensorDmItems->compassDegs, "Compass Degrees", COMPASS_DEGREES_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->compassRads, "Compass Radians", COMPASS_RADIANS_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->gyroValid, "Gyro Data Valid", GYRO_DATA_VALID_DM, DM_MBOOL, 1 }, { &vmeSensorDmItems->gyroRads, "Gyro Reading (Radians)", GYRO_RADIANS_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->gyroDegs, "Gyro Reading (Degrees)", GYRO_DEGREES_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->gyroRateRads, "Gyro Reading (Radians)", GYRO_RATE_RADIANS_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->gyroRateDegs, "Gyro Reading (Degrees)", GYRO_RATE_DEGREES_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->pitchRads, "Pitch Reading (Radians)", PITCH_RADIANS_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->pitchDegs, "Pitch Reading (Degrees)", PITCH_DEGREES_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->rollRads, "Roll Reading (Radians)", ROLL_RADIANS_DM, DM_FLT32, 1 }, { &vmeSensorDmItems->rollDegs, "Roll Reading (Degrees)", ROLL_DEGREES_DM, DM_FLT32, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; initMicroDmItemList(sensorDmInit, sensorDmPrefix, vmeDataConName); /* Initialize gyro switch status DM Item */ if ( (vmeSensorDmItems->gyroSwitchState = initMicroDmItem("", GYRO_SWITCH_STATUS_DM, DM_ENUM, 1)) == ERROR) { logMsg("VME IBC: Error initializing Gyro Switch Status Item\n"); return (ERROR); } /* if */ dm_start_provider(vmeSensorDmItems->compassValid, DM_STATIC); dm_start_provider(vmeSensorDmItems->compassDegs, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->compassRads, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->gyroValid, DM_STATIC); dm_start_provider(vmeSensorDmItems->gyroRads, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->gyroDegs, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->gyroRateRads, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->gyroRateDegs, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->pitchRads, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->pitchDegs, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->rollRads, NAV_SENSOR_DM_PERIOD); dm_start_provider(vmeSensorDmItems->rollDegs, NAV_SENSOR_DM_PERIOD); dm_start_consumer(vmeSensorDmItems->gyroSwitchState, DM_ASYNC, SEM_NULL); return(OK); } /* createVmeSensorDmItems() */ /****************************************************************************/ /* Function : VME_IBC_Task */ /* Purpose : Initializes communication with VME Can Data Concentrator. */ /* Provides interface between VME Data Concentrator and Data */ /* Manager items. */ /* Inputs : SIO32 serial channel number for VME Data Concentrator */ /* Physically maps the Data Concentrators to a serial channel */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS VME_IBC_Task( Nat16 serialChannel ) { vmeIbcDmItems dmItems; /* VME Application Data Manager handles */ microIOControl microIOCtrl; /* Microcontroller IO Task control */ Int16 board; /* LPS Board Index */ /* Initialize serial channel and task */ /* synchronization resources */ if (initMicroTaskIO( µIOCtrl, NUM_TASKS, serialChannel, vmeDmPrefix, "/sio32/vmeIBC", NO_ITEM) == ERROR) { logMsg("Error initializing IO for VME Can IBC"); return(ERROR); } /* if */ /* Clear list of VME Data Manager handles */ bzero((char *) &dmItems, sizeof(dmItems)); /* Make static power request for VME Can */ staticPowerMgrRequest( VME_240V_BUS_PWR_REQ_DM, VME_240V_BUS_PWR_OK_DM, VME_240V_BUS_POWER, A240, HOTEL ); staticPowerMgrRequest( VME_48V_BUS_PWR_REQ_DM, VME_48V_BUS_PWR_OK_DM, VME_48V_BUS_POWER, A48, HOTEL ); /* Create IBC CPU Data Manager Items */ if (createIbcCpuDmItems(&dmItems.ibcDm, vmeDmPrefix, vmeDataConName) == ERROR) { /* Error occurred so print message & exit */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ /* Create Virtual Serial Port Data Manager Items */ if (createIbcVspDmItems(&dmItems.vspDm, vmeDmPrefix, vmeDataConName, 0) == ERROR) { /* Error occured so print message & exit */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ /* Create VME Application Items */ if (createVmeAppDmItems(&dmItems, vmeDmPrefix, vmeDataConName) == ERROR) { /* Error occurred so print message & exit */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ /* Declare this task an item provider */ dm_start_provider(dmItems.waterAlarm0, DM_STATIC); dm_start_provider(dmItems.waterAlarm1, DM_STATIC); dm_start_provider(dmItems.vmePsuStatus, DM_STATIC); dm_start_provider(dmItems.navIfPsuStatus, DM_STATIC); dm_start_provider(dmItems.kvhCalPosition, DM_STATIC); dm_start_provider(dmItems.kvhCalScore, DM_STATIC); dm_start_provider(dmItems.kvhCalCount, DM_STATIC); /* Create GF/5V Board Data Manager Items */ createIbcGf5vDmItems( &dmItems.gf5vDm, vmeDmPrefix, vmeDataConName ); initIbcGf5vAlarmProvider( &dmItems.gf5vDm ); /* Create IBC LPS Data Manager Items */ for (board = 0; board < LPS_BOARDS; board++) { /* Create LPS Data Manager Items */ createIbcLpsDmItems( &dmItems.lpsDm[board], vmeDmPrefix, vmeDataConName, board ); initIbcLpsAlarmProvider( &dmItems.lpsDm[board] ); } /* for */ /* Create Quad Serial Data Mgr Items */ createIbcQuadSerDmItems(&dmItems.quadSerDm, vmeDmPrefix, vmeDataConName, 0); initIbcQuadSerAlarmProvider( &dmItems.quadSerDm ); initIbcVspProvider(&dmItems.vspDm); /* Initialize VME IBC Data Output Task */ if ((microIOCtrl.childTasks[OUT_TASK].taskId = taskSpawn("vmeIbcOut", VME_IBC_OUT_PRIORITY, 0, VME_IBC_OUT_STACKSIZE, (FUNCPTR) VME_IBC_OutTask, (int) &dmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { /* Close serial communication channel */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); logMsg("VME IBC: Error spawning data output task\n"); return(ERROR); } /* if */ /* Initialize VME IBC Data Input Task */ if (( microIOCtrl.childTasks[IN_TASK].taskId = taskSpawn( "vmeIbcIn", VME_IBC_IN_PRIORITY, 0, VME_IBC_IN_STACKSIZE, (FUNCPTR) VME_IBC_InTask, (int) &dmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0) ) == ERROR) { /* Delete VME IBC A Output Task */ taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); logMsg("VME IBC: Error spawning data input task\n"); return(ERROR); } /* if */ /* Initialzie VME Sensor Data Input Task */ if (( microIOCtrl.childTasks[AUX_IN_TASK].taskId = taskSpawn( "vmeIbcISensor", VME_IBC_SENSOR_PRIORITY, VX_FP_TASK, VME_SENSOR_STACKSIZE, (FUNCPTR) VME_IBC_SensorTask, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { /* Delete VME IBC A Output Task */ taskDelete(microIOCtrl.childTasks[IN_TASK].taskId); taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); logMsg("VME IBC: Error spawning data input task\n"); return(ERROR); } /* if */ FOREVER { /* Check if VME DCon is switched on */ if ((microDevPowerSwitchState(µIOCtrl) != SWITCH_ON) || ((microDevPowerSwitchState(µIOCtrl) == SWITCH_ON) && (microIOCtrl.resetEventCount != 0))) /* Process SRQ's from micro */ while(processVmeIbcSRQ(&dmItems, µ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( &dmItems.ibcDm, µIOCtrl.sio32DataChan, vmeDataConName, ibcCardSet) == OK) { /* Initialize VME IBC Application Items */ initVmeIbcDmItems( &dmItems, µIOCtrl.sio32DataChan ); updateIbcGf5vAlarmsDm( µIOCtrl.sio32DataChan, GF5V_BOARD_ADDR, &dmItems.gf5vDm ); updateIbcQuadSerAlarmsDm( µIOCtrl.sio32DataChan, SERIAL_BOARD_ADDR, &dmItems.quadSerDm ); updateIbcVspDmItems( µIOCtrl.sio32DataChan, SERIAL_BOARD_ADDR, VIDEO_TELEM_CHAN, &dmItems.vspDm, vmeDataConName ); for (board = 0; board < LPS_BOARDS; board++) updateIbcLpsAlarmsDm( µIOCtrl.sio32DataChan, lpsBoardAddr[board], &dmItems.lpsDm[board] ); /* 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[AUX_IN_TASK].taskId); taskDelete(microIOCtrl.childTasks[IN_TASK].taskId); taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); /* Close serial communication channel */ sio32ChanDestroy( serialChannel, µ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 */ } /* VME_IBC_Task() */ /*****************************************************************************/ /* Function : initVmeIbcDmItems */ /* Purpose : Reads parameters from VME IBC & update Data Manager items */ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : None, but Data Manager Items change */ /*****************************************************************************/ LOCAL Void initVmeIbcDmItems( vmeIbcDmItems* dmItems, sio32Chan* vmeIbcDataChan ) { /* Set Micro Water Alarms initial value */ if (updateWaterAlarmsDm(vmeIbcDataChan, dmItems->waterAlarm0, dmItems->waterAlarm1) == ERROR) logMsg("VME IBC: Error checking water alarm status\n"); /* Set VME Can PSU Status initial value */ if (updateVmePsuStatusDm(vmeIbcDataChan, dmItems->vmePsuStatus) == ERROR) logMsg("VME IBC: Error checking VME PSU Status\n"); /* Set Nav IF PSU Status initial value */ if (updateNavIfPsuStatusDm(vmeIbcDataChan, dmItems->navIfPsuStatus) == ERROR) logMsg("VME IBC: Error checking VME Navigation I/F PSU Status\n"); } /* initVmeIbcDmItems() */ /*****************************************************************************/ /* Function : VME_IBC_OutTask */ /* Purpose : VME IBC Output Task. Read DM items and write them to VME IBC*/ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void VME_IBC_OutTask(Reg vmeIbcDmItems *dmItems, microIOControl *microIOCtrl) { DM_Group cpuGroup; /* CPU & Misc Item Data Manager Group */ DM_Group gf5vGroup; /* GF/5V Board Data Manager Group */ DM_Group vspGroup; /* VSP Data Manager Group */ DM_Group lpsGroup[LPS_BOARDS]; /* LPS Board Data Manager Group */ SEM_ID dmUpdateSem; /* Data Manager Item Change wakeup sem */ SEM_ID dmCalPosSem; /* Calibration Position wakeup sem */ SEM_ID powerReqSem; /* Power Request Update Semaphore */ Int16 board; /* LPS Board Index */ cpuUpdateBits ibcCpuBits; /* CPU Board Parameter Update Bits */ gf5vUpdateBits gf5vBits; /* GF/5V Board Paramater Update Bits */ vspUpdateBits vspBits; /* VSP Parameter Update Bits */ lpsUpdateBits lpsBits[LPS_BOARDS]; /* LPS Board Paramater Update Bits */ DWord vmeIbcGroupBits; /* Holds dm_group_changes bit vector */ DWord waterAlarmThrshBit; /* Water alarm threshold item change bit */ DWord videoSwitchBit; /* Video Switch cotnrol item change bit */ DWord kvhCalibrateBit; /* KVH Compass Calibrate item change bit */ DWord kvhStringCmdBit; /* KVH Compass String Cmd item change bit*/ DWord lpsEnableBit; /* LPS Output Enable Request changed bit */ DWord gyroSwitchBit; /* Gyro Switch Request changed bit */ DWord sonarSwitchBit; /* Sonar Switch Request changed bit */ DWord hydrophoneSwitchBit; /* Hydrophone Switch Request changed bit */ switchEntry vmeLpsEnableSwitch; /* VME IBC Low Power Switch Enable switch*/ switchEntry gyroSwitch; /* Gyro Power Switch */ switchEntry sonarSwitch; /* Imaging Sonar Power Switch */ switchEntry hydrophoneSwitch; /* Hydrophone Power Switch */ /*****************************************************************************/ /* The code is this task implements the following interconnected switches for the VME can IBC. LPS Enable LPS 0-4 48V Bus _______________ \____________ \_______ spare LPS _______| | | | | |_ \_______ spare LPS _______| | | | | |_ \______ Imaging Sonar ____| | | | | |_ \_________ gyro __________| | | | | |____________________________ hydrophones _______| */ /*****************************************************************************/ /* Create switch & power sync semaphores */ if ( ((dmUpdateSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) || ((dmCalPosSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) || ((powerReqSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) ) { logMsg("VME IBC: Error initializing Input Task Resources\n"); return; } /* if */ initIbcSwitchEntry( &vmeLpsEnableSwitch, SERIAL_CHAN, VICOR_STANDBY_POWER, A48, HOTEL, LOW_POWER_SWITCH, LPS_28V_BOARD_ADDR, LPS_VICOR_ENABLE, VME_LPS_ENABLE_SWITCH_REQ_DM, VME_LPS_ENABLE_STATUS_DM, dmUpdateSem, VME_LPS_ENABLE_PWR_REQ_DM, VME_LPS_ENABLE_PWR_OK_DM, powerReqSem, vmeDataConName, "LPS Enable"); initIbcSwitchEntry( &gyroSwitch, SERIAL_CHAN, GYRO_POWER, VME_CAN_BUS, HOTEL, LOW_POWER_SWITCH, LPS_28V_BOARD_ADDR, GYRO_SWITCH_CHAN, GYRO_SWITCH_REQ_DM, GYRO_SWITCH_STATUS_DM, dmUpdateSem, GYRO_PWR_REQ_DM, GYRO_PWR_OK_DM, powerReqSem, vmeDataConName, "Gyro Switch"); initIbcSwitchEntry( &sonarSwitch, SERIAL_CHAN, SONAR_POWER, VME_CAN_BUS, HOTEL, LOW_POWER_SWITCH, LPS_28V_BOARD_ADDR, IMAG_SONAR_SWITCH_CHAN, IMAG_SONAR_SWITCH_REQ_DM, IMAG_SONAR_SWITCH_STATUS_DM, dmUpdateSem, IMAG_SONAR_PWR_REQ_DM, IMAG_SONAR_PWR_OK_DM, powerReqSem, vmeDataConName, "Imaging Sonar Switch"); initIbcSwitchEntry( &hydrophoneSwitch, SERIAL_CHAN, HYDROPHONE_POWER, VME_CAN_BUS, HOTEL, HYDROPHONE_PSU, HYDROPHONE_PSU_ADDR, 0, HYDROPHONE_SWITCH_REQ_DM, HYDROPHONE_SWITCH_STATUS_DM, dmUpdateSem, HYDROPHONE_PWR_REQ_DM, HYDROPHONE_PWR_OK_DM, powerReqSem, vmeDataConName, "Hydrophone Power Switch"); /* Devices on VME Can Low Power Switch */ ibcAddSwitchInSeries( &vmeLpsEnableSwitch, &gyroSwitch ); ibcAddSwitchInSeries( &vmeLpsEnableSwitch, &sonarSwitch ); ibcConnectLpsDmItems( &dmItems->lpsDm[0], &gyroSwitch, vmeDataConName, "Gyro", GYRO_SWITCH_CHAN, GYRO_CURRENT_DM, GYRO_CURRENT_ALARM_DM, GYRO_CURRENT_WARN_THRESH_DM, GYRO_CURRENT_ALARM_THRESH_DM ); ibcConnectLpsDmItems( &dmItems->lpsDm[0], &sonarSwitch, vmeDataConName, "Imaging Sonar", IMAG_SONAR_SWITCH_CHAN, IMAG_SONAR_TEMP_DM, IMAG_SONAR_TEMP_ALARM_DM, IMAG_SONAR_TEMP_WARN_THRESH_DM, IMAG_SONAR_TEMP_ALARM_THRESH_DM ); /* Create Data Manager Group for items */ cpuGroup = initIbcCpuConsumer(&dmItems->ibcDm, &ibcCpuBits, dmUpdateSem); dm_start_consumer(dmItems->waterAlarmThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->videoSwitchCtrl, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->kvhCompassString, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->kvhCompassCal, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->kvhCalPosition, DM_STATIC, dmCalPosSem); /* Use CPU Group for misc items */ dm_group_add_item(cpuGroup, dmItems->waterAlarmThresh, &waterAlarmThrshBit); dm_group_add_item(cpuGroup, dmItems->videoSwitchCtrl, &videoSwitchBit); dm_group_add_item(cpuGroup, dmItems->kvhCompassString, &kvhStringCmdBit); dm_group_add_item(cpuGroup, dmItems->kvhCompassCal, &kvhCalibrateBit); /* Create GF/5V Data Manager Group */ gf5vGroup = initIbcGf5vConsumer( &dmItems->gf5vDm, &gf5vBits, dmUpdateSem ); /* Create LPS Data Manager Group */ for (board = 0; board < LPS_BOARDS; board++) lpsGroup[board] = initIbcLpsConsumer( &dmItems->lpsDm[board], &lpsBits[board], dmUpdateSem ); /* Create Virtual Serial Port Data Manager Group */ vspGroup = initIbcVspConsumer( &dmItems->vspDm, &vspBits, dmUpdateSem ); /* Add Switch Requests to DM Group */ dm_group_add_item(cpuGroup, vmeLpsEnableSwitch.switchRequestDm, &lpsEnableBit); dm_group_add_item(cpuGroup, gyroSwitch.switchRequestDm, &gyroSwitchBit); dm_group_add_item(cpuGroup, sonarSwitch.switchRequestDm, &sonarSwitchBit); dm_group_add_item(cpuGroup, hydrophoneSwitch.switchRequestDm, &hydrophoneSwitchBit); FOREVER { /* Mark micro configuration as invalid */ microIOCtrl->microConfigured = FALSE; /* Wait for micro reset via SRQ Task */ semTake(microIOCtrl->childTasks[OUT_TASK].taskSyncSem, WAIT_FOREVER); ibcSetSwitchState( &gyroSwitch, SWITCH_OFF ); ibcSetSwitchState( &sonarSwitch, SWITCH_OFF ); ibcSetSwitchState( &hydrophoneSwitch, SWITCH_OFF ); ibcSetSwitchState( &vmeLpsEnableSwitch, SWITCH_ON ); initIbcHumidityThreshDm(SERIAL_CHAN, dmItems->ibcDm.humidityWarnThresh, dmItems->ibcDm.humidityAlarmThresh ); initWaterAlarmThreshDm( SERIAL_CHAN, dmItems->waterAlarmThresh ); #if 0 initVideoSwitchCtrlDm( SERIAL_CHAN, dmItems->videoSwitchCtrl, VIDEO_RELAY_A2 | VIDEO_RELAY_B2 | VIDEO_RELAY_C2 | VIDEO_RELAY_D1 | VIDEO_RELAY_E1 | VIDEO_RELAY_F1 ); #endif initVideoSwitchCtrlDm( SERIAL_CHAN, dmItems->videoSwitchCtrl, 0x0055); updateIbcGf5vDmItems( SERIAL_CHAN, GF5V_BOARD_ADDR, &dmItems->gf5vDm, vmeDataConName ); /* Set default charge time for VME IBC */ ibcGf5vSetChargeTimes(SERIAL_CHAN, GF5V_BOARD_ADDR, &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, VIDEO_TELEM_CHAN, &dmItems->vspDm, vmeDataConName ); for (board = 0; board < LPS_BOARDS; board++) updateIbcLpsDmItems( SERIAL_CHAN, lpsBoardAddr[board], &dmItems->lpsDm[board], vmeDataConName ); setLpsVoltageThresh( SERIAL_CHAN, lpsBoardAddr[0], &dmItems->lpsDm[0], VME_LPS_VOLT_WARN_THRESH, VME_LPS_VOLT_ALARM_THRESH ); /* Clear changed item bits in DM */ vmeIbcGroupBits = 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 sensor input task */ semGive(microIOCtrl->childTasks[AUX_IN_TASK].taskSyncSem); while (microIOCtrl->serialLinkState == LINK_UP) { /* Repeat while serial link is up */ /* Wait for any data items to change */ semTake(dmUpdateSem, WAIT_FOREVER); /* Ask Data Manager which items changed */ vmeIbcGroupBits = dm_get_group_changes(cpuGroup); /* Check if micro Reset Event occurred */ if (vmeIbcGroupBits & ibcCpuBits.resetEventBit) break; /* Exit loop and re-initialize DM items */ taskSafe(); /* Prevent task from being deleted during*/ /* serial IO and Data Manager updates */ /* VME Can LPS Output Enable Request */ if (vmeIbcGroupBits & lpsEnableBit) ibcProcessSwitchRequest( &vmeLpsEnableSwitch ); /* Gyro Power Switch Request */ if (vmeIbcGroupBits & gyroSwitchBit) ibcProcessSwitchRequest( &gyroSwitch ); /* Sonar Power Switch Request */ if (vmeIbcGroupBits & sonarSwitchBit) ibcProcessSwitchRequest( &sonarSwitch ); /* Hydrophone Power Switch Request */ if (vmeIbcGroupBits & hydrophoneSwitchBit) ibcProcessSwitchRequest( &hydrophoneSwitch ); /* 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, VIDEO_TELEM_CHAN, &dmItems->vspDm, &vspBits, vspGroup ); /* Check for LPS Board Parameter Update */ for (board = 0; board < LPS_BOARDS; board++) ibcLpsUpdateCheck( SERIAL_CHAN, lpsBoardAddr[board], &dmItems->lpsDm[board], &lpsBits[board], lpsGroup[board] ); /* Send Set Humidity Alarm Threshold cmd */ if ((vmeIbcGroupBits & ibcCpuBits.humidityWarnThreshBit) || (vmeIbcGroupBits & ibcCpuBits.humidityAlarmThreshBit)) writeIbcHumidityThresh(SERIAL_CHAN, dmItems->ibcDm.humidityWarnThresh, dmItems->ibcDm.humidityAlarmThresh); /* Send Set Water Alarm Threshold cmd */ if (vmeIbcGroupBits & waterAlarmThrshBit) writeWaterAlarmThresh(SERIAL_CHAN,dmItems->waterAlarmThresh); /* Send Video Switch Control Word Cmd */ if (vmeIbcGroupBits & videoSwitchBit) writeVideoSwitchCtrl(SERIAL_CHAN, dmItems->videoSwitchCtrl); /* Send String to KVH Compass */ if (vmeIbcGroupBits & kvhStringCmdBit) kvhCompassStringCmd(SERIAL_CHAN, dmItems->kvhCompassString); /* Send Start Calibration Cmd */ if (vmeIbcGroupBits & kvhCalibrateBit) kvhCompassCalibrate(SERIAL_CHAN); /* Check if Reset Command request occurred*/ if (vmeIbcGroupBits & ibcCpuBits.resetCmdBit) { /* Mark serial link as down */ setSerialLinkState(microIOCtrl, LINK_DOWN); /* Manual reset so ammounce 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 */ } /* 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 ! */ } /* VME_IBC_OutTask() */ /*****************************************************************************/ /* Function : VME_IBC_InTask */ /* Purpose : VME IBC Input Task. Wakes up periodically, read data from */ /* the VME IBC 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 VME_IBC_InTask(Reg vmeIbcDmItems* dmItems, microIOControl *microIOCtrl) { SEM_ID wakeupSem; /* Used to wake up this task periodically */ Int32 updates = 0; /* Counts dm updates for rate calculation */ Int16 board; /* LPS Board Index */ /* Create wakeup semaphore */ /* timer for provider rate control */ if ( (wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("VME IBC: Error initializing Input Task Resources\n"); return; } /* if */ /* Provider Task for non-static data items*/ initIbcCpuProvider (&dmItems->ibcDm, HUMIDITY_DM_PERIOD); initIbcGf5vProvider (&dmItems->gf5vDm, VME_IBC_DM_PERIOD); for (board = 0; board < LPS_BOARDS; board++) initIbcLpsProvider(&dmItems->lpsDm[board], VME_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 */ updateNewIbcGf5vDm(SERIAL_CHAN, GF5V_BOARD_ADDR, &dmItems->gf5vDm); for (board = 0; board < LPS_BOARDS; board++) updateIbcLpsDm(SERIAL_CHAN, lpsBoardAddr[board], &dmItems->lpsDm[board]); /* if ((updates % (HUMIDITY_DM_PERIOD / VME_IBC_DM_PERIOD)) == 0) */ updateIbcHumidityDm(SERIAL_CHAN, dmItems->ibcDm.microHumidity); taskUnsafe(); /* Task may now be deleted safely */ /* Increment & range check update counter */ if (++updates == 10000) updates = 0; /* Pend until sem timeout wakes us up */ semTake(wakeupSem, sysClkRateGet() / 2); /* (USECS_PER_SEC / VME_IBC_DM_PERIOD)); */ } /* while */ } /* 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 ! */ } /* VME_IBC_InTask() */ /*****************************************************************************/ /* Function : VME_IBC_SensorTask */ /* Purpose : VME IBC Sensor Task. Wakes up periodically, read sensor data*/ /* from VME IBC and write Data Manager Items */ /* Inputs : Serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void VME_IBC_SensorTask(microIOControl *microIOCtrl) { SEM_ID wakeupSem; /* Used to wake up this task periodically*/ Nat32 ticks; /* For subtacting loop time from timer */ Int32 delay; /* Calculated loop delay */ /* VME Application Data Manager handles */ vmeIbcSensorDmItems vmeSensorDmItems; /* Clear list of VME Data Manager handles*/ bzero((char *) &vmeSensorDmItems, sizeof(vmeSensorDmItems)); /* Create VME Sensor Application Items */ if (createVmeSensorDmItems(&vmeSensorDmItems, sensorDmPrefix, vmeDataConName) == ERROR) { /* Error occurred so print message & exit*/ logMsg("VME IBC: Error initializing Sensor Data Manater Items\n"); return; } /* if */ /* Create wakeup semaphore */ /* timer for provider rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("VME IBC: Error initializing Sensor Task Resources\n"); return; } /* if */ FOREVER { /* Wait for IBC Coms to be initialized */ semTake(microIOCtrl->childTasks[AUX_IN_TASK].taskSyncSem, WAIT_FOREVER); ticks = tickGet(); /* Update Rate is not guaranteed due to */ /* the possibility of task preemption */ while (microIOCtrl->serialLinkState == LINK_UP) { delay = sysClkRateGet() / (USECS_PER_SEC / NAV_SENSOR_DM_PERIOD) - min(tickGet() - ticks, sysClkRateGet() / (USECS_PER_SEC / NAV_SENSOR_DM_PERIOD)); if (delay > 0) /* Pend until sem timeout wakes us up */ semTake(wakeupSem, delay); ticks = tickGet(); taskSafe(); /* Prevent task from being deleted during */ /* serial IO and Data Manager updates */ updateVmeNavSensorsDm( SERIAL_CHAN, &vmeSensorDmItems ); taskUnsafe(); /* Task may now be deleted safely */ } /* while */ } /* 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 ! */ } /* VME_IBC_SensorTask() */ /******************************************************************************/ /* Function : processVmeIbcSRQ */ /* Purpose : Decode Service Request and take appropriate action */ /* Inputs : SRQ code, array of DM items, serial channel */ /* Outputs : Returns Reset Status & many DM items are changed */ /******************************************************************************/ MLocal Word processVmeIbcSRQ(Reg vmeIbcDmItems* dmItems, microIOControl *microIOCtrl) { Word srqRequest; /* Service Request Value */ Word alarmStatus; /* Microcontroller Alarm Status Word */ Nat16 calScore, calCount; /* KVH Compass calibration results */ Nat16 calPosition; /* KVH Compass calibration position */ Byte buffer[SRQ_PACKET_LEN]; /* Buffer for micro communications data */ /* Wait for Service Requests */ if (readSRQPacket(SERIAL_CHAN, buffer, SRQ_PACKET_LEN, WAIT_FOREVER) == ERROR) return (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 WATER_ALARM_ON_SRQ: /* Water Alarm Active */ case WATER_ALARM0_ON_SRQ: /* Water Alarm 0 Active */ writeBooleanDmItem(dmItems->waterAlarm0, TRUE); break; case WATER_ALARM0_OFF_SRQ: /* Water Alarm 0 Inactive */ writeBooleanDmItem(dmItems->waterAlarm0, FALSE); break; case WATER_ALARM1_ON_SRQ: /* Water Alarm 1 Active */ writeBooleanDmItem(dmItems->waterAlarm1, TRUE); break; case WATER_ALARM1_OFF_SRQ: /* Water Alarm 1 Inactive */ writeBooleanDmItem(dmItems->waterAlarm1, FALSE); break; case VME_CAN_PSU_ERROR_SRQ: /* VME Can Power Supply Failure */ checkVmeCanPsuStatus( dmItems, buffer ); break; case NAV_IF_PSU_STATUS: /* Navigation Interface PSU Status */ checkNavIfPsuStatus( dmItems, buffer ); break; case KVH_COMPASS_CAL_SRQ: /* KVH Compass Cal Position Update */ wordsFromBuf(buffer, 2, &srqRequest, &calPosition); writeDmItem(dmItems->kvhCalPosition, &calPosition, sizeof(calPosition)); break; case KVH_COMPASS_CAL_DATA_SRQ: /* KVH Compass Calibration Data */ wordsFromBuf(buffer, 4, &srqRequest, &calPosition, &calScore, &calCount); writeDmItem(dmItems->kvhCalPosition, &calPosition, sizeof(calPosition)); writeDmItem(dmItems->kvhCalScore, &calScore, sizeof(calCount)); writeDmItem(dmItems->kvhCalCount, &calCount, sizeof(calScore)); break; default: if (processIbcCpuSRQ(srqRequest, buffer, &dmItems->ibcDm ) != TRUE) if (processIbcLpsSRQ(srqRequest, buffer, lpsBoardAddr[0], &dmItems->lpsDm[0] ) != TRUE) if (processIbcGf5vSRQ(srqRequest, buffer, GF5V_BOARD_ADDR, &dmItems->gf5vDm ) != TRUE) if (processIbcVspSRQ(srqRequest, buffer, SERIAL_BOARD_ADDR, VIDEO_TELEM_CHAN, &dmItems->vspDm ) != TRUE) if (processIbcQuadSerSRQ(srqRequest, buffer, SERIAL_BOARD_ADDR, &dmItems->quadSerDm ) != TRUE) printf("VME IBC: Unrecognized SRQ 0x%x\n", srqRequest); break; } /* switch */ return(srqRequest); /* Return SRQ received to caller */ } /* processVmeIbcSRQ() */