/****************************************************************************/ /* Copyright 1992 to 1996 MBARI */ /****************************************************************************/ /* Summary : ROV Pilot Console Interface module for vxWorks */ /* Filename : console.c */ /* Author : Andrew Pearce */ /* Project : Tiburon ROV */ /* Version : Version 1.0 */ /* Created : 11/12/92 */ /* Modified : 07/30/96 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: console.c,v 1.1 93/07/01 14:40:03 pean Exp $ * $Log: console.c,v $ * Revision 1.1 93/07/01 14:40:03 14:40:03 pean (Andrew Pearce) * Initial revision * */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks linked list library */ #include /* vxWorks string library functions */ #include /* vxWorks task library functions */ #include /* vxWorks watch dog timer functions */ #include /* vxWorks semaphore functions */ #include /* vxWorks Message Queue Library */ #include /* MBARI style guide type declarations */ #include /* Miscellaneous constants */ #include /* MBARI module numbers for error code */ #include /* MBARI Rov Application Priorities */ #include /* MBARI time and date functions */ #include /* Data Manager declarations */ #include /* Data Manager error declarations */ #include "tiburon.h" /* Tiburon definitions */ #include "powerDM.h" /* Power System Data Manager Items */ #include "powerTypes.h" /* Power management Data Types */ #include "thrusterDM.h" /* Thruster Data Manager Item Names */ #include "controlDM.h" /* Control Data Manager Item Names */ #include "bellyPack.h" /* Bellypack Data Manager Item Names */ #include "lightingDcon.h" /* Lighting Data Concentrator */ #include "console.h" /* Console Data Manager Item Names */ #include "datel612.h" /* Datel 612 A to D board definitions */ #include "vmi2120.h" /* VMI 2120 digital output board defs */ #include "vmi1110.h" /* VMI 1110 digital input board defs */ #include "sio32.h" /* Sio32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #include "microDm.h" /* Microcontroller Data Manager Items */ #include "powerIbc.h" /* Power Can IBC Data Manager Items */ #include "switches.h" /* Console switch channel assignments */ /* Joystick Analog Channel Assignments */ #define JOYSTICK_Z_CHAN 0 /* Depth Control Joystick Channel */ #define JOYSTICK_Z_SWITCH_CHAN 1 /* Depth Control Joystick Switch */ #define JOYSTICK_X_CHAN 2 /* Main Joystick X Axis Channel */ #define JOYSTICK_Y_CHAN 3 /* Main Joystick Y Axis Channel */ #define JOYSTICK_YAW_CHAN 4 /* Main Joystick Yaw Axis Channel */ #define JOYSTICK_SWITCH_CHAN 5 /* Main Joystick Switch Channel */ #define PORT_PAN_CHAN 6 /* Port Pan Axis Channel */ #define PORT_TILT_CHAN 7 /* Port Tilt Axis Channel */ #define PORT_PT_THETA_CHAN 8 /* Port Rotation Channel */ #define PORT_PT_SWITCH_CHAN 9 /* Port Pan/Tilt Switch Channel */ #define STBD_PAN_CHAN 10 /* Stbd Pan Axis Channel */ #define STBD_TILT_CHAN 11 /* Stbd Tilt Axis Channel */ #define STBD_PT_THETA_CHAN 12 /* Stbd Rotation Channel */ #define STBD_PT_SWITCH_CHAN 13 /* Stbd Pan/Tilt Switch Channel */ #define JOYSTICK1_FULL_SCALE 1024.0 /* Main Joystick Full Scale Value */ #define JOYSTICK2_FULL_SCALE 1024.0 /* Depth Joystick Full Scale Value */ /* Joystick Zero Offset Calibration */ #define JOYSTICK_X_OFFSET 51 /* Main Joystick X axis offset */ #define JOYSTICK_Y_OFFSET 24 /* Main Joystick Y axis offset */ #define JOYSTICK_YAW_OFFSET -16 /* Main Joystick Yaw axis offset */ #define JOYSTICK_Z_OFFSET -3 /* Depth Joystick Z azis offset */ #define PT_PORT_PAN_OFFSET -3 #define PT_PORT_TILT_OFFSET -3 #define PT_STBD_PAN_OFFSET -1 #define PT_STBD_TILT_OFFSET 4 /* Joystick Full Scale Calibration */ #define JOYSTICK_X_POS_FSV 973.0 /* Main Joystick positive X Max Value */ #define JOYSTICK_X_NEG_FSV -1073.0 /* Main Joystick negative X Max Value */ #define JOYSTICK_Y_POS_FSV 1002.0 /* Main Joystick positive Y Max Value */ #define JOYSTICK_Y_NEG_FSV -1043.0 /* Main Joystick negative Y Max Value */ #define JOYSTICK_YAW_POS_FSV 1045.0 /* Main Joystick positive Yaw Max Value*/ #define JOYSTICK_YAW_NEG_FSV -1001.0 /* Main Joystick negative Yaw Max Value*/ #define JOYSTICK_Z_POS_FSV 204.0 /* Depth Joystick positive X Max Value */ #define JOYSTICK_Z_NEG_FSV -217.0 /* Depth Joystick negative X Max Value */ #define PT_PORT_PAN_POS_FSV 212.0 #define PT_PORT_PAN_NEG_FSV -214.0 #define PT_PORT_TILT_POS_FSV 211.0 #define PT_PORT_TILT_NEG_FSV -215.0 #define PT_STBD_PAN_POS_FSV 216.0 #define PT_STBD_PAN_NEG_FSV -214.0 #define PT_STBD_TILT_POS_FSV 216.0 #define PT_STBD_TILT_NEG_FSV -214.0 #define JOYSTICK1_DEADBAND 2.0 /* Main Joystick Deadband region */ #define JOYSTICK2_DEADBAND 8.0 /* Depth Joystick Deadband region */ #define JOYSTICK_YAW_DEADBAND 4.0 /* Yaw Joystick Deadband region */ #define PT_JOYSTICK_DEADBAND 6.0 /* Pan/Tilt joystick Deadband region */ #define ANALOG_SWITCH_THRESH 512 /* Threshold for analog switch channels */ const Nat16 ballastSwitchAssignments[] = { LIGHT_BALLAST_0_SWITCH, LIGHT_BALLAST_1_SWITCH, LIGHT_BALLAST_2_SWITCH }; const Nat16 lightSwitchAssignments[] = { LIGHT_1_SWITCH, LIGHT_2_SWITCH, LIGHT_3_SWITCH, LIGHT_4_SWITCH, LIGHT_5_SWITCH, LIGHT_6_SWITCH }; /****************************************************************************/ typedef struct /* Console Switch & Lamp Data Manager Items */ { DM_Item motorSwitch; DM_Item portHorizSwitch; DM_Item stbdHorizSwitch; DM_Item fwdLatSwitch; DM_Item aftLatSwitch; DM_Item portVertSwitch; DM_Item stbdVertSwitch; DM_Item motorEnable; DM_Item portHorizEnable; DM_Item stbdHorizEnable; DM_Item fwdLatEnable; DM_Item aftLatEnable; DM_Item portVertEnable; DM_Item stbdVertEnable; DM_Item cameraDconSwitch; DM_Item sensorDconSwitch; DM_Item sonarDconSwitch; DM_Item ersDconSwitch; DM_Item manip48VSwitch; DM_Item manip240VSwitch; DM_Item toolsled48VSwitch; DM_Item toolsled240VSwitch; DM_Item lightingDcon48VSwitch; DM_Item lightingDcon240VSwitch; DM_Item ballastSwitch[NUM_LIGHT_BALLASTS]; DM_Item lightSwitch[NUM_LIGHTS]; DM_Item portPanTiltSwitch; DM_Item stbdPanTiltSwitch; DM_Item backupLightsSwitch; DM_Item vbSystemSwitch; DM_Item gyroSwitch; DM_Item imagSonarSwitch; DM_Item responderSwitch; DM_Item hydrophoneSwitch; DM_Item autoDepth; DM_Item autoAltitude; DM_Item autoHeading; DM_Item autoPosition; DM_Item joystickLock; /* Joystick locked mode flag */ DM_Item joystickRange; /* Joystick range (percentage) */ DM_Item bellypackEnable; /* Bellypack Enable flag (MBool) */ } consoleSwitchDmItems; typedef struct { DM_Item joystickX; /* Joystick X value */ DM_Item joystickY; /* Joystick Y value */ DM_Item joystickZ; /* Joystick Z value */ DM_Item joystickYaw; /* Joystick rotation */ DM_Item autoDepth; /* Auto depth mode flag */ DM_Item autoHeading; /* Auto heading mode flag */ } ctrlFunctionDmItems; typedef struct { DM_Item portPanAxis; DM_Item portTiltAxis; DM_Item stbdPanAxis; DM_Item stbdTiltAxis; } panTiltDmItems; MLocal STATUS createCtrlFunctionDmItems(const char *prefix, ctrlFunctionDmItems *dmItems) { dmItemList ctrlFuncDmInit[] = /* Data Manager Item Table for table */ { { &dmItems->joystickX, "Joystick X", CONSOLE_JOYSTICK_X_DM, DM_INT16, 1 }, { &dmItems->joystickY, "Joystick Y", CONSOLE_JOYSTICK_Y_DM, DM_INT16, 1 }, { &dmItems->joystickZ, "Joystick Z", CONSOLE_JOYSTICK_Z_DM, DM_INT16, 1 }, { &dmItems->joystickYaw, "Joystick Heading", CONSOLE_JOYSTICK_HEADING_DM, DM_INT16, 1 }, { &dmItems->autoDepth, "Auto Depth Switch", CONSOLE_AUTO_DEPTH_DM, DM_MBOOL, 1 }, { &dmItems->autoHeading, "Auto Heading Switch", CONSOLE_AUTO_HEADING_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Initialize Data Manager Items from table*/ if (initMicroDmItemList(ctrlFuncDmInit, prefix, "") == ERROR) return (ERROR); } /* createCtrlFunctionDmItems() */ /****************************************************************************/ /* Function : consoleLampInit */ /* Purpose : Initialize VMI output card and sequence through lamp rows */ /* Inputs : None */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS consoleLampInit( Void ) { Int32 delay; if (vmiOutputBoardInit() != OK) /* Initialize VMI Digital Output Board */ return(ERROR); delay = sysClkRateGet() / 10; vmiOutputSet( 0x5fffffff, 0xffffffff ); taskDelay(sysClkRateGet() ); vmiOutputSet( 0x0fffdef7, 0xbdef7fff ); taskDelay(delay); vmiOutputSet( 0x0fff9ce7, 0x39ce777e ); taskDelay(delay); vmiOutputSet( 0x0fff18c6, 0x318c666c ); taskDelay(delay); vmiOutputSet( 0x0ffe1084, 0x21084448 ); taskDelay(delay); vmiOutputSet( 0x0ffc0000, 0x00000000 ); taskDelay(delay); vmiOutputAllCtrl( OFF ); return(OK); } /* consoleLampInit() */ /****************************************************************************/ /* Function : consolePowerLampCtrl */ /* Purpose : Set lamp state based on value of switch data manager item */ /* Inputs : Switch status data manager Item handle & switch channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS consolePowerLampCtrl( DM_Item switchStatusDm, Nat16 vmiSwitchChan ) { switchStatus switchState; /* Actual State of Subsystem Switch */ /* Read current switch status */ if (dm_read( switchStatusDm, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) { /* Check subsystem switch request */ if (switchState == SWITCH_ON) vmiOutputCtrl(vmiSwitchChan, ON); else vmiOutputCtrl(vmiSwitchChan, OFF); } /* if */ } /* consolePowerLampCtrl() */ /****************************************************************************/ /* Function : consoleMotorEnableLampCtrl */ /* Purpose : Set lamp state based on value of motor enable data mgr item*/ /* Inputs : Switch status data manager Item handle & switch channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS consoleMotorEnableLampCtrl( DM_Item enableStatusDm, Nat16 vmiSwitchChan ) { motorEnableState enableState; /* Motor Enable Status */ /* Read current enable switch status*/ if (dm_read( enableStatusDm, (char *) &enableState, sizeof(enableState), (DM_Time *) NULL) == SUCCESS) vmiOutputCtrl(vmiSwitchChan, (enableState == ENABLE_MOTOR ? ON : OFF)); } /* consoleMotorEnableLampCtrl() */ /****************************************************************************/ /* Function : consoleLampCtrl */ /* Purpose : Set lamp state based on value of boolean data manager item */ /* Inputs : Switch status data manager Item handle & switch channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS consoleLampCtrl( DM_Item switchStatusDm, Nat16 vmiSwitchChan ) { MBool switchState; /* Switch Status */ /* Read current switch status */ if (dm_read( switchStatusDm, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) vmiOutputCtrl(vmiSwitchChan, switchState); } /* consoleLampCtrl() */ /****************************************************************************/ /* Function : consoleLampFlashCtrl */ /* Purpose : Toggle lamp On/Off if state Dmgr Item is set to POWER_ERROR*/ /* Inputs : Switch status data manager Item handle & switch channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS consoleLampFlashCtrl( DM_Item switchStatusDm, Nat16 vmiSwitchChan, MBool state ) { switchStatus switchState; /* Switch Status */ /* Read current switch status */ if (dm_read( switchStatusDm, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) if (switchState == SWITCH_POWER_ERROR) vmiOutputCtrl(vmiSwitchChan, state); } /* consoleLampFlashCtrl() */ /****************************************************************************/ /* Function : motorEnableLampFlashCtrl */ /* Purpose : Toggle lamp On/Off if state Dmgr Item is set to MOOG_FAULT */ /* Inputs : Switch status data manager Item handle & switch channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS motorEnableLampFlashCtrl( DM_Item enableStatusDm, Nat16 vmiSwitchChan, MBool state ) { motorEnableState enableState; /* Motor Enable State */ /* Read current enable status */ if (dm_read( enableStatusDm, (char *) &enableState, sizeof(enableState), (DM_Time *) NULL) == SUCCESS) if (enableState == MOOG_MOTOR_FAULT) vmiOutputCtrl(vmiSwitchChan, state); } /* motorEnableLampFlashCtrl() */ /****************************************************************************/ /* Function : setPowerSwitchStateDm */ /* Purpose : Set Power switch REQUEST Data Manger item to desired state */ /* Inputs : Data manager Item handles & desired switch state */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS setPowerSwitchStateDm( DM_Item switchRequestDm, switchStatus switchRequest ) { DM_Time switchTime; /* Switch update time for Data Mgr */ /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (switchRequestDm, (char *) &switchRequest, sizeof(switchRequest), &switchTime ); } /* setPowerSwitchStateDm() */ /****************************************************************************/ /* Function : initPowerSwitchStatus */ /* Purpose : Set Power switch STATUS Data Manger item to SWITCH_OFF */ /* Inputs : Data manager Item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS initPowerSwitchStatus( DM_Item switchStatusDm ) { DM_Time switchTime; /* Switch update time for Data Mgr */ switchStatus switchOff = SWITCH_OFF; return (urgentWriteDmItem(switchStatusDm, &switchOff, sizeof(switchOff))); } /* initPowerSwitchStatus() */ /****************************************************************************/ /* Function : checkPowerSwitchState */ /* Purpose : Check Power Switch state and update Data Manger item */ /* Inputs : Switch channel, data manager Item handles & switch state */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkPowerSwitchState( Nat16 switchChan, DM_Item switchRequestDm, DM_Item switchStatusDm ) { STATUS switchState; /* Physical switch state */ switchStatus devSwitchRequest; /* Desired State of Subsystem Switch*/ switchStatus switchStatus; /* Actual State of Subsystem Switch */ if ( (switchState = vmiInputState( switchChan )) == ERROR) return(ERROR); if (switchState) { /* Read current switch status */ if (dm_read( switchStatusDm, (char *) &switchStatus, sizeof(switchStatus), (DM_Time *) NULL) == SUCCESS) { /* Check subsystem switch request */ devSwitchRequest = (switchStatus == SWITCH_OFF ? SWITCH_ON : SWITCH_OFF); /* Update data manager item value */ setPowerSwitchStateDm(switchRequestDm, devSwitchRequest); /* Wait for user to release switch */ while (switchState == vmiInputState( switchChan )) taskDelay(sysClkRateGet() / 20); } /* if */ } /* if */ return(OK); } /* checkPowerSwitchState() */ /****************************************************************************/ /* Function : setBoolSwitchStateDm */ /* Purpose : Set Boolean switch Data Manger item to desired state */ /* Inputs : Data manager Item handles & desired switch state */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS setBoolSwitchStateDm( DM_Item switchRequestDm, MBool state ) { DM_Time switchTime; /* Switch update time for Data Mgr */ /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (switchRequestDm, (char *) &state, sizeof(state), &switchTime ); } /* setBoolSwitchStateDm() */ /****************************************************************************/ /* Function : checkBoolSwitchState */ /* Purpose : Check Switch state and update Data Manger item if needed */ /* Inputs : Switch channel, data manager Item handles & switch state */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkBoolSwitchState( Nat16 switchChan, DM_Item switchRequestDm, MBool *lastState) { STATUS switchState; /* Physical switch state */ MBool desiredState; /* Desired switch Status */ if ( (switchState = vmiInputState( switchChan )) == ERROR) return(ERROR); if (switchState) { /* Desired state is compliment */ desiredState = (*lastState == ON ? OFF : ON); /* Update data manager item value */ setBoolSwitchStateDm( switchRequestDm, desiredState ); *lastState = desiredState; /* Save static state of switch */ /* Wait for user to release switch */ while (switchState == vmiInputState( switchChan )) taskDelay(sysClkRateGet() / 20); } /* if */ return(OK); } /* checkBoolSwitchState() */ /****************************************************************************/ /* Function : checkMasterMotorEnableSwitch */ /* Purpose : Check Master Motor Enable Switch state */ /* Inputs : Array of Data Manager Item handles & switch states */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkMasterMotorEnableSwitch( consoleSwitchDmItems *dmItems, MBool *lastState ) { STATUS switchState; /* Physical switch state */ motorEnableState enableRequest; /* Desired Enable Status */ MBool masterRequest; /* Desired Master Enable Status */ DM_Time switchTime; /* Switch update time for Data Mgr */ if ( (switchState = vmiInputState(THRUSTER_MASTER_ENABLE_SWITCH)) == ERROR) return(ERROR); if (switchState) { masterRequest = (*lastState == OFF ? ON : OFF); enableRequest = (*lastState == OFF ? ENABLE_MOTOR : DISABLE_MOTOR); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (dmItems->motorEnable, (char *) &masterRequest, sizeof(masterRequest), &switchTime ); dm_write (dmItems->stbdHorizEnable, (char *) &enableRequest, sizeof(enableRequest), &switchTime ); dm_write (dmItems->fwdLatEnable, (char *) &enableRequest, sizeof(enableRequest), &switchTime ); dm_write (dmItems->portHorizEnable, (char *) &enableRequest, sizeof(enableRequest), &switchTime ); dm_write (dmItems->aftLatEnable, (char *) &enableRequest, sizeof(enableRequest), &switchTime ); dm_write (dmItems->portVertEnable, (char *) &enableRequest, sizeof(enableRequest), &switchTime ); dm_write (dmItems->stbdVertEnable, (char *) &enableRequest, sizeof(enableRequest), &switchTime ); *lastState = enableRequest; /* Wait for user to release switch */ while (switchState == vmiInputState(THRUSTER_MASTER_ENABLE_SWITCH)) taskDelay(sysClkRateGet() / 20); } /* if */ return(OK); } /* checkMasterMotorEnableSwitch() */ /****************************************************************************/ /* Function : setMotorEnableSwitchStateDm */ /* Purpose : Set Motor Enable switch Data Manger item to desired state */ /* Inputs : Data manager Item handles & desired enable switch state */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS setMotorEnableSwitchStateDm( DM_Item enableRequestDm, motorEnableState state ) { DM_Time switchTime; /* Switch update time for Data Mgr */ /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (enableRequestDm, (char *) &state, sizeof(state), &switchTime ); } /* setMotorEnableSwitchStateDm() */ /****************************************************************************/ /* Function : checkMotorEnableSwitchState */ /* Purpose : Check Motor Enable Switch & update Data Manger item */ /* Inputs : Switch channel, data manager Item handles & switch state */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkMotorEnableSwitchState( Nat16 switchChan, DM_Item enableRequestDm ) { STATUS switchState; /* Physical switch state */ motorEnableState desiredState; /* Desired motor enable switch state*/ if ( (switchState = vmiInputState( switchChan )) == ERROR) return(ERROR); if (switchState) { /* Read current enable switch status*/ if (dm_read( enableRequestDm, (char *) &desiredState, sizeof(desiredState), (DM_Time *) NULL) == SUCCESS) { /* Check subsystem switch request */ /* Desired state is compliment */ desiredState = (desiredState == DISABLE_MOTOR ? ENABLE_MOTOR : DISABLE_MOTOR); /* Update data manager item value */ setMotorEnableSwitchStateDm( enableRequestDm, desiredState ); /* Wait for user to release switch */ while (switchState == vmiInputState( switchChan )) taskDelay(sysClkRateGet() / 20); } /* if */ } /* if */ return(OK); } /* checkMotorEnableSwitchState() */ /****************************************************************************/ /* Function : checkMasterMotorPowerSwitch */ /* Purpose : Check Master Motor Power Switch state */ /* Inputs : Array of Data Manager Item handles & switch states */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkMasterMotorPowerSwitch( consoleSwitchDmItems *dmItems, MBool *lastState ) { STATUS switchState; /* Physical switch state */ MBool masterSwitch; /* Desired Master Switch status */ switchStatus motorSwitch; /* Desired Motor Switch status */ DM_Time switchTime; /* Switch update time for Data Mgr */ if ( (switchState = vmiInputState(THRUSTER_MASTER_POWER_SWITCH)) == ERROR) return(ERROR); if (switchState) { masterSwitch = (*lastState == ON ? OFF : ON); motorSwitch = (*lastState == ON ? SWITCH_OFF : SWITCH_ON); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (dmItems->motorSwitch, (char *) &masterSwitch, sizeof(masterSwitch), &switchTime ); dm_write (dmItems->stbdHorizSwitch, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (dmItems->fwdLatSwitch, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (dmItems->portHorizSwitch, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (dmItems->aftLatSwitch, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (dmItems->portVertSwitch, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (dmItems->stbdVertSwitch, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); *lastState = masterSwitch; /* Wait for user to release switch */ while (switchState == vmiInputState(THRUSTER_MASTER_POWER_SWITCH)) taskDelay(sysClkRateGet() / 20); } /* if */ return(OK); } /* checkMasterMotorPowerSwitch() */ /****************************************************************************/ /* Function : checkAutoDepthAltitude */ /* Purpose : Check Mutually exclusive Auto Depth & Altitude Switches */ /* Inputs : Switch channels, Data Manager item handles & switch states */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkAutoDepthAltitude(MBool bellypackEnabled, Nat16 autoDepthSwitch, DM_Item autoDepthDm, MBool *autoDepthState, Nat16 autoAltitudeSwitch, DM_Item autoAltitudeDm, MBool *autoAltitudeState ) { STATUS switchState; /* Physical switch state */ DM_Time switchTime; /* Switch update time for Data Mgr */ if (!bellypackEnabled) /* Auto depth function is disabled */ { /* when the bellypack is enabled */ /* Check Auto Depth Switch State */ if ( (switchState = vmiInputState( autoDepthSwitch )) == ERROR) return(ERROR); if (switchState) /* If switch is depressed then */ { /* Toggle state of status variable */ *autoDepthState = (*autoDepthState == ON ? OFF : ON); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); /* Switches are mutually exclusive */ if ( (*autoDepthState == ON) && (*autoAltitudeState == ON) ) { *autoAltitudeState = OFF; /* Cannot have both on at once */ dm_write (autoAltitudeDm, (char *) autoAltitudeState, sizeof(*autoAltitudeState), &switchTime ); } /* if */ dm_write (autoDepthDm, (char *) autoDepthState, sizeof(*autoDepthState), &switchTime ); /* Wait for user to release switch */ while (switchState == vmiInputState( autoDepthSwitch )) taskDelay(sysClkRateGet() / 20); } /* if */ } /* if (!bellypackEnabled) */ /* Check Auto Autitude Switch State */ if ( (switchState = vmiInputState( autoAltitudeSwitch )) == ERROR) return(ERROR); if (switchState) /* If switch is depressed then */ { if (bellypackEnabled) /* Bellypack disables Auto Altitude */ { if (*autoAltitudeState == ON) /* Pilot can disable Auto Altitude*/ { /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); *autoAltitudeState = OFF; /* Disable Auto Altitude */ dm_write (autoAltitudeDm, (char *) autoAltitudeState, sizeof(*autoAltitudeState), &switchTime ); } /* if */ } /* if */ else { /* Toggle state of status variable */ *autoAltitudeState = (*autoAltitudeState == ON ? OFF : ON); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); /* Switches are mutually exclusive */ if ( (*autoAltitudeState == ON) && (*autoDepthState == ON) ) { *autoDepthState = OFF; /* Cannot have both on at once */ dm_write (autoDepthDm, (char *) autoDepthState, sizeof(*autoDepthState), &switchTime ); } /* if */ dm_write (autoAltitudeDm, (char *) autoAltitudeState, sizeof(*autoAltitudeState), &switchTime ); } /* if */ /* Wait for user to release switch */ while (switchState == vmiInputState( autoAltitudeSwitch )) taskDelay(sysClkRateGet() / 20); } /* if */ return(OK); } /* checkAutoDepthAltitude() */ /****************************************************************************/ /* Function : updateJoystickGain */ /* Purpose : Update joystick gain DM item. */ /* Inputs : New joystick gain value */ /* Outputs : None, but data manager items change */ /****************************************************************************/ LOCAL STATUS updateJoystickGainDm( Nat16 joystickGain ) { DM_Item gainDm; /* Joystick gain data manager item */ if ((gainDm = initMicroDmItem("", JOYSTICK_GAIN_DM, DM_NAT16, 1)) == ERROR) { /* Error occurred so print message & exit */ logMsg("Console Out Task: Error Initializing Joystick Gain DM Item\n"); return(ERROR); } /* if */ return (urgentWriteDmItem(gainDm, &joystickGain, sizeof(joystickGain))); } /* updateJoystickGainDm() */ /****************************************************************************/ /* Function : checkBellypackEnableState */ /* Purpose : Check Switch state and update Data Manger item if needed */ /* Inputs : Switch channel, data manager Item handles & switch state */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkBellypackEnableState( Nat16 switchChan, DM_Item switchRequestDm, DM_Item switchStatusDm, MBool *state ) { STATUS switchState; /* Physical switch state */ MBool desiredState; /* Desired switch Status */ if ( (switchState = vmiInputState( switchChan )) == ERROR) return(ERROR); if (switchState) { if (dm_read( switchStatusDm, (char *) &desiredState, sizeof(desiredState), (DM_Time *) NULL) != SUCCESS) desiredState = ON; else /* Desired state is compliment */ desiredState = (desiredState == ON ? OFF : ON); /* Update data manager item value */ setBoolSwitchStateDm( switchRequestDm, desiredState ); *state = desiredState; /* Save static state of switch */ /* Wait for user to release switch */ while (switchState == vmiInputState( switchChan )) taskDelay(sysClkRateGet() / 20); } /* if */ return(OK); } /* checkBellypackEnableState() */ MLocal Void processBellypackEnable(consoleSwitchDmItems *consoleSwitch, ctrlFunctionDmItems *console, ctrlFunctionDmItems *bellypack, ctrlFunctionDmItems *controlSystem) { MBool switchState; /* Switch Status */ /* Read current switch status */ if (dm_read( consoleSwitch->bellypackEnable, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) { dm_disconnect(controlSystem->joystickX); dm_disconnect(controlSystem->joystickY); dm_disconnect(controlSystem->joystickZ); dm_disconnect(controlSystem->joystickYaw); dm_disconnect(controlSystem->autoDepth); dm_disconnect(controlSystem->autoHeading); if (switchState) { /* Bellypack enabled, console disabled */ setBoolSwitchStateDm(consoleSwitch->joystickLock, FALSE); setBoolSwitchStateDm(consoleSwitch->joystickRange, FALSE); dm_connect(bellypack->joystickX, controlSystem->joystickX); dm_connect(bellypack->joystickY, controlSystem->joystickY); dm_connect(bellypack->joystickZ, controlSystem->joystickZ); dm_connect(bellypack->joystickYaw, controlSystem->joystickYaw); dm_connect(bellypack->autoDepth, controlSystem->autoDepth); dm_connect(bellypack->autoHeading, controlSystem->autoHeading); } /* if */ else { /* Bellypack disabled, console enabled */ dm_connect(console->joystickX, controlSystem->joystickX); dm_connect(console->joystickY, controlSystem->joystickY); dm_connect(console->joystickZ, controlSystem->joystickZ); dm_connect(console->joystickYaw, controlSystem->joystickYaw); dm_connect(console->autoDepth, controlSystem->autoDepth); dm_connect(console->autoHeading, controlSystem->autoHeading); } /* else */ } /* if */ } /* processBellypackEnable() */ /****************************************************************************/ /* Function : consoleOutTask */ /* Purpose : Pilot console software switch lamp output module. */ /* Inputs : None */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS consoleOutTask( Void ) { Int32 item; /* Counter for DM Item Initialization */ Int16 ballast, light; /* ballast & light identifiers */ DM_Item powerIbcGfDisable; /* Enable Power IBC Ground Fault Monitor*/ DM_Group dmGroup1; /* Console Data Manager Group */ DM_Group dmGroup2; /* Console Data Manager Group */ SEM_ID dmUpdateSem; /* Data Manager Item Change wakeup sem */ DWord consoleGroup1Bits; /* Holds dm_group_changes bit vector */ DWord consoleGroup2Bits; /* Holds Power Switch changed bit vector*/ DWord motorSwitchBit; /* Master Motor Switch changed bit */ DWord portHorizSwitchBit; /* Port Horiz Thruster Switch change bit*/ DWord stbdHorizSwitchBit; /* Stbd Horiz Thruster Switch change bit*/ DWord fwdLatSwitchBit; /* Fwd Lateral Thruster Switch change bit*/ DWord aftLatSwitchBit; /* Aft Lateral Thruster Switch change bit*/ DWord portVertSwitchBit; /* Port Vert Thruster Switch change bit */ DWord stbdVertSwitchBit; /* Stbd Vert Thruster Switch change bit */ DWord motorEnableBit; /* Master Motor Enable changed bit */ DWord portHorizEnableBit; /* Port Horiz Thruster Enable change bit*/ DWord stbdHorizEnableBit; /* Stbd Horiz Thruster Enable change bit*/ DWord fwdLatEnableBit; /* Fwd Lateral Thruster Enable change bit*/ DWord aftLatEnableBit; /* Aft Lateral Thruster Enable change bit*/ DWord portVertEnableBit; /* Port Vert Thruster Enable change bit */ DWord stbdVertEnableBit; /* Stbd Vert Thruster Enable change bit */ DWord cameraDconSwitchBit; DWord sensorDconSwitchBit; DWord sonarDconSwitchBit; DWord ersDconSwitchBit; DWord manip48VSwitchBit; DWord manip240VSwitchBit; DWord toolsled48VSwitchBit; DWord toolsled240VSwitchBit; DWord lightingDcon48VSwitchBit; DWord lightingDcon240VSwitchBit; DWord ballastSwitchBit[NUM_LIGHT_BALLASTS]; DWord lightSwitchBit[NUM_LIGHTS]; DWord portPanTiltSwitchBit; /* Port Pan & Tilt Switch changed bit */ DWord stbdPanTiltSwitchBit; /* Stbd Pan & Tilt Switch changed bit */ DWord backupLightsSwitchBit; /* backup Light Switch changed bit */ DWord vbSystemSwitchBit; /* VB System Power Switch changed bit */ DWord gyroSwitchBit; /* Gyro Switch Request changed bit */ DWord responderSwitchBit; /* Responder Switch Request changed bit */ DWord imagSonarSwitchBit; /* Sonar Switch Request changed bit */ DWord hydrophoneSwitchBit; /* Hydrophone Switch Request changed bit*/ DWord autoDepthBit; /* Auto Depth Switch changed bit */ DWord autoAltitudeBit; /* Auto Autitude Switch changed bit */ DWord autoHeadingBit; /* Auto Heading Switch changed bit */ DWord autoPositionBit; /* Auto Position Switch changed bit */ DWord joystickLockBit; /* Joystick Lock Switch changed bit */ DWord joystickRangeBit; /* Joystick Range Switch changed bit */ DWord bellypackEnableBit; /* Bellypack Enable Switch changed bit */ consoleSwitchDmItems dmItems; /* structure containing dm item handles */ ctrlFunctionDmItems consoleDm; /* Console Control Function DM Items */ ctrlFunctionDmItems bellypackDm;/* Bellypack Control Function DM Items */ ctrlFunctionDmItems controlDm; /* Control System Control Function Items*/ dmItemList consoleDmInit[] = { { &dmItems.motorSwitch, "Master Motor Switch status", "CONSOLE.MASTER_MOTOR_SWITCH", DM_MBOOL, 1 }, { &dmItems.portHorizSwitch, "Port Horizontal Motor Switch status", THRUSTER_PORT_HORIZ_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.stbdHorizSwitch, "Stbd Horizontal Motor Switch status", THRUSTER_STBD_HORIZ_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.fwdLatSwitch, "Fwd Lateral Motor Switch status", THRUSTER_FWD_LAT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.aftLatSwitch, "Aft Lateral Motor Switch status", THRUSTER_AFT_LAT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.portVertSwitch, "Port Vertical Motor Switch status", THRUSTER_PORT_VERT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.stbdVertSwitch, "Stbd Vertical Motor Switch status", THRUSTER_STBD_VERT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.motorEnable, "Master Motor Enable status", "CONSOLE.MASTER_MOTOR_ENABLE", DM_MBOOL, 1 }, { &dmItems.portHorizEnable, "Port Horizontal Motor Enable status", "THRUSTER.PORT_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmItems.stbdHorizEnable, "Stbd Horizontal Motor Enable status", "THRUSTER.STBD_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmItems.fwdLatEnable, "Fwd Lateral Motor Enable status", "THRUSTER.FWD_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmItems.aftLatEnable, "Aft Lateral Motor Enable status", "THRUSTER.AFT_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmItems.portVertEnable, "Port Vertical Motor Enable status", "THRUSTER.PORT_VERT.ENABLE", DM_ENUM, 1 }, { &dmItems.stbdVertEnable, "Stbd Vertical Motor Enable status", "THRUSTER.STBD_VERT.ENABLE", DM_ENUM, 1 }, { &dmItems.cameraDconSwitch, "Camera DCon Switch Status", CAMERA_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.sensorDconSwitch, "Sensor DCon Switch Status", SENSOR_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.sonarDconSwitch, "Sonar DCon Switch Status", SONAR_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ersDconSwitch, "Emergency Recovery DCon Switch Status", ERS_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightingDcon48VSwitch, "Lighting DCon 48V Switch Status", LIGHTING_DCON_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightingDcon240VSwitch, "Lighting DCon 240V Switch Status", LIGHTING_DCON_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_0], "Ballast 0 Switch Status", LIGHT_BALLAST_0_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_1], "Ballast 1 Switch Status", LIGHT_BALLAST_1_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_2], "Ballast 2 Switch Status", LIGHT_BALLAST_2_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_1], "Light 1 Switch Status", LIGHT_0_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_2], "Light 2 Switch Status", LIGHT_1_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_3], "Light 3 Switch Status", LIGHT_2_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_4], "Light 4 Switch Status", LIGHT_3_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_5], "Light 5 Switch Status", LIGHT_4_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_6], "Light 6 Switch Status", LIGHT_5_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.portPanTiltSwitch, "Port Pan & Tilts Switch Status", PORT_PAN_TILT_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.stbdPanTiltSwitch, "Stbd Pan & Tilts Switch Status", STBD_PAN_TILT_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.backupLightsSwitch, "Backup Lights Switch Status", BACKUP_LIGHT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.vbSystemSwitch, "VB Motor Switch Status", VB_MOTOR_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.manip48VSwitch, "Manipulator 48V Switch Status", MANIP_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.manip240VSwitch, "Manipulator 240V Switch Status", MANIP_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.toolsled48VSwitch, "Toolsled 48V Switch Status", TOOLSLED_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.toolsled240VSwitch, "Toolsled 240V Switch Status", TOOLSLED_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.gyroSwitch, "Gyro switch status", GYRO_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.responderSwitch, "Responder switch status", RESPONDER_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.imagSonarSwitch, "Imaging Sonar switch status", IMAG_SONAR_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.hydrophoneSwitch, "Hydrophone switch status", HYDROPHONE_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.bellypackEnable, "Bellypack Enable Switch", BELLY_PACK_DM_PREFIX BELLY_PACK_ENABLED_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; dmItemList controlDmInit[] = { { &dmItems.autoDepth, "Auto Depth Switch", AUTO_DEPTH_DM, DM_MBOOL, 1 }, { &dmItems.autoAltitude, "Auto Altitude Switch", AUTO_ALTITUDE_DM, DM_MBOOL, 1 }, { &dmItems.autoHeading, "Auto Heading Switch", AUTO_HEADING_DM, DM_MBOOL, 1 }, { &dmItems.autoPosition, "Auto Position Switch", AUTO_POSITION_DM, DM_MBOOL, 1 }, { &dmItems.joystickLock, "Joystick Lock Switch", CONSOLE_DM_PREFIX CONSOLE_JOYSTICK_LOCK_DM, DM_MBOOL, 1 }, { &dmItems.joystickRange, "Joystick Range Switch", JOYSTICK_GAIN_SWITCH_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; consoleLampInit(); /* Initialize Console Lamps */ /* Create switch wakeup semaphore */ if ( (dmUpdateSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Console Out Task: Error initializing Wakeup Semaphore\n"); return; } /* if */ /* Clear list of Data Manager Item handles*/ bzero((char *) &dmItems, sizeof(dmItems)); /* Initialize Data Manager Items from table*/ for (item = 0; consoleDmInit[item].dmItem != NO_ITEM; item++) { if ((*consoleDmInit[item].dmItem = initMicroDmItem( ((strstr(consoleDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), consoleDmInit[item].name, consoleDmInit[item].type, consoleDmInit[item].len)) == ERROR) { /* Error occurred so print message & exit */ logMsg("Console Out Task: Error Initializing %s DM Item\n", consoleDmInit[item].errorMsg); return(ERROR); } /* if */ else /* Declare this task a consumer of switch*/ /* status data manager items */ dm_start_consumer(*consoleDmInit[item].dmItem, DM_STATIC, dmUpdateSem); } /* for */ /* Initialize Data Manager Items from table*/ for (item = 0; controlDmInit[item].dmItem != NO_ITEM; item++) { if ((*controlDmInit[item].dmItem = initMicroDmItem( ((strstr(controlDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), controlDmInit[item].name, controlDmInit[item].type, controlDmInit[item].len)) == ERROR) { /* Error occurred so print message & exit */ logMsg("Console Out Task: Error Initializing %s DM Item\n", controlDmInit[item].errorMsg); return(ERROR); } /* if */ else /* Declare this task a consumer of switch*/ /* status data manager items */ dm_start_consumer(*controlDmInit[item].dmItem, DM_STATIC, dmUpdateSem); } /* for */ /* Enable Power Can GF Monitoring each */ /* time ROV Console is re-booted */ if ((powerIbcGfDisable = initMicroDmItem(PWR_IBC_A_DM_PREFIX, GF_MONITOR_DISABLE_DM, DM_MBOOL, 1)) != ERROR) { dm_start_provider (powerIbcGfDisable, DM_STATIC); writeBooleanDmItem(powerIbcGfDisable, TRUE); } /* if */ if ((powerIbcGfDisable = initMicroDmItem(PWR_IBC_B_DM_PREFIX, GF_MONITOR_DISABLE_DM, DM_MBOOL, 1)) != ERROR) { dm_start_provider (powerIbcGfDisable, DM_STATIC); writeBooleanDmItem(powerIbcGfDisable, TRUE); } /* if */ createCtrlFunctionDmItems(CONSOLE_DM_PREFIX, &consoleDm); createCtrlFunctionDmItems(BELLY_PACK_DM_PREFIX, &bellypackDm); createCtrlFunctionDmItems("TIBURON.CONTROL.", &controlDm); initPowerSwitchStatus(dmItems.portHorizSwitch); initPowerSwitchStatus(dmItems.stbdHorizSwitch); initPowerSwitchStatus(dmItems.fwdLatSwitch); initPowerSwitchStatus(dmItems.aftLatSwitch); initPowerSwitchStatus(dmItems.portVertSwitch); initPowerSwitchStatus(dmItems.stbdVertSwitch); initPowerSwitchStatus(dmItems.cameraDconSwitch); initPowerSwitchStatus(dmItems.sensorDconSwitch); initPowerSwitchStatus(dmItems.sonarDconSwitch); initPowerSwitchStatus(dmItems.ersDconSwitch); initPowerSwitchStatus(dmItems.manip48VSwitch); initPowerSwitchStatus(dmItems.manip240VSwitch); initPowerSwitchStatus(dmItems.toolsled48VSwitch); initPowerSwitchStatus(dmItems.toolsled240VSwitch); initPowerSwitchStatus(dmItems.lightingDcon48VSwitch); initPowerSwitchStatus(dmItems.lightingDcon240VSwitch); for (ballast = BALLAST_0; ballast <= BALLAST_2; ballast++) initPowerSwitchStatus(dmItems.ballastSwitch[ballast]); for (light = LIGHT_1; light <= LIGHT_6; light++) initPowerSwitchStatus(dmItems.lightSwitch[light]); initPowerSwitchStatus(dmItems.portPanTiltSwitch); initPowerSwitchStatus(dmItems.stbdPanTiltSwitch); initPowerSwitchStatus(dmItems.backupLightsSwitch); initPowerSwitchStatus(dmItems.vbSystemSwitch); initPowerSwitchStatus(dmItems.gyroSwitch); initPowerSwitchStatus(dmItems.responderSwitch); initPowerSwitchStatus(dmItems.imagSonarSwitch); initPowerSwitchStatus(dmItems.hydrophoneSwitch); /* Switch On Panel Light for pilot */ vmiOutputCtrl(CONSOLE_ACTIVE_LAMP, ON); /* Initialize device switch lamps */ consoleLampCtrl(dmItems.motorSwitch, THRUSTER_MASTER_POWER_SWITCH); consolePowerLampCtrl(dmItems.portHorizSwitch, PORT_HORIZ_POWER_SWITCH ); consolePowerLampCtrl(dmItems.stbdHorizSwitch, STBD_HORIZ_POWER_SWITCH ); consolePowerLampCtrl(dmItems.fwdLatSwitch, FWD_LAT_POWER_SWITCH ); consolePowerLampCtrl(dmItems.aftLatSwitch, AFT_LAT_POWER_SWITCH ); consolePowerLampCtrl(dmItems.portVertSwitch, PORT_VERT_POWER_SWITCH ); consolePowerLampCtrl(dmItems.stbdVertSwitch, STBD_VERT_POWER_SWITCH ); consoleLampCtrl( dmItems.motorEnable, THRUSTER_MASTER_ENABLE_SWITCH ); consoleMotorEnableLampCtrl( dmItems.portHorizEnable, PORT_HORIZ_ENABLE_SWITCH ); consoleMotorEnableLampCtrl( dmItems.stbdHorizEnable, STBD_HORIZ_ENABLE_SWITCH ); consoleMotorEnableLampCtrl( dmItems.fwdLatEnable, FWD_LAT_ENABLE_SWITCH ); consoleMotorEnableLampCtrl( dmItems.aftLatEnable, AFT_LAT_ENABLE_SWITCH ); consoleMotorEnableLampCtrl( dmItems.portVertEnable, PORT_VERT_ENABLE_SWITCH ); consoleMotorEnableLampCtrl( dmItems.stbdVertEnable, STBD_VERT_ENABLE_SWITCH ); consolePowerLampCtrl(dmItems.cameraDconSwitch, CAMERA_DCON_POWER_SWITCH ); consolePowerLampCtrl(dmItems.sensorDconSwitch, SENSOR_DCON_POWER_SWITCH ); consolePowerLampCtrl(dmItems.sonarDconSwitch, SONAR_DCON_POWER_SWITCH ); consolePowerLampCtrl(dmItems.ersDconSwitch, EMERGENCY_DCON_POWER_SWITCH); consolePowerLampCtrl(dmItems.manip48VSwitch, MANIP_48V_POWER_SWITCH); consolePowerLampCtrl(dmItems.manip240VSwitch, MANIP_240V_POWER_SWITCH); consolePowerLampCtrl(dmItems.toolsled48VSwitch, TOOLSLED_48V_POWER_SWITCH); consolePowerLampCtrl(dmItems.toolsled240VSwitch, TOOLSLED_240V_POWER_SWITCH); consolePowerLampCtrl(dmItems.lightingDcon48VSwitch, LIGHTING_DCON_48V_POWER_SWITCH); consolePowerLampCtrl(dmItems.lightingDcon240VSwitch, LIGHTING_DCON_240V_POWER_SWITCH); for (ballast = BALLAST_0; ballast <= BALLAST_2; ballast++) consolePowerLampCtrl(dmItems.ballastSwitch[ballast], ballastSwitchAssignments[ballast]); for (light = LIGHT_1; light <= LIGHT_6; light++) consolePowerLampCtrl(dmItems.lightSwitch[light], lightSwitchAssignments[light]); consolePowerLampCtrl(dmItems.portPanTiltSwitch, PORT_PAN_TILT_DCON_POWER_SWITCH); consolePowerLampCtrl(dmItems.stbdPanTiltSwitch, STBD_PAN_TILT_DCON_POWER_SWITCH); consolePowerLampCtrl(dmItems.backupLightsSwitch, BACKUP_LIGHT_POWER_SWITCH); consolePowerLampCtrl(dmItems.vbSystemSwitch, VB_MOTOR_POWER_SWITCH); consolePowerLampCtrl(dmItems.gyroSwitch, GYRO_POWER_SWITCH ); consolePowerLampCtrl(dmItems.responderSwitch, RESPONDER_POWER_SWITCH); consolePowerLampCtrl(dmItems.imagSonarSwitch, IMAG_SONAR_POWER_SWITCH); consolePowerLampCtrl(dmItems.hydrophoneSwitch, HYDROPHONE_POWER_SWITCH ); consoleLampCtrl( dmItems.autoDepth, AUTO_DEPTH_SWITCH ); consoleLampCtrl( dmItems.autoAltitude, AUTO_ALTITUDE_SWITCH ); consoleLampCtrl( dmItems.autoHeading, AUTO_HEADING_SWITCH ); consoleLampCtrl( dmItems.autoPosition, AUTO_POSITION_SWITCH ); consoleLampCtrl( dmItems.joystickLock, JOYSTICK_LOCK_SWITCH ); consoleLampCtrl( dmItems.joystickRange, JOYSTICK_RANGE_SWITCH ); consoleLampCtrl( dmItems.bellypackEnable, BELLY_PACK_ENABLE_SWITCH); dmGroup1 = dm_create_group(); /* Create Data Manager Group for items */ dmGroup2 = dm_create_group(); /* Create Data Manager Group for items */ dm_group_add_item(dmGroup1, dmItems.motorSwitch, &motorSwitchBit); dm_group_add_item(dmGroup1, dmItems.portHorizSwitch, &portHorizSwitchBit); dm_group_add_item(dmGroup1, dmItems.stbdHorizSwitch, &stbdHorizSwitchBit); dm_group_add_item(dmGroup1, dmItems.fwdLatSwitch, &fwdLatSwitchBit); dm_group_add_item(dmGroup1, dmItems.aftLatSwitch, &aftLatSwitchBit); dm_group_add_item(dmGroup1, dmItems.portVertSwitch, &portVertSwitchBit); dm_group_add_item(dmGroup1, dmItems.stbdVertSwitch, &stbdVertSwitchBit); dm_group_add_item(dmGroup1, dmItems.motorEnable, &motorEnableBit); dm_group_add_item(dmGroup1, dmItems.portHorizEnable, &portHorizEnableBit); dm_group_add_item(dmGroup1, dmItems.stbdHorizEnable, &stbdHorizEnableBit); dm_group_add_item(dmGroup1, dmItems.fwdLatEnable, &fwdLatEnableBit); dm_group_add_item(dmGroup1, dmItems.aftLatEnable, &aftLatEnableBit); dm_group_add_item(dmGroup1, dmItems.portVertEnable, &portVertEnableBit); dm_group_add_item(dmGroup1, dmItems.stbdVertEnable, &stbdVertEnableBit); dm_group_add_item(dmGroup1, dmItems.cameraDconSwitch, &cameraDconSwitchBit); dm_group_add_item(dmGroup1, dmItems.sensorDconSwitch, &sensorDconSwitchBit); dm_group_add_item(dmGroup1, dmItems.sonarDconSwitch, &sonarDconSwitchBit); dm_group_add_item(dmGroup1, dmItems.ersDconSwitch, &ersDconSwitchBit); dm_group_add_item(dmGroup1, dmItems.manip48VSwitch, &manip48VSwitchBit); dm_group_add_item(dmGroup1, dmItems.manip240VSwitch, &manip240VSwitchBit); dm_group_add_item(dmGroup1, dmItems.toolsled48VSwitch, &toolsled48VSwitchBit); dm_group_add_item(dmGroup1, dmItems.toolsled240VSwitch, &toolsled240VSwitchBit); dm_group_add_item(dmGroup1, dmItems.lightingDcon48VSwitch, &lightingDcon48VSwitchBit); dm_group_add_item(dmGroup1, dmItems.lightingDcon240VSwitch, &lightingDcon240VSwitchBit); for (ballast = BALLAST_0; ballast <= BALLAST_2; ballast++) dm_group_add_item(dmGroup2, dmItems.lightingDcon240VSwitch, &ballastSwitchBit[ballast]); for (light = LIGHT_1; light <= LIGHT_6; light++) dm_group_add_item(dmGroup2, dmItems.lightingDcon240VSwitch, &lightSwitchBit[light]); dm_group_add_item(dmGroup2, dmItems.portPanTiltSwitch, &portPanTiltSwitchBit); dm_group_add_item(dmGroup2, dmItems.stbdPanTiltSwitch, &stbdPanTiltSwitchBit); dm_group_add_item(dmGroup2, dmItems.backupLightsSwitch, &backupLightsSwitchBit); dm_group_add_item(dmGroup2, dmItems.vbSystemSwitch, &vbSystemSwitchBit); dm_group_add_item(dmGroup2, dmItems.gyroSwitch, &gyroSwitchBit); dm_group_add_item(dmGroup2, dmItems.imagSonarSwitch, &imagSonarSwitchBit); dm_group_add_item(dmGroup2, dmItems.responderSwitch, &responderSwitchBit); dm_group_add_item(dmGroup2, dmItems.hydrophoneSwitch, &hydrophoneSwitchBit); dm_group_add_item(dmGroup2, dmItems.autoDepth, &autoDepthBit); dm_group_add_item(dmGroup2, dmItems.autoAltitude, &autoAltitudeBit); dm_group_add_item(dmGroup2, dmItems.autoHeading, &autoHeadingBit); dm_group_add_item(dmGroup2, dmItems.autoPosition, &autoPositionBit); dm_group_add_item(dmGroup2, dmItems.joystickLock, &joystickLockBit); dm_group_add_item(dmGroup2, dmItems.joystickRange, &joystickRangeBit); dm_group_add_item(dmGroup2, dmItems.bellypackEnable, &bellypackEnableBit); /* Connect console joysticks to control */ processBellypackEnable(&dmItems, &consoleDm, &bellypackDm, &controlDm); FOREVER /* Repeat forever */ { /* Wait for any data items to change */ semTake(dmUpdateSem, WAIT_FOREVER); /* Ask Data Manager which items changed */ consoleGroup1Bits = dm_get_group_changes(dmGroup1); consoleGroup2Bits = dm_get_group_changes(dmGroup2); /* Master Motor Power Switch Changed */ if (consoleGroup1Bits & motorSwitchBit) consoleLampCtrl(dmItems.motorSwitch, THRUSTER_MASTER_POWER_SWITCH); /* Starbord Horizontal Motor Switch */ if (consoleGroup1Bits & stbdHorizSwitchBit) consolePowerLampCtrl(dmItems.stbdHorizSwitch, STBD_HORIZ_POWER_SWITCH); /* Forward Lateral Motor Switch */ if (consoleGroup1Bits & fwdLatSwitchBit) consolePowerLampCtrl(dmItems.fwdLatSwitch, FWD_LAT_POWER_SWITCH ); /* Port Horizontal Motor Switch */ if (consoleGroup1Bits & portHorizSwitchBit) consolePowerLampCtrl(dmItems.portHorizSwitch, PORT_HORIZ_POWER_SWITCH); /* Aft Lateral Motor Switch */ if (consoleGroup1Bits & aftLatSwitchBit) consolePowerLampCtrl(dmItems.aftLatSwitch, AFT_LAT_POWER_SWITCH ); /* Port Vertical Motor Switch */ if (consoleGroup1Bits & portVertSwitchBit) consolePowerLampCtrl(dmItems.portVertSwitch, PORT_VERT_POWER_SWITCH ); /* Starbord Vertical Motor Switch */ if (consoleGroup1Bits & stbdVertSwitchBit) consolePowerLampCtrl(dmItems.stbdVertSwitch, STBD_VERT_POWER_SWITCH ); /* Master Motor Enable Switch Changed */ if (consoleGroup1Bits & motorEnableBit) consoleLampCtrl(dmItems.motorEnable, THRUSTER_MASTER_ENABLE_SWITCH); /* Port Horizontal Motor Enable */ if (consoleGroup1Bits & portHorizEnableBit) consoleMotorEnableLampCtrl(dmItems.portHorizEnable, PORT_HORIZ_ENABLE_SWITCH); /* Starbord Horizontal Motor Enable */ if (consoleGroup1Bits & stbdHorizEnableBit) consoleMotorEnableLampCtrl(dmItems.stbdHorizEnable, STBD_HORIZ_ENABLE_SWITCH); /* Forward Lateral Motor Enable */ if (consoleGroup1Bits & fwdLatEnableBit) consoleMotorEnableLampCtrl(dmItems.fwdLatEnable, FWD_LAT_ENABLE_SWITCH ); /* Aft Lateral Motor Enable */ if (consoleGroup1Bits & aftLatEnableBit) consoleMotorEnableLampCtrl(dmItems.aftLatEnable, AFT_LAT_ENABLE_SWITCH ); /* Port Vertical Motor Enable */ if (consoleGroup1Bits & portVertEnableBit) consoleMotorEnableLampCtrl(dmItems.portVertEnable, PORT_VERT_ENABLE_SWITCH ); /* Starbord Vertical Motor Enable */ if (consoleGroup1Bits & stbdVertEnableBit) consoleMotorEnableLampCtrl(dmItems.stbdVertEnable, STBD_VERT_ENABLE_SWITCH ); if (consoleGroup1Bits & cameraDconSwitchBit) consolePowerLampCtrl(dmItems.cameraDconSwitch, CAMERA_DCON_POWER_SWITCH); if (consoleGroup1Bits & sensorDconSwitchBit) consolePowerLampCtrl(dmItems.sensorDconSwitch, SENSOR_DCON_POWER_SWITCH); if (consoleGroup1Bits & sonarDconSwitchBit) consolePowerLampCtrl(dmItems.sonarDconSwitch, SONAR_DCON_POWER_SWITCH); if (consoleGroup1Bits & ersDconSwitchBit) consolePowerLampCtrl(dmItems.ersDconSwitch, EMERGENCY_DCON_POWER_SWITCH); if (consoleGroup1Bits & manip48VSwitchBit) consolePowerLampCtrl(dmItems.manip48VSwitch, MANIP_48V_POWER_SWITCH); if (consoleGroup1Bits & manip240VSwitchBit) consolePowerLampCtrl(dmItems.manip240VSwitch, MANIP_240V_POWER_SWITCH); if (consoleGroup1Bits & toolsled48VSwitchBit) consolePowerLampCtrl(dmItems.toolsled48VSwitch, TOOLSLED_48V_POWER_SWITCH); if (consoleGroup1Bits & toolsled240VSwitchBit) consolePowerLampCtrl(dmItems.toolsled240VSwitch, TOOLSLED_240V_POWER_SWITCH); if (consoleGroup1Bits & lightingDcon48VSwitchBit) consolePowerLampCtrl(dmItems.lightingDcon48VSwitch, LIGHTING_DCON_48V_POWER_SWITCH); if (consoleGroup1Bits & lightingDcon240VSwitchBit) consolePowerLampCtrl(dmItems.lightingDcon240VSwitch, LIGHTING_DCON_240V_POWER_SWITCH); for (light = LIGHT_1; light <= LIGHT_6; light++) if (consoleGroup2Bits & lightSwitchBit[light]) consolePowerLampCtrl(dmItems.lightSwitch[light], lightSwitchAssignments[light]); for (ballast = BALLAST_0; ballast <= BALLAST_2; ballast++) if (consoleGroup2Bits & ballastSwitchBit[ballast]) consolePowerLampCtrl(dmItems.ballastSwitch[ballast], ballastSwitchAssignments[ballast]); if (consoleGroup2Bits & portPanTiltSwitchBit) consolePowerLampCtrl(dmItems.portPanTiltSwitch, PORT_PAN_TILT_DCON_POWER_SWITCH); if (consoleGroup2Bits & stbdPanTiltSwitchBit) consolePowerLampCtrl(dmItems.stbdPanTiltSwitch, STBD_PAN_TILT_DCON_POWER_SWITCH); if (consoleGroup2Bits & backupLightsSwitchBit) consolePowerLampCtrl(dmItems.backupLightsSwitch, BACKUP_LIGHT_POWER_SWITCH); if (consoleGroup2Bits & vbSystemSwitchBit) consolePowerLampCtrl(dmItems.vbSystemSwitch, VB_MOTOR_POWER_SWITCH); /* Gyro Power Switch Status Changed */ if (consoleGroup2Bits & gyroSwitchBit) consolePowerLampCtrl( dmItems.gyroSwitch, GYRO_POWER_SWITCH ); /* Sonar Power Switch Status Changed */ if (consoleGroup2Bits & imagSonarSwitchBit) consolePowerLampCtrl( dmItems.imagSonarSwitch, IMAG_SONAR_POWER_SWITCH ); /* Responder Power Switch Status Changed*/ if (consoleGroup2Bits & responderSwitchBit) consolePowerLampCtrl( dmItems.responderSwitch, RESPONDER_POWER_SWITCH ); /* Hydrophone Power Switch Status Changed*/ if (consoleGroup2Bits & hydrophoneSwitchBit) consolePowerLampCtrl(dmItems.hydrophoneSwitch, HYDROPHONE_POWER_SWITCH); if (consoleGroup2Bits & autoDepthBit) consoleLampCtrl(dmItems.autoDepth, AUTO_DEPTH_SWITCH ); if (consoleGroup2Bits & autoAltitudeBit) consoleLampCtrl(dmItems.autoAltitude, AUTO_ALTITUDE_SWITCH ); if (consoleGroup2Bits & autoHeadingBit) consoleLampCtrl(dmItems.autoHeading, AUTO_HEADING_SWITCH ); if (consoleGroup2Bits & autoPositionBit) consoleLampCtrl(dmItems.autoPosition, AUTO_POSITION_SWITCH ); if (consoleGroup2Bits & joystickLockBit) consoleLampCtrl(dmItems.joystickLock, JOYSTICK_LOCK_SWITCH ); if (consoleGroup2Bits & joystickRangeBit) consoleLampCtrl(dmItems.joystickRange, JOYSTICK_RANGE_SWITCH); if (consoleGroup2Bits & bellypackEnableBit) { consoleLampCtrl(dmItems.bellypackEnable, BELLY_PACK_ENABLE_SWITCH); processBellypackEnable(&dmItems, &consoleDm, &bellypackDm, &controlDm); } /* if */ } /* FOREVER */ /* 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 */ } /* consoleOutTask() */ /****************************************************************************/ /* Function : consoleFlashTask */ /* Purpose : Pilot console software switch lamp flasher task */ /* Inputs : None */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS consoleFlashTask( Void ) { SEM_ID wakeupSem; /* Used by watch dog to wake up task */ Int32 item; /* Counter for DM Item Initialization */ MBool toggle; /* Present lamp state */ Int16 ballast, light; /* ballast & light identifiers */ consoleSwitchDmItems dmItems; /* structure containing dm item handles */ dmItemList consoleDmInit[] = { { &dmItems.portHorizSwitch, "Port Horizontal Motor Switch status", THRUSTER_PORT_HORIZ_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.stbdHorizSwitch, "Stbd Horizontal Motor Switch status", THRUSTER_STBD_HORIZ_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.fwdLatSwitch, "Fwd Lateral Motor Switch status", THRUSTER_FWD_LAT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.aftLatSwitch, "Aft Lateral Motor Switch status", THRUSTER_AFT_LAT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.portVertSwitch, "Port Vertical Motor Switch status", THRUSTER_PORT_VERT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.stbdVertSwitch, "Stbd Vertical Motor Switch status", THRUSTER_STBD_VERT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.portHorizEnable, "Port Horizontal Motor Enable status", "THRUSTER.PORT_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmItems.stbdHorizEnable, "Stbd Horizontal Motor Enable status", "THRUSTER.STBD_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmItems.fwdLatEnable, "Fwd Lateral Motor Enable status", "THRUSTER.FWD_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmItems.aftLatEnable, "Aft Lateral Motor Enable status", "THRUSTER.AFT_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmItems.portVertEnable, "Port Vertical Motor Enable status", "THRUSTER.PORT_VERT.ENABLE", DM_ENUM, 1 }, { &dmItems.stbdVertEnable, "Stbd Vertical Motor Enable status", "THRUSTER.STBD_VERT.ENABLE", DM_ENUM, 1 }, { &dmItems.cameraDconSwitch, "Camera DCon Switch Status", CAMERA_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.sensorDconSwitch, "Sensor DCon Switch Status", SENSOR_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.sonarDconSwitch, "Sonar DCon Switch Status", SONAR_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ersDconSwitch, "Emergency Recovery DCon Switch Status", ERS_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightingDcon48VSwitch, "Lighting DCon 48V Switch Status", LIGHTING_DCON_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightingDcon240VSwitch, "Lighting DCon 240V Switch Status", LIGHTING_DCON_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_0], "Ballast 0 Switch Status", LIGHT_BALLAST_0_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_1], "Ballast 1 Switch Status", LIGHT_BALLAST_1_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_2], "Ballast 2 Switch Status", LIGHT_BALLAST_2_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_1], "Light 1 Switch Status", LIGHT_0_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_2], "Light 2 Switch Status", LIGHT_1_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_3], "Light 3 Switch Status", LIGHT_2_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_4], "Light 4 Switch Status", LIGHT_3_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_5], "Light 5 Switch Status", LIGHT_4_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_6], "Light 6 Switch Status", LIGHT_5_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.portPanTiltSwitch, "Port Pan & Tilts Switch Status", PORT_PAN_TILT_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.stbdPanTiltSwitch, "Stbd Pan & Tilts Switch Status", STBD_PAN_TILT_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.backupLightsSwitch, "Backup Lights Switch Status", BACKUP_LIGHT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.vbSystemSwitch, "VB Motor Switch Status", VB_MOTOR_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.manip48VSwitch, "Manipulator 48V Switch Status", MANIP_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.manip240VSwitch, "Manipulator 240V Switch Status", MANIP_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.toolsled48VSwitch, "Toolsled 48V Switch Status", TOOLSLED_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.toolsled240VSwitch, "Toolsled 240V Switch Status", TOOLSLED_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.gyroSwitch, "Gyro switch status", GYRO_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.responderSwitch, "Responder switch status", RESPONDER_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.imagSonarSwitch, "Imaging Sonar switch status", IMAG_SONAR_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmItems.hydrophoneSwitch, "Hydrophone switch status", HYDROPHONE_SWITCH_STATUS_DM, DM_ENUM, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Create wakeup semaphore and watch dog */ /* timer for lamp flash rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Console Flash Task: Error initializing task resources\n"); return; } /* if */ /* Clear list of Data Manager Item handles*/ bzero((char*) &dmItems, sizeof(dmItems)); /* Initialize Data Manager Items from table*/ for (item = 0; consoleDmInit[item].dmItem != NO_ITEM; item++) { if ((*consoleDmInit[item].dmItem = initMicroDmItem( ((strstr(consoleDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), consoleDmInit[item].name, consoleDmInit[item].type, consoleDmInit[item].len)) == ERROR) { /* Error occurred so print message & exit */ logMsg("Console Flash Task: Error Initializing %s DM Item\n", consoleDmInit[item].errorMsg); return(ERROR); } /* if */ else /* Declare this task a consumer of switch*/ /* status data manager items */ dm_start_consumer(*consoleDmInit[item].dmItem, DM_STATIC, SEM_NULL); } /* for */ #if 0 /* Initialize Data Manager Items from table*/ if (initMicroDmItemList(consoleDmInit, "", "Console Lamp Flash Task") == ERROR) return(ERROR); /* Declare this task a consumer of switch*/ /* status data manager items */ for (item = 0; consoleDmInit[item].dmItem != NO_ITEM; item++) dm_start_consumer(*consoleDmInit[item].dmItem, DM_STATIC, SEM_NULL); #endif FOREVER { consoleLampFlashCtrl(dmItems.stbdHorizSwitch, STBD_HORIZ_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.fwdLatSwitch, FWD_LAT_POWER_SWITCH , toggle); consoleLampFlashCtrl(dmItems.portHorizSwitch, PORT_HORIZ_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.aftLatSwitch, AFT_LAT_POWER_SWITCH , toggle); consoleLampFlashCtrl(dmItems.portVertSwitch, PORT_VERT_POWER_SWITCH , toggle); consoleLampFlashCtrl(dmItems.stbdVertSwitch, STBD_VERT_POWER_SWITCH , toggle); motorEnableLampFlashCtrl(dmItems.stbdHorizEnable, STBD_HORIZ_ENABLE_SWITCH, toggle); motorEnableLampFlashCtrl(dmItems.fwdLatEnable, FWD_LAT_ENABLE_SWITCH , toggle); motorEnableLampFlashCtrl(dmItems.portHorizEnable, PORT_HORIZ_ENABLE_SWITCH, toggle); motorEnableLampFlashCtrl(dmItems.aftLatEnable, AFT_LAT_ENABLE_SWITCH , toggle); motorEnableLampFlashCtrl(dmItems.portVertEnable, PORT_VERT_ENABLE_SWITCH , toggle); motorEnableLampFlashCtrl(dmItems.stbdVertEnable, STBD_VERT_ENABLE_SWITCH , toggle); consoleLampFlashCtrl(dmItems.cameraDconSwitch, CAMERA_DCON_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.sensorDconSwitch, SENSOR_DCON_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.sonarDconSwitch, SONAR_DCON_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.ersDconSwitch, EMERGENCY_DCON_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.manip48VSwitch, MANIP_48V_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.manip240VSwitch, MANIP_240V_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.toolsled48VSwitch, TOOLSLED_48V_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.toolsled240VSwitch, TOOLSLED_240V_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.lightingDcon48VSwitch, LIGHTING_DCON_48V_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.lightingDcon240VSwitch, LIGHTING_DCON_240V_POWER_SWITCH, toggle); for (ballast = BALLAST_0; ballast <= BALLAST_2; ballast++) consoleLampFlashCtrl(dmItems.ballastSwitch[ballast], ballastSwitchAssignments[ballast], toggle); for (light = LIGHT_1; light <= LIGHT_6; light++) consoleLampFlashCtrl(dmItems.lightSwitch[light], lightSwitchAssignments[light], toggle); consoleLampFlashCtrl(dmItems.portPanTiltSwitch, PORT_PAN_TILT_DCON_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.stbdPanTiltSwitch, STBD_PAN_TILT_DCON_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.backupLightsSwitch, BACKUP_LIGHT_POWER_SWITCH, toggle); consoleLampFlashCtrl(dmItems.vbSystemSwitch, VB_MOTOR_POWER_SWITCH, toggle); consoleLampFlashCtrl( dmItems.gyroSwitch, GYRO_POWER_SWITCH, toggle ); consoleLampFlashCtrl( dmItems.responderSwitch, RESPONDER_POWER_SWITCH , toggle); consoleLampFlashCtrl( dmItems.imagSonarSwitch, IMAG_SONAR_POWER_SWITCH , toggle); consoleLampFlashCtrl(dmItems.hydrophoneSwitch, HYDROPHONE_POWER_SWITCH, toggle); /* Change desired state of Flashing Lamps */ toggle = (toggle == ON ? OFF : ON); /* Pend until sem timeout wakes us up */ semTake(wakeupSem, sysClkRateGet() / (USECS_PER_SEC / LAMP_FLASH_PERIOD)); } /* FOREVER */ /* 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 */ } /* consoleFlashTask() */ /*****************************************************************************/ /* Function : consoleInTask */ /* Purpose : Console Input Task. Wakes up periodically, reads data from */ /* the console switches and write Data Manager Items */ /* Inputs : None */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ STATUS consoleInTask( Void ) { SEM_ID wakeupSem; /* Used by watch dog to wake up this task */ Int32 item; /* Counter for DM Item Initialization */ Int16 ballast, light; /* ballast & light identifiers */ MBool autoDepth = OFF; MBool autoAltitude = OFF; MBool autoHeading = OFF; MBool autoPosition = OFF; MBool joystickLock = OFF; MBool joystickRange = OFF; MBool masterMotorSwitch = OFF; MBool masterMotorEnable = OFF; MBool bellypackEnable = OFF; consoleSwitchDmItems dmItems; /* structure of dm request handles */ consoleSwitchDmItems dmStatus; /* structure of switch status handles */ dmItemList consoleDmInit[] = { { &dmItems.gyroSwitch, "Gyro switch", GYRO_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.responderSwitch, "Responder switch", RESPONDER_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.imagSonarSwitch, "Imaging Sonar switch", IMAG_SONAR_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.hydrophoneSwitch, "Hydrophone switch", HYDROPHONE_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.joystickLock, "Joystick Lock Switch", CONSOLE_DM_PREFIX CONSOLE_JOYSTICK_LOCK_DM, DM_MBOOL, 1 }, { &dmItems.motorSwitch, "Master Thruster Power Switch", "CONSOLE.MASTER_MOTOR_SWITCH", DM_MBOOL, 1 }, { &dmItems.portHorizSwitch, "Port Horizontal Motor Switch", THRUSTER_PORT_HORIZ_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.stbdHorizSwitch, "Stbd Horizontal Motor Switch", THRUSTER_STBD_HORIZ_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.fwdLatSwitch, "Fwd Lateral Motor Switch", THRUSTER_FWD_LAT_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.aftLatSwitch, "Aft Lateral Motor Switch", THRUSTER_AFT_LAT_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.portVertSwitch, "Port Vertical Motor Switch", THRUSTER_PORT_VERT_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.stbdVertSwitch, "Stbd Vertical Motor Switch", THRUSTER_STBD_VERT_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.motorEnable, "Master Thruster enable", "CONSOLE.MASTER_MOTOR_ENABLE", DM_MBOOL, 1 }, { &dmItems.portHorizEnable, "Port Horizontal Motor Enable", "THRUSTER.PORT_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmItems.stbdHorizEnable, "Stbd Horizontal Motor Enable", "THRUSTER.STBD_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmItems.fwdLatEnable, "Fwd Lateral Motor Enable", "THRUSTER.FWD_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmItems.aftLatEnable, "Aft Lateral Motor Enable", "THRUSTER.AFT_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmItems.portVertEnable, "Port Vertical Motor Enable", "THRUSTER.PORT_VERT.ENABLE", DM_ENUM, 1 }, { &dmItems.stbdVertEnable, "Stbd Vertical Motor Enable", "THRUSTER.STBD_VERT.ENABLE", DM_ENUM, 1 }, { &dmItems.cameraDconSwitch, "Camera DCon Switch", CAMERA_DCON_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.sensorDconSwitch, "Sensor DCon Switch", SENSOR_DCON_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.sonarDconSwitch, "Sonar DCon Switch", SONAR_DCON_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.ersDconSwitch, "Emergency Recovery DCon Switch", ERS_DCON_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightingDcon48VSwitch, "Lighting DCon 48V Switch", LIGHTING_DCON_48V_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightingDcon240VSwitch, "Lighting DCon 240V Switch", LIGHTING_DCON_240V_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_0], "Ballast 0 Switch Status", LIGHT_BALLAST_0_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_1], "Ballast 1 Switch Status", LIGHT_BALLAST_1_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.ballastSwitch[BALLAST_2], "Ballast 2 Switch Status", LIGHT_BALLAST_2_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_1], "Light 1 Switch Status", LIGHT_0_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_2], "Light 2 Switch Status", LIGHT_1_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_3], "Light 3 Switch Status", LIGHT_2_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_4], "Light 4 Switch Status", LIGHT_3_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_5], "Light 5 Switch Status", LIGHT_4_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.lightSwitch[LIGHT_6], "Light 6 Switch Status", LIGHT_5_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.portPanTiltSwitch, "Port Pan & Tilts Switch", PORT_PAN_TILT_DCON_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.stbdPanTiltSwitch, "Stbd Pan & Tilts Switch", STBD_PAN_TILT_DCON_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.backupLightsSwitch, "Backup Lights Switch", BACKUP_LIGHT_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.vbSystemSwitch, "VB Motor Switch", VB_MOTOR_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.manip48VSwitch, "Manipulator 48V Switch", MANIP_48V_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.manip240VSwitch, "Manipulator 240V Switch", MANIP_240V_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.toolsled48VSwitch, "Toolsled 48V Switch", TOOLSLED_48V_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.toolsled240VSwitch, "Toolsled 240V Switch", TOOLSLED_240V_SWITCH_REQ_DM, DM_ENUM, 1 }, { &dmItems.bellypackEnable, "Bellypack Enable Switch", BELLY_PACK_DM_PREFIX BELLY_PACK_ENABLE_REQ_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; dmItemList controlDmInit[] = { { &dmItems.autoDepth, "Auto Depth Switch", CONSOLE_DM_PREFIX CONSOLE_AUTO_DEPTH_DM, DM_MBOOL, 1 }, { &dmItems.autoAltitude, "Auto Altitude Switch", AUTO_ALTITUDE_DM, DM_MBOOL, 1 }, { &dmItems.autoHeading, "Auto Heading Switch", CONSOLE_DM_PREFIX CONSOLE_AUTO_HEADING_DM, DM_MBOOL, 1 }, { &dmItems.autoPosition, "Auto Position Switch", AUTO_POSITION_DM, DM_MBOOL, 1 }, { &dmItems.joystickRange, "Joystick Range Switch", JOYSTICK_GAIN_SWITCH_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; dmItemList statusDmInit[] = { { &dmStatus.gyroSwitch, "Gyro switch status", GYRO_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.responderSwitch, "Responder switch status", RESPONDER_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.imagSonarSwitch, "Imaging Sonar switch status", IMAG_SONAR_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.hydrophoneSwitch, "Hydrophone switch status", HYDROPHONE_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.motorSwitch, "Master Motor Switch status", "CONSOLE.MASTER_MOTOR_SWITCH", DM_MBOOL, 1 }, { &dmStatus.portHorizSwitch, "Port Horiz Motor Switch status", THRUSTER_PORT_HORIZ_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.stbdHorizSwitch, "Stbd Horiz Motor Switch status", THRUSTER_STBD_HORIZ_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.fwdLatSwitch, "Fwd Lateral Motor Switch status", THRUSTER_FWD_LAT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.aftLatSwitch, "Aft Lateral Motor Switch status", THRUSTER_AFT_LAT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.portVertSwitch, "Port Vertical Motor Switch status", THRUSTER_PORT_VERT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.stbdVertSwitch, "Stbd Vertical Motor Switch status", THRUSTER_STBD_VERT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.portHorizEnable, "Port Horizontal Motor Enable status", "THRUSTER.PORT_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmStatus.stbdHorizEnable, "Stbd Horizontal Motor Enable status", "THRUSTER.STBD_HORIZ.ENABLE", DM_ENUM, 1 }, { &dmStatus.fwdLatEnable, "Fwd Lateral Motor Enable status", "THRUSTER.FWD_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmStatus.aftLatEnable, "Aft Lateral Motor Enable status", "THRUSTER.AFT_LATERAL.ENABLE", DM_ENUM, 1 }, { &dmStatus.portVertEnable, "Port Vertical Motor Enable status", "THRUSTER.PORT_VERT.ENABLE", DM_ENUM, 1 }, { &dmStatus.stbdVertEnable, "Stbd Vertical Motor Enable status", "THRUSTER.STBD_VERT.ENABLE", DM_ENUM, 1 }, { &dmStatus.cameraDconSwitch, "Camera DCon Switch Status", CAMERA_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.sensorDconSwitch, "Sensor DCon Switch Status", SENSOR_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.sonarDconSwitch, "Sonar DCon Switch Status", SONAR_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.ersDconSwitch, "Emergency Recovery DCon Switch Status", ERS_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightingDcon48VSwitch, "Lighting DCon 48V Switch Status", LIGHTING_DCON_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightingDcon240VSwitch, "Lighting DCon 240V Switch Status", LIGHTING_DCON_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.ballastSwitch[BALLAST_0], "Ballast 0 Switch Status", LIGHT_BALLAST_0_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.ballastSwitch[BALLAST_1], "Ballast 1 Switch Status", LIGHT_BALLAST_1_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.ballastSwitch[BALLAST_2], "Ballast 2 Switch Status", LIGHT_BALLAST_2_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightSwitch[LIGHT_1], "Light 1 Switch Status", LIGHT_0_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightSwitch[LIGHT_2], "Light 2 Switch Status", LIGHT_1_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightSwitch[LIGHT_3], "Light 3 Switch Status", LIGHT_2_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightSwitch[LIGHT_4], "Light 4 Switch Status", LIGHT_3_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightSwitch[LIGHT_5], "Light 5 Switch Status", LIGHT_4_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.lightSwitch[LIGHT_6], "Light 6 Switch Status", LIGHT_5_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.portPanTiltSwitch, "Port Pan & Tilts Switch Status", PORT_PAN_TILT_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.stbdPanTiltSwitch, "Stbd Pan & Tilts Switch Status", STBD_PAN_TILT_DCON_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.backupLightsSwitch, "Backup Lights Switch Status", BACKUP_LIGHT_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.vbSystemSwitch, "VB Motor Switch Status", VB_MOTOR_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.manip48VSwitch, "Manipulator 48V Switch Status", MANIP_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.manip240VSwitch, "Manipulator 240V Switch Status", MANIP_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.toolsled48VSwitch, "Toolsled 48V Switch Status", TOOLSLED_48V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.toolsled240VSwitch, "Toolsled 240V Switch Status", TOOLSLED_240V_SWITCH_STATUS_DM, DM_ENUM, 1 }, { &dmStatus.bellypackEnable, "Bellypack Enable Status", BELLY_PACK_DM_PREFIX BELLY_PACK_ENABLED_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; vmiInputBoardInit(); /* Initialize VMI digital input board */ /* Clear list of Data Manager Item handles*/ bzero((char *) &dmItems, sizeof(dmItems)); bzero((char *) &dmStatus, sizeof(dmStatus)); #if 0 /* Initialize Data Manager Items from table*/ if (initMicroDmItemList(consoleDmInit, ((strstr(consoleDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), "Console In Task") == ERROR) return (ERROR); /* Initialize Data Manager Items from table*/ if (initMicroDmItemList(statusDmInit, ((strstr(statusDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), "Console In Task") == ERROR) return (ERROR); /* Initialize Data Manager Items from table*/ if (initMicroDmItemList(controlDmInit, ((strstr(controlDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), "Console In Task") == ERROR) return (ERROR); /* Declare this task a provider of switch*/ /* request data manager items */ for (item = 0; consoleDmInit[item].dmItem != NO_ITEM; item++) dm_start_multiple( *consoleDmInit[item].dmItem ); /* Declare this task a consumer of switch*/ /* status data manager items */ for (item = 0; statusDmInit[item].dmItem != NO_ITEM; item++) dm_start_consumer( *statusDmInit[item].dmItem, DM_STATIC, SEM_NULL); /* Declare this task a provider of switch*/ /* request data manager items */ for (item = 0; controlDmInit[item].dmItem != NO_ITEM; item++) dm_start_multiple( *controlDmInit[item].dmItem ); #endif /* Initialize Data Manager Items from table*/ for (item = 0; consoleDmInit[item].dmItem != NO_ITEM; item++) { if ((*consoleDmInit[item].dmItem = initMicroDmItem( ((strstr(consoleDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), consoleDmInit[item].name, consoleDmInit[item].type, consoleDmInit[item].len)) == ERROR) { /* Error occurred so print message & exit */ logMsg("Console In Task: Error Initializing %s DM Item\n", consoleDmInit[item].errorMsg); return(ERROR); } /* if */ else /* Declare this task a provider of switch*/ /* request data manager items */ dm_start_multiple( *consoleDmInit[item].dmItem ); } /* for */ bzero((char *) &dmStatus, sizeof(dmStatus)); /* Initialize Data Manager Items from table*/ for (item = 0; statusDmInit[item].dmItem != NO_ITEM; item++) { if ((*statusDmInit[item].dmItem = initMicroDmItem( ((strstr(statusDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), statusDmInit[item].name, statusDmInit[item].type, statusDmInit[item].len)) == ERROR) { /* Error occurred so print message & exit */ logMsg("Console In Task: Error Initializing %s DM Item\n", statusDmInit[item].errorMsg); return(ERROR); } /* if */ else /* Declare this task a consumer of switch*/ /* status data manager items */ dm_start_consumer( *statusDmInit[item].dmItem, DM_STATIC, SEM_NULL); } /* for */ /* Initialize Data Manager Items from table*/ for (item = 0; controlDmInit[item].dmItem != NO_ITEM; item++) { if ((*controlDmInit[item].dmItem = initMicroDmItem( ((strstr(controlDmInit[item].name, TIBURON_DM_PREFIX) == NULL) ? TIBURON_DM_PREFIX : ""), controlDmInit[item].name, controlDmInit[item].type, controlDmInit[item].len)) == ERROR) { /* Error occurred so print message & exit */ logMsg("Console Out Task: Error Initializing %s DM Item\n", controlDmInit[item].errorMsg); return(ERROR); } /* if */ else /* Declare this task a provider of switch*/ /* request data manager items */ dm_start_multiple( *controlDmInit[item].dmItem ); } /* for */ /* Create wakeup semaphore */ /* for provider rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Console In Task: Error initializing Resources\n"); return; } /* if */ setBoolSwitchStateDm ( dmItems.motorSwitch, masterMotorSwitch ); setPowerSwitchStateDm( dmItems.portHorizSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.stbdHorizSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.fwdLatSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.aftLatSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.portVertSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.stbdVertSwitch, SWITCH_OFF ); setBoolSwitchStateDm( dmItems.motorEnable, masterMotorEnable ); setMotorEnableSwitchStateDm( dmItems.portHorizEnable, DISABLE_MOTOR ); setMotorEnableSwitchStateDm( dmItems.stbdHorizEnable, DISABLE_MOTOR ); setMotorEnableSwitchStateDm( dmItems.fwdLatEnable, DISABLE_MOTOR ); setMotorEnableSwitchStateDm( dmItems.aftLatEnable, DISABLE_MOTOR ); setMotorEnableSwitchStateDm( dmItems.portVertEnable, DISABLE_MOTOR ); setMotorEnableSwitchStateDm( dmItems.stbdVertEnable, DISABLE_MOTOR ); setPowerSwitchStateDm( dmItems.cameraDconSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.sensorDconSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.sonarDconSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.ersDconSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.manip48VSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.manip240VSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.toolsled48VSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.toolsled240VSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.lightingDcon48VSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.lightingDcon240VSwitch, SWITCH_OFF ); for (ballast = BALLAST_0; ballast <= BALLAST_2; ballast++) setPowerSwitchStateDm( dmItems.ballastSwitch[ballast], SWITCH_OFF ); for (light = LIGHT_1; light <= LIGHT_6; light++) setPowerSwitchStateDm( dmItems.lightSwitch[light], SWITCH_OFF ); setPowerSwitchStateDm( dmItems.portPanTiltSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.stbdPanTiltSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.backupLightsSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.vbSystemSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.gyroSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.responderSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.imagSonarSwitch, SWITCH_OFF ); setPowerSwitchStateDm( dmItems.hydrophoneSwitch, SWITCH_OFF ); setBoolSwitchStateDm( dmItems.autoDepth, autoDepth ); setBoolSwitchStateDm( dmItems.autoAltitude, autoAltitude ); setBoolSwitchStateDm( dmItems.autoHeading, autoHeading ); setBoolSwitchStateDm( dmItems.autoPosition, autoPosition ); setBoolSwitchStateDm( dmItems.joystickLock, joystickLock ); setBoolSwitchStateDm( dmItems.joystickRange, joystickRange ); setBoolSwitchStateDm( dmItems.bellypackEnable, bellypackEnable); /* Set Default Joystick Gain Value */ updateJoystickGainDm(DEFAULT_JOYSTICK_GAIN); FOREVER /* Update Rate is not guaranteed due to */ { /* the possibility of task preemption */ /* Check motor master power switch state */ checkMasterMotorPowerSwitch( &dmItems, &masterMotorSwitch ); /* Check port Horiz motor power switch */ checkPowerSwitchState( PORT_HORIZ_POWER_SWITCH, dmItems.portHorizSwitch, dmStatus.portHorizSwitch ); /* Check stbd Horiz motor power switch */ checkPowerSwitchState( STBD_HORIZ_POWER_SWITCH, dmItems.stbdHorizSwitch, dmStatus.stbdHorizSwitch ); /* Check Fwd Lateral motor power switch */ checkPowerSwitchState( FWD_LAT_POWER_SWITCH, dmItems.fwdLatSwitch, dmStatus.fwdLatSwitch ); /* Check Aft Lateral motor power switch */ checkPowerSwitchState( AFT_LAT_POWER_SWITCH, dmItems.aftLatSwitch, dmStatus.aftLatSwitch ); /* Check Port Vert motor power switch */ checkPowerSwitchState( PORT_VERT_POWER_SWITCH, dmItems.portVertSwitch, dmStatus.portVertSwitch ); /* Check Stbd Vert motor power switch */ checkPowerSwitchState( STBD_VERT_POWER_SWITCH, dmItems.stbdVertSwitch, dmStatus.stbdVertSwitch ); /* Check motor master enable switch state*/ checkMasterMotorEnableSwitch( &dmItems, &masterMotorEnable ); /* Check port Horiz motor enable switch */ checkMotorEnableSwitchState( PORT_HORIZ_ENABLE_SWITCH, dmItems.portHorizEnable ); /* Check stbd Horiz motor enable switch */ checkMotorEnableSwitchState( STBD_HORIZ_ENABLE_SWITCH, dmItems.stbdHorizEnable ); /* Check Fwd Lateral motor enable switch */ checkMotorEnableSwitchState( FWD_LAT_ENABLE_SWITCH, dmItems.fwdLatEnable ); /* Check Aft Lateral motor enable switch */ checkMotorEnableSwitchState( AFT_LAT_ENABLE_SWITCH, dmItems.aftLatEnable ); /* Check Port Vert motor enable switch */ checkMotorEnableSwitchState( PORT_VERT_ENABLE_SWITCH, dmItems.portVertEnable ); /* Check Stbd Vert motor enable switch */ checkMotorEnableSwitchState( STBD_VERT_ENABLE_SWITCH, dmItems.stbdVertEnable ); /* Check for Camera Dcon switch state */ checkPowerSwitchState( CAMERA_DCON_POWER_SWITCH, dmItems.cameraDconSwitch, dmStatus.cameraDconSwitch ); /* Check for Sensor Dcon switch state */ checkPowerSwitchState( SENSOR_DCON_POWER_SWITCH, dmItems.sensorDconSwitch, dmStatus.sensorDconSwitch ); /* Check for Sonar Dcon switch state */ checkPowerSwitchState( SONAR_DCON_POWER_SWITCH, dmItems.sonarDconSwitch, dmStatus.sonarDconSwitch ); /* Check for ERS Dcon switch state */ checkPowerSwitchState( EMERGENCY_DCON_POWER_SWITCH, dmItems.ersDconSwitch, dmStatus.ersDconSwitch ); checkPowerSwitchState( LIGHTING_DCON_48V_POWER_SWITCH, dmItems.lightingDcon48VSwitch, dmStatus.lightingDcon48VSwitch ); checkPowerSwitchState( LIGHTING_DCON_240V_POWER_SWITCH, dmItems.lightingDcon240VSwitch, dmStatus.lightingDcon240VSwitch ); for (ballast = BALLAST_0; ballast <= BALLAST_2; ballast++) checkPowerSwitchState( ballastSwitchAssignments[ballast], dmItems.ballastSwitch[ballast], dmStatus.ballastSwitch[ballast] ); for (light = LIGHT_1; light <= LIGHT_6; light++) checkPowerSwitchState( lightSwitchAssignments[light], dmItems.lightSwitch[light], dmStatus.lightSwitch[light] ); checkPowerSwitchState( MANIP_48V_POWER_SWITCH, dmItems.manip48VSwitch, dmStatus.manip48VSwitch ); checkPowerSwitchState( MANIP_240V_POWER_SWITCH, dmItems.manip240VSwitch, dmStatus.manip240VSwitch ); checkPowerSwitchState( TOOLSLED_48V_POWER_SWITCH, dmItems.toolsled48VSwitch, dmStatus.toolsled48VSwitch ); checkPowerSwitchState( TOOLSLED_240V_POWER_SWITCH, dmItems.toolsled240VSwitch, dmStatus.toolsled240VSwitch ); checkPowerSwitchState( PORT_PAN_TILT_DCON_POWER_SWITCH, dmItems.portPanTiltSwitch, dmStatus.portPanTiltSwitch ); checkPowerSwitchState( STBD_PAN_TILT_DCON_POWER_SWITCH, dmItems.stbdPanTiltSwitch, dmStatus.stbdPanTiltSwitch ); checkPowerSwitchState( BACKUP_LIGHT_POWER_SWITCH, dmItems.backupLightsSwitch, dmStatus.backupLightsSwitch ); checkPowerSwitchState( VB_MOTOR_POWER_SWITCH, dmItems.vbSystemSwitch, dmStatus.vbSystemSwitch ); /* Check for Imaging sonar switch state */ checkPowerSwitchState( IMAG_SONAR_POWER_SWITCH, dmItems.imagSonarSwitch, dmStatus.imagSonarSwitch ); /* Check for gyro switch state */ checkPowerSwitchState( GYRO_POWER_SWITCH, dmItems.gyroSwitch, dmStatus.gyroSwitch ); /* Check for responder switch state */ checkPowerSwitchState( RESPONDER_POWER_SWITCH, dmItems.responderSwitch, dmStatus.responderSwitch ); /* Check for hydrophone switch state */ checkPowerSwitchState( HYDROPHONE_POWER_SWITCH, dmItems.hydrophoneSwitch, dmStatus.hydrophoneSwitch ); /* Check Auto Depth & Altitude switches */ checkAutoDepthAltitude(bellypackEnable, AUTO_DEPTH_SWITCH, dmItems.autoDepth, &autoDepth, AUTO_ALTITUDE_SWITCH, dmItems.autoAltitude, &autoAltitude ); /* Check Auto Heading switch */ if (!bellypackEnable) /* Bellypack has priority over console */ checkBoolSwitchState( AUTO_HEADING_SWITCH, dmItems.autoHeading, &autoHeading ); /* Check Auto Position switch */ checkBoolSwitchState( AUTO_POSITION_SWITCH, dmItems.autoPosition, &autoPosition ); /* Check Joystick Lock switch state */ checkBoolSwitchState( JOYSTICK_LOCK_SWITCH, dmItems.joystickLock, &joystickLock ); /* Check Joystick Range switch state */ checkBoolSwitchState( JOYSTICK_RANGE_SWITCH, dmItems.joystickRange, &joystickRange ); /* Check Bellypack Enable switch state */ checkBellypackEnableState( BELLY_PACK_ENABLE_SWITCH, dmItems.bellypackEnable, dmStatus.bellypackEnable, &bellypackEnable ); /* Pend until sem timeout wakes us up */ semTake(wakeupSem, (sysClkRateGet() / (USECS_PER_SEC / SWITCH_DM_PERIOD))); } /* FOREVER */ /* 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 */ } /* consoleInTask() */ /*****************************************************************************/ /* Function : signedMin */ /* Purpose : Test signed value against a limit & return signed minimum */ /* Inputs : Value and minumum value */ /* Outputs : Returns minumum of value and limit */ /*****************************************************************************/ LOCAL Int16 signedMin( Int16 value, Int16 minimum ) { if ( value > minimum) value = minimum; else if ( value < -minimum) value = -minimum; return(value); } /* signedMin() */ /*****************************************************************************/ /* Function : scaleJoystick */ /* Purpose : Scale raw joystick values into required range & add deadband*/ /* Inputs : Raw Joystick value, zero offset, desired full scale value, */ /* observed fullscale +ve and -ve values and deadband range */ /* Outputs : Returns scaled, adjusted joystick value */ /*****************************************************************************/ LOCAL Int16 scaleJoystick( Int16 joystickRaw, Int16 offset, Flt32 fullScaleActual, Flt32 fullScalePosObs, Flt32 fullScaleNegObs, Flt32 deadband ) { Flt32 joystickScaled; /* Adjusted joystick value to return */ joystickRaw -= offset; /* Subtract measured zero offset */ if (joystickRaw >= 0) /* Check quadrant of raw value */ { /* Scale raw value into desired range*/ joystickScaled = joystickRaw * fullScaleActual / fullScalePosObs; /* Limit scaled value to max limit */ if (joystickScaled > fullScaleActual) joystickScaled = fullScaleActual; /* Apply deadband range to joystick */ if (joystickScaled < deadband) joystickScaled = 0.0; } /* if */ else { /* Scale raw value into desired range*/ joystickScaled = joystickRaw * fullScaleActual / -fullScaleNegObs; /* Limit scaled value to max limit */ if (joystickScaled < - fullScaleActual) joystickScaled = - fullScaleActual; /* Apply deadband range to joystick */ if (joystickScaled > -deadband) joystickScaled = 0.0; } /* else */ /* Return scaled, adjusted value */ return( (Int16) joystickScaled ); } /* scaleJoystick() */ /*****************************************************************************/ /* Function : joystickInTask */ /* Purpose : Reads console joysticks and updates Data Manager Items */ /* Inputs : None */ /* Outputs : None, but Data Manager items change */ /*****************************************************************************/ STATUS joystickInTask( Void ) { ctrlFunctionDmItems dmItems; /* Console joystick DM item handles */ panTiltDmItems panTiltDm; /* Pan/Tilt joystick DM item handles */ DM_Item joystickLockDm; /* Joystick lock DM Item */ DM_Time joystickTime; /* Switch update time for Data Mgr */ SEM_ID wakeupSem; /* Used by watch dog to wake up task */ Int16 joystickRaw[12] = {0}; /* Raw joystick readings from A/D board */ Int16 joystickScaled[12] = {0}; /* Scalled & dead banded joysitck values */ Int16 lastJs[12] = {0}; /* Last set of scaled joystick readings */ MBool joystickLock; /* Main Joystick Lock Mode Enabled */ Int16 joystickZ; /* Joystick Z value for dm_write */ Int16 setPointZ; /* Joystick Z offset set point value */ MBool waitForZeroReturn; /* Set when waiting for joystick zero */ Int32 item; Nat16 axis; /* Pan/Tilt axis */ Flt64 squaredJs; /* Joystick value squared */ dmItemList joystickDmInit[] = /* Data Manager Item Table for table */ { { &dmItems.joystickX, "Joystick X", CONSOLE_JOYSTICK_X_DM, DM_INT16, 1 }, { &dmItems.joystickY, "Joystick Y", CONSOLE_JOYSTICK_Y_DM, DM_INT16, 1 }, { &dmItems.joystickZ, "Joystick Z", CONSOLE_JOYSTICK_Z_DM, DM_INT16, 1 }, { &dmItems.joystickYaw, "Joystick Heading", CONSOLE_JOYSTICK_HEADING_DM, DM_INT16, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; dmItemList panTiltDmInit[] = /* Data Manager Item Table for table */ { { &panTiltDm.portPanAxis, "Port Pan Axis", PORT_PAN_JOYSTICK_DM, DM_INT16, 1 }, { &panTiltDm.portTiltAxis, "Port Tilt Axis", PORT_TILT_JOYSTICK_DM, DM_INT16, 1 }, { &panTiltDm.stbdPanAxis, "Stbd Pan Axis", STBD_PAN_JOYSTICK_DM, DM_INT16, 1 }, { &panTiltDm.stbdTiltAxis, "Stbd Tilt Axis", STBD_TILT_JOYSTICK_DM, DM_INT16, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Initialize A to D board hardware with */ /* unity gain */ if (datel612CardInit(A2D_MODE_SCAN, 1) != OK) { logMsg("Joystick In Task: Error initializing A to D board\n"); return(ERROR); } /* if */ /* Create wakeup semaphore timer */ /* for provider rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Joystick In task: Error initializing task resources\n"); return; } /* if */ /* Clear list of Data Manager Item handles*/ bzero((char *) &dmItems, sizeof(dmItems)); bzero((char *) &panTiltDm, sizeof(panTiltDm)); /* Initialize Data Manager Items from table*/ if (initMicroDmItemList(joystickDmInit, CONSOLE_DM_PREFIX, "Joystick In Task") == ERROR) return(ERROR); /* Initialize Data Manager Items from table*/ if (initMicroDmItemList(panTiltDmInit, CONSOLE_DM_PREFIX, "Joystick In Task") == ERROR) return(ERROR); if ((joystickLockDm = initMicroDmItem(CONSOLE_DM_PREFIX, CONSOLE_JOYSTICK_LOCK_DM, DM_MBOOL, 1)) == ERROR) { logMsg("Joystick In Task: Error initializing Joystick Lock DM Item\n"); return (ERROR); } /* if */ /* Initialize A to D board hardware with */ /* Declare this task a provider of */ /* joystick data manager items */ dm_start_provider(dmItems.joystickX, JOYSTICK_DM_PERIOD); dm_start_provider(dmItems.joystickY, JOYSTICK_DM_PERIOD); dm_start_provider(dmItems.joystickZ, JOYSTICK_DM_PERIOD); dm_start_provider(dmItems.joystickYaw, JOYSTICK_DM_PERIOD); dm_start_multiple(panTiltDm.portPanAxis); dm_start_multiple(panTiltDm.portTiltAxis); dm_start_multiple(panTiltDm.stbdPanAxis); dm_start_multiple(panTiltDm.stbdTiltAxis); /* Declare this task a consumer of items */ dm_start_consumer(joystickLockDm, DM_STATIC, SEM_NULL); setPointZ = 0; /* Zero depth set point value */ waitForZeroReturn = FALSE; /* Clear waiting for joystick zero flag */ FOREVER /* Update Rate is not guaranteed due to */ { /* the possibility of task preemption */ /* Read A/D Board analog channels */ if (datel612ScanChannels(JOYSTICK_Z_CHAN, JOYSTICK_SWITCH_CHAN, joystickRaw) != ERROR) { /* Read state of joystick lock switch */ dm_read( joystickLockDm, (char *) &joystickLock, sizeof(joystickLock), NULL); if (!joystickLock) /* If joystick lock not enabled then */ { /* scale Joystick X and Y axies */ joystickScaled[JOYSTICK_X_CHAN] = scaleJoystick( joystickRaw[JOYSTICK_X_CHAN], JOYSTICK_X_OFFSET, JOYSTICK1_FULL_SCALE, JOYSTICK_X_POS_FSV, JOYSTICK_X_NEG_FSV, JOYSTICK1_DEADBAND); joystickScaled[JOYSTICK_Y_CHAN] = scaleJoystick( joystickRaw[JOYSTICK_Y_CHAN], JOYSTICK_Y_OFFSET, JOYSTICK1_FULL_SCALE, JOYSTICK_Y_POS_FSV, JOYSTICK_Y_NEG_FSV, JOYSTICK1_DEADBAND); } /* if */ /* Scale Joystick Z axis */ joystickScaled[JOYSTICK_Z_CHAN] = scaleJoystick( joystickRaw[JOYSTICK_Z_CHAN], JOYSTICK_Z_OFFSET, JOYSTICK2_FULL_SCALE, JOYSTICK_Z_POS_FSV, JOYSTICK_Z_NEG_FSV, JOYSTICK2_DEADBAND); /* Scale Joystick Yaw axis */ joystickScaled[JOYSTICK_YAW_CHAN] = scaleJoystick( joystickRaw[JOYSTICK_YAW_CHAN], JOYSTICK_YAW_OFFSET, JOYSTICK1_FULL_SCALE, JOYSTICK_YAW_POS_FSV, JOYSTICK_YAW_NEG_FSV, JOYSTICK_YAW_DEADBAND); /* Test to see if Z offset switch has */ /* changed from Off to Ok State */ if ( (!waitForZeroReturn) && (joystickRaw[JOYSTICK_Z_SWITCH_CHAN] >= ANALOG_SWITCH_THRESH)) { /* Z offset switch is depressed so test */ /* if Z joystick is at zero */ if (( joystickScaled[JOYSTICK_Z_CHAN]) == 0) setPointZ = 0; /* Yes so set Z offset set point to zero */ else /* Joystick value not zero so change the */ /* Z offset set point */ setPointZ = signedMin(setPointZ + joystickScaled[JOYSTICK_Z_CHAN], JOYSTICK2_FULL_SCALE); joystickZ = setPointZ; waitForZeroReturn = TRUE; } /* if */ if (!waitForZeroReturn) /* If we are not waiting for joystick to */ /* return to zero, add Z axis setpoint */ /* to Z axis joystick value */ joystickZ = signedMin(joystickScaled[JOYSTICK_Z_CHAN] + setPointZ, JOYSTICK2_FULL_SCALE); /* If we are waiting for joystick zero */ /* return, then see if joystick = zero */ else if (joystickScaled[JOYSTICK_Z_CHAN] == 0) waitForZeroReturn = FALSE; /* Fill in switch time structure for */ /* data manager writes */ gettimeofday(&joystickTime, (struct timezone *) NULL); dm_write(dmItems.joystickY, (char *) &joystickScaled[JOYSTICK_X_CHAN], sizeof(joystickScaled[JOYSTICK_X_CHAN]), &joystickTime ); dm_write(dmItems.joystickX, (char *) &joystickScaled[JOYSTICK_Y_CHAN], sizeof(joystickScaled[JOYSTICK_Y_CHAN]), &joystickTime ); dm_write(dmItems.joystickZ, (char *) &joystickZ, sizeof(joystickZ), &joystickTime ); dm_write(dmItems.joystickYaw, (char *) &joystickScaled[JOYSTICK_YAW_CHAN], sizeof(joystickScaled[JOYSTICK_YAW_CHAN]), &joystickTime ); } /* if */ /* Read Pan/Tilt analog channels */ if (datel612ScanChannels(PORT_PAN_CHAN, STBD_TILT_CHAN, &joystickRaw[PORT_PAN_CHAN]) != ERROR) { joystickScaled[PORT_PAN_CHAN] = scaleJoystick( joystickRaw[PORT_PAN_CHAN], PT_PORT_PAN_OFFSET, PT_JOYSTICK_FSV, PT_PORT_PAN_POS_FSV, PT_PORT_PAN_NEG_FSV, PT_JOYSTICK_DEADBAND); joystickScaled[PORT_TILT_CHAN] = scaleJoystick( joystickRaw[PORT_TILT_CHAN], PT_PORT_TILT_OFFSET, PT_JOYSTICK_FSV, PT_PORT_TILT_POS_FSV, PT_PORT_TILT_NEG_FSV, PT_JOYSTICK_DEADBAND); joystickScaled[STBD_PAN_CHAN] = scaleJoystick( joystickRaw[STBD_PAN_CHAN], PT_STBD_PAN_OFFSET, PT_JOYSTICK_FSV, PT_STBD_PAN_POS_FSV, PT_STBD_PAN_NEG_FSV, PT_JOYSTICK_DEADBAND); joystickScaled[STBD_TILT_CHAN] = scaleJoystick( joystickRaw[STBD_TILT_CHAN], PT_STBD_TILT_OFFSET, PT_JOYSTICK_FSV, PT_STBD_TILT_POS_FSV, PT_STBD_TILT_NEG_FSV, PT_JOYSTICK_DEADBAND); /* Fill in switch time structure for */ /* data manager writes */ gettimeofday(&joystickTime, (struct timezone *) NULL); for (axis = PORT_PAN_CHAN; axis <= PORT_TILT_CHAN; axis++) { squaredJs = ((Flt64) joystickScaled[axis]) / sqrt(PT_JOYSTICK_FSV); if (squaredJs < 0.0) joystickScaled[axis] =(Int16) -(squaredJs * squaredJs); else joystickScaled[axis] = (Int16) (squaredJs * squaredJs); if ( abs(joystickScaled[axis] - lastJs[axis]) ) { if (axis == PORT_PAN_CHAN) dm_write(panTiltDm.portPanAxis, (char *) &joystickScaled[axis], sizeof(Int16), &joystickTime ); else dm_write(panTiltDm.portTiltAxis, (char *) &joystickScaled[axis], sizeof(Int16), &joystickTime ); lastJs[axis] = joystickScaled[axis]; } /* if */ } /* for */ for (axis = STBD_PAN_CHAN; axis <= STBD_TILT_CHAN; axis++) { if ( abs(joystickScaled[axis]) - lastJs[axis]) { if (axis == STBD_PAN_CHAN) dm_write(panTiltDm.stbdPanAxis, (char *) &joystickScaled[axis], sizeof(Int16), &joystickTime ); else dm_write(panTiltDm.stbdTiltAxis, (char *) &joystickScaled[axis], sizeof(Int16), &joystickTime ); lastJs[axis] = joystickScaled[axis]; } /* if */ } /* for */ } /* if */ /* Pend until watch dog timer wakes us up */ semTake(wakeupSem, (sysClkRateGet() / (USECS_PER_SEC / JOYSTICK_DM_PERIOD))); } /* 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. */ } /* joystickInTask */ /*****************************************************************************/ /* Function : consoleTask */ /* Purpose : Initializes pilot console software module. Provides */ /* Inputs : None */ /* Outputs : None, but other tasks are spawned */ /*****************************************************************************/ STATUS consoleTask( Void ) { logMsg("Initializing Console Tasks\n"); /* Initialize Console Data Output Task */ if ( taskSpawn("consoleOut", CONSOLE_OUT_PRIORITY, VX_DEALLOC_STACK | VX_STDIO, CONSOLE_OUT_STACKSIZE, (FUNCPTR) consoleOutTask, NULL, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) == ERROR) { logMsg("Console: Error spawning console switch output task\n"); return(ERROR); } /* if */ taskDelay(sysClkRateGet() * 2);/* Wait 2 seconds */ /* Initialize Console Lamp Flasher Task */ if ( taskSpawn("consoleFlash", CONSOLE_FLASH_PRIORITY, VX_DEALLOC_STACK | VX_STDIO, CONSOLE_FLASH_STACKSIZE, (FUNCPTR) consoleFlashTask, NULL, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) == ERROR) { logMsg("Console: Error spawning console switch output task\n"); return(ERROR); } /* if */ /* Initialize Console Data Input Task */ if ( taskSpawn("consoleIn", CONSOLE_IN_PRIORITY, VX_DEALLOC_STACK | VX_STDIO, CONSOLE_IN_STACKSIZE, (FUNCPTR) consoleInTask, NULL, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) == ERROR) { logMsg("Console: Error spawning console switch input task\n"); return(ERROR); } /* if */ /* Initialize Joystick Data Input Task */ if ( taskSpawn("joystick", JOYSTICK_PRIORITY, VX_DEALLOC_STACK | VX_STDIO | VX_FP_TASK, JOYSTICK_STACKSIZE, (FUNCPTR) joystickInTask, NULL, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) == ERROR) { logMsg("Console: Error spawning console joystick input task\n"); return(ERROR); } /* if */ } /* consoleTask() */