/****************************************************************************/ /* Copyright 1992 to 1996 MBARI */ /****************************************************************************/ /* Summary : ROV Pilot Console Interface module for vxWorks */ /* Filename : new_console.c */ /* Author : Douglas Au */ /* Project : Tiburon ROV */ /* Version : Version 1.0 */ /* Created : 10/16/96 */ /* Modified : 10/16/96 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: new_console.c,v 1.1 96/10/16 15:57:00 audo Exp $ * $Log: new_console.c,v $ * Revision 1.1 96/10/16 15:57:00 audo (Douglas Au) * 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 "datel612.h" /* Datel 612 A to D board definitions */ #include "vmi2120.h" /* VMI 2120 digital output board defs */ #include "vmi1111.h" /* VMI 1110 digital input board defs */ #include "sio32Drv.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 "ers.h" #include "ersDcon.h" #include "switches.h" /* Console switch channel assignments */ #include "lamps.h" /* Console Lamp Channel Assignments */ #include "new_consoleDM.h" /* Console Data Manager Item Names */ #include "new_console.h" /* Console Data Types */ /* Joystick Analog Channel Assignments */ #define JOYSTICK_X_CHAN 0 /* Lateral Control Joystick Channel */ #define JOYSTICK_Y_CHAN 1 /* Fwd - Aft Control Joystick Channel */ #define JOYSTICK_YAW_CHAN 2 /* Yaw Control Joystick Axis Channel */ #define JOYSTICK_Z_CHAN 3 /* Depth Control Joystick Channel */ #define PILOT_PAN_CHAN 4 /* Main Joystick PT Pan Channel */ #define PILOT_TILT_CHAN 5 /* Main Joystick PT Tilt Channel */ #define ZOOM_PEDAL_CHAN 6 /* Foot Pedal Camera Zoom Channel */ #define FOCUS_PEDAL_CHAN 7 /* Foot Pedal Camera Focus Channel */ #define PORT_PAN_CHAN 8 /* Port Pan Axis Channel */ #define PORT_TILT_CHAN 9 /* Port Tilt Axis Channel */ #define STBD_PAN_CHAN 10 /* Stbd Pan Axis Channel */ #define STBD_TILT_CHAN 11 /* Stbd Tilt Axis Channel */ #define JOYSTICK1_FULL_SCALE 1024.0 /* Main Joystick Full Scale Value */ #define JOYSTICK2_FULL_SCALE 1024.0 /* Depth Joystick Full Scale Value */ #define PT_PILOT_JOYSTICK_FSV 1024.0 #define PEDAL_FSV 1024.0 /* Joystick Zero Offset Calibration */ #define JOYSTICK_X_OFFSET 0 /* Main Joystick X axis offset */ #define JOYSTICK_Y_OFFSET 0 /* Main Joystick Y axis offset */ #define JOYSTICK_YAW_OFFSET 0 /* Main Joystick Yaw axis offset */ #define JOYSTICK_Z_OFFSET 0 /* Depth Joystick Z azis offset */ #define PT_PORT_PAN_OFFSET -3 #define PT_PORT_TILT_OFFSET 4 #define PT_STBD_PAN_OFFSET -1 #define PT_STBD_TILT_OFFSET 4 #define PT_PILOT_PAN_OFFSET -21 #define PT_PILOT_TILT_OFFSET -6 #define ZOOM_PEDAL_OFFSET 0 #define FOCUS_PEDAL_OFFSET 0 /* Joystick Full Scale Calibration */ #define JOYSTICK_X_POS_FSV 2047.0 /* Main Joystick positive X Max Value */ #define JOYSTICK_X_NEG_FSV -1989.0 /* Main Joystick negative X Max Value */ #define JOYSTICK_Y_POS_FSV 2022.0 /* Main Joystick positive Y Max Value */ #define JOYSTICK_Y_NEG_FSV -2003.0 /* Main Joystick negative Y Max Value */ #define JOYSTICK_YAW_POS_FSV 2047.0 /* Main Joystick positive Yaw Max Value*/ #define JOYSTICK_YAW_NEG_FSV -2008.0 /* Main Joystick negative Yaw Max Value*/ #define JOYSTICK_Z_POS_FSV 2047.0 /* Depth Joystick positive X Max Value */ #define JOYSTICK_Z_NEG_FSV -2019.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 PT_PILOT_PAN_POS_FSV 460.0 #define PT_PILOT_PAN_NEG_FSV -469.0 #define PT_PILOT_TILT_POS_FSV 470.0 #define PT_PILOT_TILT_NEG_FSV -467.0 #define ZOOM_PEDAL_POS_FSV 2046.0 #define ZOOM_PEDAL_NEG_FSV -2039.0 #define FOCUS_PEDAL_POS_FSV 2046.0 #define FOCUS_PEDAL_NEG_FSV -2039.0 #define JOYSTICK1_DEADBAND 2.0 /* Main Joystick Deadband region */ #define JOYSTICK2_DEADBAND 2.0 /* Depth Joystick Deadband region */ #define JOYSTICK_YAW_DEADBAND 2.0 /* Yaw Joystick Deadband region */ #define PT_JOYSTICK_DEADBAND 6.0 /* Pan/Tilt joystick Deadband region */ #define PT_PILOT_JOYSTICK_DEADBAND 6.0 #define PEDAL_DEADBAND 100.0 #define ANALOG_SWITCH_THRESH 512 /* Threshold for analog switch channels */ const Nat16 ballastSwitchAssignments[] = { PORT_BALLAST_SWITCH, AUX_BALLAST_SWITCH, STBD_BALLAST_SWITCH }; const Nat16 lightSwitchAssignments[] = { PORT_HMI_LIGHT_POWER_SWITCH, AUX_HMI_LIGHT_POWER_SWITCH, STBD_HMI_LIGHT_POWER_SWITCH, }; LOCAL STATUS lampTest(MBool *testState); /****************************************************************************/ /* 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( 0x1e3fffff, 0xffffffff ); vmiOutputSet( 0x00000000, 0x0000001f ); /* Dcon Power */ taskDelay(delay); vmiOutputSet( 0x00000000, 0x000003e0 ); /* Light Balasts and P/T's */ taskDelay(delay); vmiOutputSet( 0x00000000, 0x00007c00 ); /* Lights and Resopnder */ taskDelay(delay); vmiOutputSet( 0x00000000, 0x000f8000 ); /* Toolsled and Manipulator */ taskDelay(delay); vmiOutputSet( 0x00000000, 0x00600000 ); /* Spares */ taskDelay(delay); vmiOutputSet( 0x00000fff, 0x01800000 ); /* Thrusters */ taskDelay(delay); vmiOutputSet( 0x00003000, 0x00100000 ); /* Alarm Warning and Alive */ taskDelay(delay); vmiOutputSet( 0x0000c000, 0x00000000 ); /* Master Thruster */ taskDelay(delay); vmiOutputSet( 0x00030000, 0x00000000 ); /* Belly Pack */ taskDelay(delay); vmiOutputSet( 0x000c0000, 0x00000000 ); /* Vehicle Release */ taskDelay(delay); vmiOutputSet( 0x00300000, 0x00000000 ); /* Toolsled Release */ taskDelay(delay); vmiOutputSet( 0x00000000, 0x3e000000 ); /* Auto Modes */ taskDelay(delay); vmiOutputSet( 0x0e000000, 0xc0000000 ); /* joystick and Science Modes */ taskDelay(delay); vmiOutputSet( 0x10000000, 0x00000000 ); /* Suction Sampler */ taskDelay(delay); vmiOutputAllCtrl( OFF ); return(OK); } /* consoleLampInit() */ /****************************************************************************/ /* Function : powerButtonLampCtrl */ /* Purpose : Set lamp state based on value of switch item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS powerButtonLampCtrl(buttonObject *button ) { switchStatus switchState; /* Actual State of Subsystem Switch */ /* Read current switch status */ if (dm_read( button->statusDm.item, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) /* Check subsystem switch request */ vmiOutputCtrl(button->lampBit, (switchState == SWITCH_ON ? ON : OFF)); } /* powerButtonLampCtrl() */ /****************************************************************************/ /* Function : motorEnableButtonLampCtrl */ /* Purpose : Set lamp state based on value ENABLE State */ /* Inputs : buttonObject */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS motorEnableButtonLampCtrl( buttonObject *button ) { motorEnableState enableState; /* Motor Enable Status */ /* Read current switch status */ if (dm_read( button->statusDm.item, (char *) &enableState, sizeof(enableState), (DM_Time *) NULL) == SUCCESS) vmiOutputCtrl(button->lampBit, (enableState == ENABLE_MOTOR ? ON : OFF)); } /* motorEnableButtonLampCtrl() */ /****************************************************************************/ /* Function : deploymentLatchArmLampCtrl */ /* Purpose : Set lamp state based on value of latch Status item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS deploymentLatchArmLampCtrl(buttonObject *button ) { Nat16 latchState; /* Actual State of Subsystem Switch */ /* Read current latch status */ if (dm_read( button->statusDm.item, (char *) &latchState, sizeof(latchState), (DM_Time *) NULL) == SUCCESS) /* Check subsystem switch request */ vmiOutputCtrl(button->lampBit, (latchState == DEPLOYMENT_LATCH_ARM_CODE ? ON : OFF)); } /* deploymentLatchArmLampCtrl() */ /****************************************************************************/ /* Function : toolsledLatchArmLampCtrl */ /* Purpose : Set lamp state based on value of latch Status item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS toolsledLatchArmLampCtrl(buttonObject *button ) { Nat16 latchState; /* Actual State of Subsystem Switch */ /* Read current latch status */ if (dm_read( button->statusDm.item, (char *) &latchState, sizeof(latchState), (DM_Time *) NULL) == SUCCESS) /* Check subsystem switch request */ vmiOutputCtrl(button->lampBit, (latchState == TOOLSLED_LATCH_ARM_CODE ? ON : OFF)); } /* toolsledLatchArmLampCtrl() */ /****************************************************************************/ /* Function : latchReleaseLampCtrl */ /* Purpose : Set lamp state based on value of latch Status item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS latchReleaseLampCtrl(buttonObject *button ) { ersLatchMode latchState; /* Actual State of Subsystem Switch */ /* Read current latch status */ if (dm_read( button->statusDm.item, (char *) &latchState, sizeof(latchState), (DM_Time *) NULL) == SUCCESS) /* Check subsystem switch request */ vmiOutputCtrl(button->lampBit, ((latchState == LATCH_ARMED_NO_PWR) | (latchState == LATCH_RUNNING)) ? ON : OFF); } /* latchReleaseLampCtrl() */ /****************************************************************************/ /* Function : buttonLampFlashCtrl */ /* Purpose : Toggle lamp On/Off if state Dmgr Item is set to POWER_ERROR*/ /* Inputs : buttonObject and toggle state */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS buttonLampFlashCtrl( buttonObject *button, MBool state ) { switchStatus switchState; /* Switch Status */ /* Read current switch status */ if (dm_read( button->statusDm.item, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) if (switchState == SWITCH_POWER_ERROR) vmiOutputCtrl(button->lampBit, state); } /* buttonLampFlashCtrl() */ #if 0 /****************************************************************************/ /* Function : gangedLightFlashCtrl */ /* Purpose : Toggle lamp On/Off if state Dmgr Item is set to POWER_ERROR*/ /* Inputs : lightSetObject and toggle state */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS gangedLightFlashCtrl( lightSetObject *lightSet ) { switchStatus switchState,switchState1, switchState2; /* Switch Status */ DM_Time switchTime; /* Switch update time for Data Mgr */ /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); /* Read current switch status */ if (dm_read( lightSet->status1Dm.item, (char *) &switchState1, sizeof(switchState1), (DM_Time *) NULL) == SUCCESS) if (dm_read( lightSet->status2Dm.item, (char *) &switchState2, sizeof(switchState2), (DM_Time *) NULL) == SUCCESS) if ((switchState1 | switchState2) == SWITCH_POWER_ERROR) { switchState = SWITCH_POWER_ERROR; urgentWriteDmItem(lightSet->requestDm.item, (char *)&switchState, sizeof(switchState)); } } /* gangedLightFlashCtrl() */ #endif /****************************************************************************/ /* Function : motorEnableButtonLampFlashCtrl */ /* Purpose : Toggle lamp On/Off if state Dmgr Item is set to MOOG_FAULT */ /* Inputs : buttonObject and toggleState */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS motorEnableButtonLampFlashCtrl( buttonObject *button, MBool state ) { motorEnableState enableState; /* Motor Enable State */ /* Read current enable status */ if (dm_read( button->statusDm.item, (char *) &enableState, sizeof(enableState), (DM_Time *) NULL) == SUCCESS) if (enableState == MOOG_MOTOR_FAULT) vmiOutputCtrl(button->lampBit, state); } /* motorEnableButtonLampFlashCtrl() */ /****************************************************************************/ /* Function : setPowerSwitchStateDm */ /* Purpose : Set Power switch REQUEST Data Manger item to desired state */ /* Inputs : buttonObject */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS setPowerSwitchStateDm( buttonObject *button, switchStatus switchRequest ) { DM_Time switchTime; /* Switch update time for Data Mgr */ /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (button->requestDm.item, (char *) &switchRequest, sizeof(switchRequest), &switchTime ); } /* setPowerSwitchStateDm() */ /****************************************************************************/ /* Function : initSwitchStatus */ /* Purpose : Set switch STATUS Data Manger item to OFF */ /* Inputs : buttonObject */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS initSwitchStatus( buttonObject *button ) { STATUS rtn = ERROR; MBool false = FALSE; Int32 off = 0; dm_start_provider (button->statusDm.item, DM_STATIC); switch(button->statusDm.type) { case DM_MBOOL: rtn = writeDmItem(button->statusDm.item, &false, sizeof(false)); break; case DM_ENUM: rtn = writeDmItem(button->statusDm.item, &off, sizeof(off)); break; } /* switch */ dm_stop_provider (button->statusDm.item); return (rtn); } /* initSwitchStatus() */ /****************************************************************************/ /* Function : initlightSetStatus */ /* Purpose : Set switch STATUS Data Manger item to OFF */ /* Inputs : buttonObject */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS initLightSetStatus(lightSetObject *lightSet ) { MBool off = FALSE; urgentWriteDmItem(lightSet->status1Dm.item, (char *)lightSet->offValue, sizeof(switchStatus)); urgentWriteDmItem(lightSet->status2Dm.item, (char *)lightSet->offValue, sizeof(switchStatus)); urgentWriteDmItem(lightSet->statusDm.item, (char *) &off, sizeof(off)); return (OK); } /* initLightSetStatus() */ /****************************************************************************/ /* Function : motorEnableButtonSwitchCtrl */ /* Purpose : Check Motor Enable Switch & update Data Manger item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS motorEnableButtonSwitchCtrl( buttonObject *button ) { DM_Time switchTime; /* Switch update time for Data Mgr */ motorEnableState desiredState; /* Desired motor enable switch state*/ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(button->statusDm.item, &desiredState, sizeof(desiredState), &switchTime); desiredState = (desiredState == DISABLE_MOTOR ? ENABLE_MOTOR : DISABLE_MOTOR); dm_write(button->requestDm.item, (char *) &desiredState, sizeof(desiredState), &switchTime); } } return(OK); } /* motorEnableButtonSwitchCtrl() */ /****************************************************************************/ /* Function : powerButtonSwitchCtrl */ /* Purpose : Check Power Switch state and update Data Manger item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS powerButtonSwitchCtrl( buttonObject *button ) { DM_Time switchTime; /* Switch update time for Data Mgr */ switchStatus desiredSwitchState; /* Desired State of Subsystem Switch*/ switchStatus switchState; /* Actual State of Subsystem Switch */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(button->statusDm.item, &switchState, sizeof(switchState), &switchTime); desiredSwitchState = (switchState== SWITCH_OFF ? SWITCH_ON : SWITCH_OFF); dm_write(button->requestDm.item, (char *) &desiredSwitchState, sizeof(desiredSwitchState), &switchTime); } } return(OK); } /* powerButtonSwitchCtrl() */ /****************************************************************************/ /* Function : lampTest */ /* Purpose : Check Power Switch state and update Data Manger item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS lampTest(MBool *testState) { MBool state; Int16 bit; Int16 button; if (*testState) { if (vmiInputState(LAMP_TEST_SWITCH) == OFF) { *testState = FALSE; #if 0 vmiOutputSet(OFF); #endif /* Restore State of lamps */ for (button = 0; button < MAX_BUTTONS; button++) if (buttonObjects[button].buttonHandler!= NULL) buttonObjects[button].buttonHandler(&buttonObjects[button]); return; } } else { if (vmiInputState(LAMP_TEST_SWITCH) == ON) { *testState = TRUE; if (vmiOutputState(BELLY_PACK_POWER_SENSE) == ON) /* BellyPack Relay On */ vmiOutputSet( 0x1e7fefff, 0xffefffff ); else /* BellyPack Relay Off */ vmiOutputSet( 0x1e3fefff, 0xffefffff ); } } return(OK); } /* lampTest() */ /****************************************************************************/ /* Function : gangedLightSwitchCtrl */ /* Purpose : Check Power Switch state and update Data Manger item */ /* Inputs : lampSetObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS gangedLightSwitchCtrl( lightSetObject *lightSet ) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool switchState; switchStatus desiredSwitchState; /* Desired State of Subsystem Switch*/ switchStatus switchState1, switchState2; /* Actual State of Subsystem Switch */ if (lightSet->switchWasPressed) { if (vmiInputState(lightSet->switchBit) == OFF) { lightSet->switchWasPressed = FALSE; return; } } else { if (vmiInputState(lightSet->switchBit) == ON) { lightSet->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(lightSet->statusDm.item, &switchState, sizeof(switchState), &switchTime); desiredSwitchState = (switchState == OFF ? SWITCH_ON : SWITCH_OFF); switchState = (switchState == OFF ? ON : OFF); dm_write(lightSet->requestDm.item, (char *) &switchState, sizeof(switchState), &switchTime); dm_write(lightSet->request1Dm.item, (char *) &desiredSwitchState, sizeof(desiredSwitchState), &switchTime); dm_write(lightSet->request2Dm.item, (char *) &desiredSwitchState, sizeof(desiredSwitchState), &switchTime); } } return(OK); } /* gangedLightSwitchCtrl() */ /****************************************************************************/ /* 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 */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (switchRequestDm, (char *) &state, sizeof(state), &switchTime ); } /* setBoolSwitchStateDm() */ /****************************************************************************/ /* Function : boolButtonLampCtrl */ /* Purpose : Set lamp state based on value on status data mgr item */ /* Inputs : buttonObject */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS boolButtonLampCtrl( buttonObject *button ) { MBool switchState; /* Switch Status */ /* Read current enable switch status*/ if (dm_read( button->statusDm.item, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) vmiOutputCtrl(button->lampBit, (switchState == ON ? ON : OFF)); } /* boolButtonLampCtrl() */ /****************************************************************************/ /* Function : bellyPackButtonLampCtrl */ /* Purpose : Set lamp state based on value on status data mgr item */ /* Inputs : buttonObject */ /* Outputs : NONE */ /****************************************************************************/ LOCAL STATUS bellyPackButtonLampCtrl( buttonObject *button ) { MBool switchState, newSwitchState; /* Switch Status */ /* Read current enable switch status*/ if (dm_read( button->statusDm.item, (char *) &switchState, sizeof(switchState), (DM_Time *) NULL) == SUCCESS) newSwitchState = ( vmiInputState(BELLY_PACK_POWER_SENSE) && switchState); vmiOutputCtrl(button->lampBit, newSwitchState); } /* bellyPackButtonLampCtrl() */ /****************************************************************************/ /* Function : boolButtonSwitchCtrl */ /* Purpose : Check Switch state and update Data Manger item if needed */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS boolButtonSwitchCtrl(buttonObject *button) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool switchState; /* Physical switch state */ MBool desiredState; /* Desired switch Status */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(button->statusDm.item, &switchState, sizeof(switchState), &switchTime); desiredState = (switchState == OFF ? ON : OFF); /* Update data manager item value */ setBoolSwitchStateDm(button->requestDm.item, desiredState ); } return(OK); } } /* boolButtonSwitchCtrl() */ /****************************************************************************/ /* Function : boolButtonThrustLevel */ /* Purpose : Check Switch state and update Data Manger item if needed */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /* Notes : Dfrench added this to put tiburon into 50% level w/joystick*/ /****************************************************************************/ LOCAL STATUS boolButtonThrustLevel(buttonObject *button) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool switchState; /* Physical switch state */ MBool desiredState; /* Desired switch Status */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(button->statusDm.item, &switchState, sizeof(switchState), &switchTime); desiredState = (switchState == OFF ? ON : OFF); /* Update data manager item value */ setBoolSwitchStateDm(button->requestDm.item, desiredState ); urgentWriteDmItem(joystickGainSwitchDm, &desiredState, sizeof(desiredState)); } return(OK); } } /* boolButtonThrustLevel() */ /****************************************************************************/ /* Function : boolButtonMSwitchCtrl */ /* Purpose : Check Switch state and update Data Manger item if needed */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS boolButtonMSwitchCtrl(buttonObject *button) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool switchState; /* Physical switch state */ MBool desiredState; /* Desired switch Status */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; desiredState = OFF; /* Update data manager item value */ setBoolSwitchStateDm(button->requestDm.item, desiredState ); return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; desiredState = ON; /* Update data manager item value */ setBoolSwitchStateDm(button->requestDm.item, desiredState ); } return(OK); } } /* boolButtonMSwitchCtrl() */ /****************************************************************************/ /* Function : bellyPackEnableSwitchCtrl */ /* Purpose : Check Switch state and update Data Manger item if needed */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS bellyPackEnableSwitchCtrl(buttonObject *button) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool switchState; /* Physical switch state */ MBool desiredState; /* Desired switch Status */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(button->statusDm.item, &switchState, sizeof(switchState), &switchTime); desiredState = (switchState == OFF ? ON : OFF); /* Update data manager item value */ setBoolSwitchStateDm(button->requestDm.item, desiredState ); } return(OK); } } /* bellyPackEnableSwitchCtrl() */ /****************************************************************************/ /* Function : masterMotorPowerSwitchCtrl */ /* Purpose : Check Master Motor Power Switch state */ /* Inputs : Array of Data Manager Item handles & switch states */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS masterMotorPowerSwitchCtrl( thrusterSetObject *motors ) { MBool lastState; MBool masterSwitch; /* Desired Master Switch status */ switchStatus motorSwitch; /* Desired Motor Switch status */ DM_Time switchTime; /* Switch update time for Data Mgr */ if (motors->switchWasPressed) { if (vmiInputState(motors->switchBit) == OFF) { motors->switchWasPressed = FALSE; return; } } else { if (vmiInputState(motors->switchBit) == ON) { motors->switchWasPressed = TRUE; dm_read (motors->statusDm.item, &lastState, sizeof(lastState), &switchTime); masterSwitch = (lastState == OFF ? ON : OFF); motorSwitch = (lastState == OFF ? SWITCH_ON : SWITCH_OFF); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (motors->requestDm.item, (char *) &masterSwitch, sizeof(masterSwitch), &switchTime ); dm_write (motors->request1Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request2Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request3Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request4Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request5Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request6Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write ( motors->statusDm.item, (char *) &masterSwitch, sizeof(masterSwitch), &switchTime ); } /* if */ } return(OK); } /* masterMotorPowerSwitchCtrl() */ /****************************************************************************/ /* Function : masterMotorEnableSwitchCtrl */ /* Purpose : Check Master Motor Power Switch state */ /* Inputs : Array of Data Manager Item handles & switch states */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS masterMotorEnableSwitchCtrl(thrusterSetObject *motors ) { MBool lastState; MBool masterSwitch; /* Desired Master Switch status */ switchStatus motorSwitch; /* Desired Motor Switch status */ DM_Time switchTime; /* Switch update time for Data Mgr */ if (motors->switchWasPressed) { if (vmiInputState(motors->switchBit) == OFF) { motors->switchWasPressed = FALSE; return; } } else { if (vmiInputState(motors->switchBit) == ON) { motors->switchWasPressed = TRUE; dm_read ( motors->statusDm.item, &lastState, sizeof(lastState), &switchTime); masterSwitch = (lastState == OFF ? ON : OFF); motorSwitch = (lastState == OFF ? ENABLE_MOTOR : DISABLE_MOTOR); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write ( motors->requestDm.item, (char *) &masterSwitch, sizeof(masterSwitch), &switchTime ); dm_write (motors->request1Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request2Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request3Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request4Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request5Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write (motors->request6Dm.item, (char *) &motorSwitch, sizeof(motorSwitch), &switchTime ); dm_write ( motors->statusDm.item, (char *) &masterSwitch, sizeof(masterSwitch), &switchTime ); } /* if */ } return(OK); } /* masterMotorEnableSwitchCtrl() */ /****************************************************************************/ /* Function : deploymentLatchArmSwitchCtrl */ /* Purpose : Check Latch Switch state and update Data Manger item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS deploymentLatchArmSwitchCtrl( buttonObject *button ) { Nat16 switchState; /* Actual State of Subsystem Switch */ Nat16 desiredSwitchState; DM_Time switchTime; /* Switch update time for Data Mgr */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; dm_read(button->statusDm.item, &switchState, sizeof(switchState), (DM_Time *) NULL); desiredSwitchState = ((switchState == DEPLOYMENT_LATCH_ARM_CODE) ? 0 : DEPLOYMENT_LATCH_ARM_CODE); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (button->statusDm.item, (char *) &desiredSwitchState, sizeof(desiredSwitchState), &switchTime ); #if 0 writeDmItem(button->requestDm.item, (void *) ((switchState == DEPLOYMENT_LATCH_ARM_CODE) ? 0 : DEPLOYMENT_LATCH_ARM_CODE), sizeof(Nat16)); #endif } } return(OK); } /* deploymentLatchArmSwitchCtrl() */ /****************************************************************************/ /* Function : toolsledLatchArmSwitchCtrl */ /* Purpose : Check Latch Switch state and update Data Manger item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS toolsledLatchArmSwitchCtrl( buttonObject *button ) { Nat16 switchState; /* Actual State of Subsystem Switch */ Nat16 desiredSwitchState; DM_Time switchTime; /* Switch update time for Data Mgr */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; dm_read(button->statusDm.item, &switchState, sizeof(switchState), (DM_Time *) NULL); desiredSwitchState = ((switchState == TOOLSLED_LATCH_ARM_CODE) ? 0 : TOOLSLED_LATCH_ARM_CODE); /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_write (button->statusDm.item, (char *) &desiredSwitchState, sizeof(desiredSwitchState), &switchTime ); #if 0 writeDmItem(button->requestDm.item, (void *) ((switchState == TOOLSLED_LATCH_ARM_CODE) ? 0 : TOOLSLED_LATCH_ARM_CODE ), sizeof(Nat16)); #endif } } return(OK); } /* toolsledLatchArmSwitchCtrl() */ /****************************************************************************/ /* Function : latchReleaseSwitchCtrl */ /* Purpose : Check Latch Switch state and update Data Manger item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS latchReleaseSwitchCtrl( buttonObject *button ) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool desiredSwitchState; /* Desired State of Subsystem Switch*/ ersLatchMode switchState; /* Actual State of Subsystem Switch */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(button->statusDm.item, &switchState, sizeof(switchState), &switchTime); if (switchState == LATCH_ARMED) dm_write(button->requestDm.item, (char *) &desiredSwitchState, 0, &switchTime); } } return(OK); } /* latchReleaseSwitchCtrl() */ /****************************************************************************/ /* Function : toolsledLatchReleaseSwitchCtrl */ /* Purpose : Check ToolsledLatch state and update Data Manger item */ /* Inputs : buttonObject */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS toolsledLatchReleaseSwitchCtrl( buttonObject *button ) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool desiredSwitchState; /* Desired State of Subsystem Switch*/ ersLatchMode switchState; /* Actual State of Subsystem Switch */ if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(button->statusDm.item, &switchState, sizeof(switchState), &switchTime); if (switchState == LATCH_ARMED) desiredSwitchState = TRUE; dm_write(button->requestDm.item, (char *) &desiredSwitchState, sizeof(desiredSwitchState), &switchTime); } } return(OK); } /* toolsledLatchReleaseSwitchCtrl() */ /****************************************************************************/ /* Function : bellyPackButtonSwitchCtrl */ /* 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 bellyPackButtonSwitchCtrl( buttonObject *button) { STATUS switchState; /* Physical switch state */ MBool desiredState; /* Desired switch Status */ Int32 delay; delay = sysClkRateGet() / 10; if (button->switchWasPressed) { if (vmiInputState(button->switchBit) == OFF) { button->switchWasPressed = FALSE; return; } } else { if (vmiInputState(button->switchBit) == ON) { button->switchWasPressed = TRUE; dm_read( button->statusDm.item, (char *) &desiredState, sizeof(desiredState), (DM_Time *) NULL); desiredState = (desiredState == OFF ? ON : OFF); vmiOutputCtrl(BELLY_PACK_POWER_RELAY, desiredState); taskDelay(delay); /* Wait for relay to switch then signal OutTask */ /* Update data manager item value */ setBoolSwitchStateDm( button->requestDm.item, desiredState ); } } return(OK); } /* bellyPackButtonSwitchCtrl() */ /****************************************************************************/ /* 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(autoZObject *items, Nat16 autoDepthSwitch, Nat16 autoAltitudeSwitch ) { STATUS switchState; /* Physical switch state */ DM_Time switchTime; MBool bellyPackEnabled, autoDepthState, autoAltitudeState; dm_read(items->bellyPackStatusDm.item, (char *) &bellyPackEnabled, sizeof(bellyPackEnabled), (DM_Time *) NULL); dm_read(items->autoDepthStatusDm.item, (char *) &autoDepthState, sizeof(autoDepthState), (DM_Time *) NULL); dm_read(items->autoAltitudeStatusDm.item, (char *) &autoAltitudeState, sizeof(autoAltitudeState), (DM_Time *) NULL); /* 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 (items->autoAltitudeDm.item, (char *) &autoAltitudeState, sizeof(autoAltitudeState), &switchTime ); } /* if */ dm_write (items->autoDepthDm.item, (char *) &autoDepthState, sizeof(autoDepthState), &switchTime ); dm_write (items->autoDepthStatusDm.item, (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 (items->autoAltitudeDm.item, (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 (items->autoDepthDm.item, (char *) &autoDepthState, sizeof(autoDepthState), &switchTime ); dm_write (items->autoDepthStatusDm.item, (char *) &autoDepthState, sizeof(autoDepthState), &switchTime ); } /* if */ dm_write (items->autoAltitudeDm.item, (char *) &autoAltitudeState, sizeof(autoAltitudeState), &switchTime ); } /* if */ /* Wait for user to release switch */ while (switchState == vmiInputState( autoAltitudeSwitch )) taskDelay(sysClkRateGet() / 20); } /* if */ return(OK); } /* checkAutoDepthAltitude() */ /****************************************************************************/ /* Function : checkAutoHeading */ /* Purpose : Check Auto Heading if BP not Enabled */ /* Inputs : auto Heading Object */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS checkAutoHeading(autoHeadingObject *items ) { DM_Time switchTime; /* Switch update time for Data Mgr */ MBool bellyPackEnable, switchState, desiredState; dm_read(items->bellyPackStatusDm.item, (char *) &bellyPackEnable, sizeof(bellyPackEnable), (DM_Time *)NULL); if(!bellyPackEnable) { if (items->switchWasPressed) { if (vmiInputState(items->switchBit) == OFF) { items->switchWasPressed = FALSE; return; } } else { if (vmiInputState(items->switchBit) == ON) { items->switchWasPressed = TRUE; /* Fill in switch time structure */ gettimeofday(&switchTime, (struct timezone *) NULL); dm_read(items->autoHeadingStatusDm.item, &switchState, sizeof(switchState), &switchTime); desiredState = (switchState == OFF ? ON : OFF); /* Update data manager item value */ setBoolSwitchStateDm(items->autoHeadingRequestDm.item, desiredState ); } return(OK); } } } /****************************************************************************/ /* Function : bellyPackButtonEnableCtrl */ /* 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 */ /****************************************************************************/ MLocal Void processBellypackEnable(bellyPackObject *consoleSwitch, ctrlFunctionDmItems *console, ctrlFunctionDmItems *bellypack, ctrlFunctionDmItems *controlSystem) { MBool switchState; /* Switch Status */ /* Read current switch status */ if (dm_read( consoleSwitch->statusDm.item, (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->joystickLockDm.item, FALSE); setBoolSwitchStateDm(consoleSwitch->joystickRangeDm.item, 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 : 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() */ 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 : consoleOutTask */ /* Purpose : Pilot console software switch lamp output module. */ /* Inputs : None */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS consoleOutTask( Void ) { STATUS status; Int32 item; /* Counter for DM Item Initialization */ Int32 button; Int32 lightSet; Int32 group; /* Group Index */ Int16 ballast, light; /* ballast & light identifiers */ DM_Item powerIbcGfDisable; /* Enable Power IBC Ground Fault Monitor*/ DM_Group dmGroup[MAX_GROUP_NUM];/* Console Data Manager Group */ DM_Group dmLightGroup; SEM_ID dmUpdateSem; /* Data Manager Item Change wakeup sem */ /* Holds dm_group_changes bit vector */ DWord consoleGroupBits[MAX_GROUP_NUM]; DWord lightGroupBits; DWord switchBit[MAX_BUTTONS]; /* Port Horiz Thruster Switch change bit*/ DWord lightSwitchBit[MAX_LIGHT_SETS]; DWord bellyPackEnableBit; ctrlFunctionDmItems consoleDm; ctrlFunctionDmItems bellyPackDm; ctrlFunctionDmItems controlDm; 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 */ else logMsg("Console Out Task: created update sem OK, SEM_ID = %#x\n", dmUpdateSem); createCtrlFunctionDmItems(CONSOLE_DM_PREFIX, &consoleDm); createCtrlFunctionDmItems(BELLY_PACK_DM_PREFIX, &bellyPackDm); createCtrlFunctionDmItems("TIBURON.CONTROL.", &controlDm); /* Initialize Data Manager Items from table*/ for (button = 0; button < MAX_BUTTONS; button++) { if (buttonObjects[button].statusDm.item != NO_ITEM) { status = dm_start_consumer(buttonObjects[button].statusDm.item, DM_STATIC, dmUpdateSem); initSwitchStatus( &buttonObjects[button] ); } } /* for */ for (lightSet = 0; lightSet < MAX_LIGHT_SETS; lightSet++) { if (lightSetObjects[lightSet].statusDm.item != NO_ITEM) dm_start_consumer(lightSetObjects[lightSet].statusDm.item, DM_STATIC, dmUpdateSem); if (lightSetObjects[lightSet].status1Dm.item != NO_ITEM) dm_start_consumer(lightSetObjects[lightSet].status1Dm.item, DM_STATIC, dmUpdateSem); if (lightSetObjects[lightSet].status2Dm.item != NO_ITEM) dm_start_consumer(lightSetObjects[lightSet].status2Dm.item, DM_STATIC, dmUpdateSem); } /* for */ if (bellyPackItem.statusDm.item != NO_ITEM) dm_start_consumer(bellyPackItem.statusDm.item, DM_STATIC, dmUpdateSem); /* 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, FALSE); } /* 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, FALSE); } /* if */ /* Initialize Light Objects here */ for (lightSet = 0; lightSet < MAX_LIGHT_SETS; lightSet++) initLightSetStatus( &lightSetObjects[lightSet]); /* Create Data Manager Group for items */ for (group = 0; group < MAX_GROUP_NUM; group ++) dmGroup[group] = dm_create_group(); for (button = 0; button < MAX_BUTTONS; button++) { if (button < 32) { group = 0; if (dm_group_add_item(dmGroup[group], buttonObjects[button].statusDm.item, &switchBit[button]) != SUCCESS) logMsg("Console Out Task: dm_group_add_item failed\n"); } else if (button < 64) { group = 1; if (dm_group_add_item(dmGroup[group], buttonObjects[button].statusDm.item, &switchBit[button]) != SUCCESS) logMsg("Console Out Task: dm_group_add_item failed\n"); } else { group = 2; if (dm_group_add_item(dmGroup[group], buttonObjects[button].statusDm.item, &switchBit[button]) != SUCCESS) logMsg("Console Out Task: dm_group_add_item failed\n"); } } dmLightGroup = dm_create_group(); dm_group_add_item(dmLightGroup, bellyPackItem.statusDm.item, &bellyPackEnableBit); /* Initialize state of lamps */ for (button = 0; button < MAX_BUTTONS; button++) if (buttonObjects[button].buttonHandler!= NULL) buttonObjects[button].buttonHandler(&buttonObjects[button]); processBellypackEnable(&bellyPackItem, &consoleDm, &bellyPackDm, &controlDm); FOREVER /* Repeat forever */ { /* Wait for any data items to change */ semTake(dmUpdateSem, WAIT_FOREVER); /* Ask Data Manager which items changed */ for (group=0; group < MAX_GROUP_NUM; group++) consoleGroupBits[group] = dm_get_group_changes(dmGroup[group]); lightGroupBits = dm_get_group_changes(dmLightGroup); for (button = 0; button < MAX_BUTTONS; button++) { if(button < 32) { group = 0; if (consoleGroupBits[group] & switchBit[button]) if (buttonObjects[button].buttonHandler!= NULL) buttonObjects[button].buttonHandler(&buttonObjects[button]); } else if(button < 64) { group = 1; if (consoleGroupBits[group] & switchBit[button]) if (buttonObjects[button].buttonHandler!= NULL) buttonObjects[button].buttonHandler(&buttonObjects[button]); } else { group = 2; if (consoleGroupBits[group] & switchBit[button]) if (buttonObjects[button].buttonHandler!= NULL) buttonObjects[button].buttonHandler(&buttonObjects[button]); } } if (lightGroupBits & bellyPackEnableBit) processBellypackEnable(&bellyPackItem, &consoleDm, &bellyPackDm, &controlDm); } /* 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 */ MBool toggle; /* Present lamp state */ Int32 button; /* Button Index */ Int32 lightSet; /* 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 */ for (button = 0; button < MAX_BUTTONS; button++) if (buttonObjects[button].statusDm.item!= NO_ITEM) dm_start_consumer(buttonObjects[button].statusDm.item, DM_STATIC, SEM_NULL); for (lightSet = 0; lightSet < MAX_LIGHT_SETS; lightSet++) { if (lightSetObjects[lightSet].status1Dm.item != NO_ITEM) dm_start_consumer(lightSetObjects[lightSet].status1Dm.item, DM_STATIC, SEM_NULL); if (lightSetObjects[lightSet].status2Dm.item != NO_ITEM) dm_start_consumer(lightSetObjects[lightSet].status2Dm.item, DM_STATIC, SEM_NULL); } /* for */ FOREVER { for (button = 0; button < MAX_BUTTONS; button++) if (buttonObjects[button].buttonFlashHandler!= NULL) buttonObjects[button].buttonFlashHandler( &buttonObjects[button],toggle); #if 0 for (lightSet = 0; lightSet < MAX_LIGHT_SETS; lightSet++) gangedLightFlashCtrl(&lightSetObjects[lightSet]); #endif /* 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 */ Int32 button; /* Button Index */ Int32 lightSet; /* LampSet Index Int16 ballast, light; /* ballast & light identifiers */ MBool testState = FALSE; vmiInputBoardInit(); /* Initialize VMI digital input board */ /* 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 */ for(button=0; button 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() */ #if 0 LOCAL Int16 scaleJoystick( Int16 joystickRaw, Int16 offset, Flt32 fullScaleActual, Flt32 fullScalePosObs, Flt32 fullScaleNegObs, Flt32 deadband ) { return( (Int16) joystickRaw ); } /* scaleJoystick() */ #endif /*****************************************************************************/ /* 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_Item vertJoystickLockDm; 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 */ MBool vertJoystickLock; /* Vertical Joystick Lock Mode Enable */ Int16 joystickZ; /* Joystick Z value for dm_write */ Int16 setPointZ; /* Joystick Z offset set point value */ MBool waitForZeroReturn; /* Set when waiting for joystick zero */ MBool goodSetPoint; 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 }, { &dmItems.zoomPedal, "Zoom Pedal", CONSOLE_ZOOM_PEDAL_DM, DM_INT16, 1 }, { &dmItems.focusPedal, "Focus Pedal", CONSOLE_FOCUS_PEDAL_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 }, { &panTiltDm.pilotPanAxis, "Pilot Pan Axis", PILOT_PAN_JOYSTICK_DM, DM_INT16, 1 }, { &panTiltDm.pilotTiltAxis, "Pilot Tilt Axis", PILOT_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_JOYSTICK_LOCK_DM, DM_MBOOL, 1)) == ERROR) { logMsg("Joystick In Task: Error initializing Joystick Lock DM Item\n"); return (ERROR); } /* if */ if ((vertJoystickLockDm = initMicroDmItem("", CONSOLE_VERT_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_provider(dmItems.zoomPedal, JOYSTICK_DM_PERIOD); dm_start_provider(dmItems.focusPedal, JOYSTICK_DM_PERIOD); dm_start_multiple(panTiltDm.portPanAxis); dm_start_multiple(panTiltDm.portTiltAxis); dm_start_multiple(panTiltDm.stbdPanAxis); dm_start_multiple(panTiltDm.stbdTiltAxis); dm_start_provider(panTiltDm.pilotPanAxis, JOYSTICK_DM_PERIOD); dm_start_provider(panTiltDm.pilotTiltAxis, JOYSTICK_DM_PERIOD); /* Declare this task a consumer of items */ dm_start_consumer(joystickLockDm, DM_STATIC, SEM_NULL); dm_start_consumer(vertJoystickLockDm, DM_STATIC, SEM_NULL); setPointZ = 0; /* Zero depth set point value */ waitForZeroReturn = FALSE; /* Clear waiting for joystick zero flag */ goodSetPoint = FALSE; FOREVER /* Update Rate is not guaranteed due to */ { /* the possibility of task preemption */ /* Read A/D Board analog channels */ if (datel612ScanChannels(JOYSTICK_X_CHAN, JOYSTICK_Z_CHAN, joystickRaw) != ERROR) { /* Read state of joystick lock switch */ dm_read( joystickLockDm, (char *) &joystickLock, sizeof(joystickLock), NULL); dm_read( vertJoystickLockDm, (char *) &vertJoystickLock, sizeof(vertJoystickLock), 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); /* 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); } /* 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); if (!vertJoystickLock) { goodSetPoint = FALSE; joystickZ = joystickScaled[JOYSTICK_Z_CHAN]; } else { if ((!waitForZeroReturn) && (!goodSetPoint)) { /* 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 setPointZ = joystickScaled[JOYSTICK_Z_CHAN]; /* Joystick value not zero so change the */ #if 0 /* Z offset set point */ setPointZ = signedMin(setPointZ + joystickScaled[JOYSTICK_Z_CHAN], JOYSTICK2_FULL_SCALE); #endif joystickZ = setPointZ; waitForZeroReturn = TRUE; goodSetPoint = 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; }/*else*/ /* 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(PILOT_PAN_CHAN, STBD_TILT_CHAN, &joystickRaw[PILOT_PAN_CHAN]) != ERROR) { joystickScaled[PILOT_PAN_CHAN] = scaleJoystick( joystickRaw[PILOT_PAN_CHAN], PT_PILOT_PAN_OFFSET, PT_PILOT_JOYSTICK_FSV, PT_PILOT_PAN_POS_FSV, PT_PILOT_PAN_NEG_FSV, PT_PILOT_JOYSTICK_DEADBAND); joystickScaled[PILOT_TILT_CHAN] = scaleJoystick( joystickRaw[PILOT_TILT_CHAN], PT_PILOT_TILT_OFFSET, PT_PILOT_JOYSTICK_FSV, PT_PILOT_TILT_POS_FSV, PT_PILOT_TILT_NEG_FSV, PT_PILOT_JOYSTICK_DEADBAND); joystickScaled[ZOOM_PEDAL_CHAN] = scaleJoystick( joystickRaw[ZOOM_PEDAL_CHAN], ZOOM_PEDAL_OFFSET, PEDAL_FSV, ZOOM_PEDAL_POS_FSV, ZOOM_PEDAL_NEG_FSV, PEDAL_DEADBAND); joystickScaled[FOCUS_PEDAL_CHAN] = scaleJoystick( joystickRaw[FOCUS_PEDAL_CHAN], FOCUS_PEDAL_OFFSET, PEDAL_FSV, FOCUS_PEDAL_POS_FSV, FOCUS_PEDAL_NEG_FSV, PEDAL_DEADBAND); 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); dm_write(dmItems.zoomPedal, (char *) &joystickScaled[ZOOM_PEDAL_CHAN], sizeof(joystickScaled[ZOOM_PEDAL_CHAN]), &joystickTime ); dm_write(dmItems.focusPedal, (char *) &joystickScaled[FOCUS_PEDAL_CHAN], sizeof(joystickScaled[FOCUS_PEDAL_CHAN]), &joystickTime ); 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 */ for (axis = PILOT_PAN_CHAN; axis <= PILOT_TILT_CHAN; axis++) { if ( abs(joystickScaled[axis]) - lastJs[axis]) { if (axis == PILOT_PAN_CHAN) dm_write(panTiltDm.pilotPanAxis, (char *) &joystickScaled[axis], sizeof(Int16), &joystickTime ); else dm_write(panTiltDm.pilotTiltAxis, (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 ) { Int32 button; Int32 lightSet; Errno status; logMsg("Initializing Console Button Objects\n"); for( button = 0; button < MAX_BUTTONS; button++ ) { /* create request object */ status = dm_create((char *)buttonObjects[button].requestDm.name, 1, &buttonObjects[button].requestDm.item, buttonObjects[button].requestDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)buttonObjects[button].statusDm.name, 1, &buttonObjects[button].statusDm.item, buttonObjects[button].statusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ } for( lightSet = 0; lightSet < MAX_LIGHT_SETS; lightSet++ ) { /* create request object */ status = dm_create((char *)lightSetObjects[lightSet].requestDm.name, 1, &lightSetObjects[lightSet].requestDm.item, lightSetObjects[lightSet].requestDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)lightSetObjects[lightSet].statusDm.name, 1, &lightSetObjects[lightSet].statusDm.item, lightSetObjects[lightSet].statusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create((char *)lightSetObjects[lightSet].request1Dm.name, 1, &lightSetObjects[lightSet].request1Dm.item, lightSetObjects[lightSet].request1Dm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)lightSetObjects[lightSet].status1Dm.name, 1, &lightSetObjects[lightSet].status1Dm.item, lightSetObjects[lightSet].status1Dm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create((char *)lightSetObjects[lightSet].request2Dm.name, 1, &lightSetObjects[lightSet].request2Dm.item, lightSetObjects[lightSet].request2Dm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)lightSetObjects[lightSet].status2Dm.name, 1, &lightSetObjects[lightSet].status2Dm.item, lightSetObjects[lightSet].status2Dm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ } /* create Belly Pack Object */ status = dm_create((char *)bellyPackItem.statusDm.name, 1, &bellyPackItem.statusDm.item, bellyPackItem.statusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)bellyPackItem.joystickLockDm.name, 1, &bellyPackItem.joystickLockDm.item, bellyPackItem.joystickLockDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create((char *)bellyPackItem.joystickRangeDm.name, 1, &bellyPackItem.joystickRangeDm.item, bellyPackItem.joystickRangeDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create auto Heading Object */ status = dm_create((char *)autoHeadingItem.bellyPackStatusDm.name, 1, &autoHeadingItem.bellyPackStatusDm.item, autoHeadingItem.bellyPackStatusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)autoHeadingItem.autoHeadingRequestDm.name, 1, &autoHeadingItem.autoHeadingRequestDm.item, autoHeadingItem.autoHeadingRequestDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create((char *)autoHeadingItem.autoHeadingStatusDm.name, 1, &autoHeadingItem.autoHeadingStatusDm.item, autoHeadingItem.autoHeadingStatusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create auto Depth / Altitude Object */ status = dm_create((char *)autoZFuncItem.bellyPackStatusDm.name, 1, &autoZFuncItem.bellyPackStatusDm.item, autoZFuncItem.bellyPackStatusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)autoZFuncItem.autoDepthDm.name, 1, &autoZFuncItem.autoDepthDm.item, autoZFuncItem.autoDepthDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create((char *)autoZFuncItem.autoDepthStatusDm.name, 1, &autoZFuncItem.autoDepthStatusDm.item, autoZFuncItem.autoDepthStatusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create status object */ status = dm_create( (char *)autoZFuncItem.autoAltitudeDm.name, 1, &autoZFuncItem.autoAltitudeDm.item, autoZFuncItem.autoAltitudeDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create((char *)autoZFuncItem.autoAltitudeStatusDm.name, 1, &autoZFuncItem.autoAltitudeStatusDm.item, autoZFuncItem.autoAltitudeStatusDm.type, 1, DM_ENDT ); /* Treat item exists the same as success*/ if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* create Master Thruster Items */ status = dm_create( (char *)thrusterMasterPowerObject.requestDm.name, 1, &thrusterMasterPowerObject.requestDm.item, thrusterMasterPowerObject.requestDm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.statusDm.name, 1, &thrusterMasterPowerObject.statusDm.item, thrusterMasterPowerObject.statusDm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.request1Dm.name, 1, &thrusterMasterPowerObject.request1Dm.item, thrusterMasterPowerObject.request1Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.status1Dm.name, 1, &thrusterMasterPowerObject.status1Dm.item, thrusterMasterPowerObject.status1Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.request2Dm.name, 1, &thrusterMasterPowerObject.request2Dm.item, thrusterMasterPowerObject.request2Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.status2Dm.name, 1, &thrusterMasterPowerObject.status2Dm.item, thrusterMasterPowerObject.status2Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.request3Dm.name, 1, &thrusterMasterPowerObject.request3Dm.item, thrusterMasterPowerObject.request3Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.status3Dm.name, 1, &thrusterMasterPowerObject.status3Dm.item, thrusterMasterPowerObject.status3Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.request4Dm.name, 1, &thrusterMasterPowerObject.request4Dm.item, thrusterMasterPowerObject.request4Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.status4Dm.name, 1, &thrusterMasterPowerObject.status4Dm.item, thrusterMasterPowerObject.status4Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.request5Dm.name, 1, &thrusterMasterPowerObject.request5Dm.item, thrusterMasterPowerObject.request5Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.status5Dm.name, 1, &thrusterMasterPowerObject.status5Dm.item, thrusterMasterPowerObject.status5Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.request6Dm.name, 1, &thrusterMasterPowerObject.request6Dm.item, thrusterMasterPowerObject.request6Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterPowerObject.status6Dm.name, 1, &thrusterMasterPowerObject.status6Dm.item, thrusterMasterPowerObject.status6Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /* Create thruster Master Enabel Items */ status = dm_create( (char *)thrusterMasterEnableObject.requestDm.name, 1, &thrusterMasterEnableObject.requestDm.item, thrusterMasterEnableObject.requestDm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.statusDm.name, 1, &thrusterMasterEnableObject.statusDm.item, thrusterMasterEnableObject.statusDm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.request1Dm.name, 1, &thrusterMasterEnableObject.request1Dm.item, thrusterMasterEnableObject.request1Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.status1Dm.name, 1, &thrusterMasterEnableObject.status1Dm.item, thrusterMasterEnableObject.status1Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.request2Dm.name, 1, &thrusterMasterEnableObject.request2Dm.item, thrusterMasterEnableObject.request2Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.status2Dm.name, 1, &thrusterMasterEnableObject.status2Dm.item, thrusterMasterEnableObject.status2Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.request3Dm.name, 1, &thrusterMasterEnableObject.request3Dm.item, thrusterMasterEnableObject.request3Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.status3Dm.name, 1, &thrusterMasterEnableObject.status3Dm.item, thrusterMasterEnableObject.status3Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.request4Dm.name, 1, &thrusterMasterEnableObject.request4Dm.item, thrusterMasterEnableObject.request4Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.status4Dm.name, 1, &thrusterMasterEnableObject.status4Dm.item, thrusterMasterEnableObject.status4Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.request5Dm.name, 1, &thrusterMasterEnableObject.request5Dm.item, thrusterMasterEnableObject.request5Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.status5Dm.name, 1, &thrusterMasterEnableObject.status5Dm.item, thrusterMasterEnableObject.status5Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.request6Dm.name, 1, &thrusterMasterEnableObject.request6Dm.item, thrusterMasterEnableObject.request6Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ status = dm_create( (char *)thrusterMasterEnableObject.status6Dm.name, 1, &thrusterMasterEnableObject.status6Dm.item, thrusterMasterEnableObject.status6Dm.type, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ /***********************************Daves Test Code *********************/ status = dm_create(JOYSTICK_GAIN_SWITCH_DM, 1, &joystickGainSwitchDm, DM_MBOOL, 1, DM_ENDT ); if ((status != SUCCESS) && (status != EDM_NAME_EXISTS)) return(ERROR); /* An error occurred so return ERROR */ 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 */