/****************************************************************************/ /* Copyright 1994 to 1997 MBARI */ /****************************************************************************/ /* Summary : Pan & Tilt Microcontroller Interface Module for VxWorks */ /* Filename : panTilt.c */ /* Author : Andrew Pearce */ /* Project : Tiburon ROV */ /* Version : Version 1.0 */ /* Created : 04/21/94 */ /* Modified : 05/01/98 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header$ * $Log$ */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks semaphore functions */ #include /* vxWorks task library functions */ #include /* vxWorks time and date functions */ #include /* vxWorks string function library */ #include /* vxWorks Message Queue Library */ #include /* vxWorks standard IO file functions */ #include /* MBARI style guide type declarations */ #include /* Miscellaneous constants */ #include /* MBARI Rov Application Priorities */ #include /* MBARI time and date functions */ #include /* Data Manager declarations */ #include /* Data Manager error declarations */ #include "powerAlloc.h" /* Device Power Requirements */ #include "powerDM.h" /* Power System Data Manager Items */ #include "powerTypes.h" /* Power management Data Types */ #include "console.h" /* Topside Console Data Manager Items */ #include "sio32.h" /* Sio32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #define Boolean MBool /* Map microcontroller Boolean to MBool */ #include "applic.h" /* Microcontroller applications */ #include "microdef.h" /* Microcontroller Definitions */ #include "microcmd.h" /* Microcontroller commands */ #include "microCmd.h" /* Microcontoller command functions */ #include "pantilt.h" /* Pan & Tilt Microcontroller defines */ #include "microDm.h" /* Micro Data Manager Items & Functions */ #include "microTask.h" /* Microcontroller Task IO definitions */ #include "tiburon.h" /* Tiburon definitions */ #include "panTilt.h" /* Pan & Tilt Can Definitions */ /* Pan & Tilt Device Name */ MLocal char* const panTiltMicroName = "Pan & Tilt"; MLocal char *panTiltChanName[] = /* sio32 serial channel names */ { "/panTilt/port", "/panTilt/stbd" }; MLocal const char *canName[] = { "port", "stbd" }; MLocal const char *panTiltId[] = { "Light Pan", "Light Tilt", "Camera Pan", "Camera Tilt" }; MLocal const char *panTiltFileName[] = { "lightPan", "lightTilt", "cameraPan", "cameraTilt" }; MLocal const char *panTiltDmPrefix[] = { PAN_TILT_PORT_DM_PREFIX, PAN_TILT_STBD_DM_PREFIX }; MLocal const char *ptAxisDmPrefix[] = /* Data Manager Item Name Prefix */ { LIGHT_PAN_DM, LIGHT_TILT_DM, CAMERA_PAN_DM, CAMERA_TILT_DM, }; MLocal const char *panTiltSwitchDmName[] = { PORT_PAN_TILT_DCON_SWITCH_STATUS_DM, STBD_PAN_TILT_DCON_SWITCH_STATUS_DM }; MLocal const char *panTiltPowerDmNames[][2] = { { PORT_PAN_TILT_MOTION_PWR_REQ_DM, PORT_PAN_TILT_MOTION_PWR_OK_DM }, { STBD_PAN_TILT_MOTION_PWR_REQ_DM, STBD_PAN_TILT_MOTION_PWR_OK_DM }, }; /* Initialize Pan & Tilt Data Manager Items*/ #if 0 MLocal const Flt32 softPtLimits[2][4][2] = { { { -60, 130 }, /* Port light pan */ { -28, 35 }, /* Port light tilt */ { -67, 120 }, /* Port camera pan */ { -20, 22 } }, /* Port camera tilt */ { { -112, 67 }, /* Stbd light pan */ { -30, 33 }, /* Stbd light tilt */ { -130, 55 }, /* Stbd camera pan */ { -23, 30 } } /* Stbd camera tilt */ }; #endif /* new limit switch values MBM 12.5.97 */ MLocal const Flt32 softPtLimits[2][4][2] = { { { -60, 150 }, /* Port light pan */ { -40, 40 }, /* Port light tilt */ { -100, 120 }, /* Port camera pan */ { -40, 50 } }, /* Port camera tilt */ { { -150, 70 }, /* Stbd light pan */ { -40, 40 }, /* Stbd light tilt */ { -130, 60 }, /* Stbd camera pan */ { -30, 50 } } /* Stbd camera tilt */ }; #define PT_POSITION_DM_PERIOD 500000 /* Analog Position update Period 2Hz */ #define GF_DM_UPDATE_PERIOD 1000000 /* GF Current Update Period 1Hz */ #define HUMIDITY_DM_PERIOD 10000000 /* Humidity sensor period .1Hz */ #define PAN_TILT_IN_STACKSIZE 5000 /* Pan & Tilt Input Task size & TCB */ #define PAN_TILT_OUT_STACKSIZE 8000 /* Pan & Tilt Output Task size & TCB */ #define CARRIAGE_RET 0x0d #define POWER_REQUEST_TIMEOUT 12 /* 200 mSec timeout for power request */ /* Pan/Tilts will stop if KA's Stop */ #define KEEP_ALIVE_COUNT 3 /* Stop KA Packets after 1 second */ #define KEEP_ALIVE_TICKS KEEP_ALIVE_TIME #define NUM_TASKS 3 /* Number of co-operating tasks */ typedef struct { DM_Group dmGroup; /* Pan & Tilt Data Manager Group */ DWord panTiltSwitchBit; /* Pan/Tilt Power Switch item changed bit*/ DWord resetEventBit; /* Microcontroller reset event bit */ DWord resetCmdBit; /* Microcontroller reset item changed bit*/ DWord gndFltTestBit; /* Ground Fault test item changed bit */ DWord humidityWarnBit; /* Humidity Warning item changed bit */ DWord humidityAlarmBit; /* Humidity Alarm item changed bit */ DWord tempWarnBit; /* Temperature Warning item changed bit */ DWord tempAlarmBit; /* Temperature Alarm item changed bit */ DWord stepperTempWarnBit; /* Stepper Temp Warning item changed bit */ DWord stepperTempAlarmBit; /* Stepper Temp Alarm item changed bit */ DWord velocityAllCmdBit; /* Commanded velocities item change bit */ } ptMicroGroup; typedef struct { DM_Item ctrlFault; /* Controller reported a fault */ DM_Item moveError; /* Pan/Tilt axis not responding to move */ DM_Item zeroPosError; /* Pan/Tilt zero calibration failed */ DM_Item analogDegs; /* Analog position feedback */ DM_Item stepperDegs; /* Stepper Controller position feedback */ DM_Item analogPosReq; /* Requests Analog Position Update */ DM_Item stepperPosReq; /* Requests Stepper Position Update */ DM_Item cwLimit; /* Clockwise limit switch closed */ DM_Item ccwLimit; /* Counter Clockwise limit switch closed*/ DM_Item velocityCmd; /* Commanded pan/tilt velocity */ DM_Item positionCmd; /* Commanded pan/tilt position */ DM_Item initPtCtrl; /* Initialize Pt Controller Command */ DM_Item resetPtCtrl; /* Reset Pan/Tilt Stepper Controller */ DM_Item homeToZero; /* Home Pan/Tilt to Zero Command */ DM_Item ctrlString; /* Write String to controller */ DM_Item writePtNVRam; /* Write Parameters to Controller NV Ram*/ DM_Item holdCurrent; /* Hold Current Parameter */ DM_Item runCurrent; /* Run Current Parameter */ DM_Item settlingTime; /* Settling Time Parameter */ DM_Item accelSlope; /* Accerlation Slope Parameter */ DM_Item decelSlope; /* Deceleration Slope Parameter */ DM_Item initialVel; /* Initial Velocity Parameter */ DM_Item slewVelocity; /* Slew Velocity Parameter */ DM_Item cwLimitPos; /* Clockwise limit position */ DM_Item ccwLimitPos; /* Counter Clockwise limit position */ } panTiltItems; typedef struct { DM_Group dmGroup; /* Pan/Tilt axis specific DM Group */ DWord initPtCtrlBit; DWord resetPtCtrlBit; DWord homeToZeroBit; DWord ctrlStringBit; DWord analogPosReqBit; DWord stepperPosReqBit; DWord holdCurrentBit; DWord runCurrentBit; DWord accelSlopeBit; DWord decelSlopeBit; DWord settlingTimeBit; /* Pan/Tilt settling time changed bit */ DWord cwLimitPosBit; DWord ccwLimitPosBit; DWord initialVelBit; DWord slewVelocityBit; DWord velocityCmdBit; DWord positionCmdBit; DWord writeNVRamBit; /* NV RAM write cmd item changed */ } ptOutGroup; typedef struct /* Pan & Tilt Data Manager Item Handles */ { microDmItems microDm; /* Microcontroller Data Manager Items */ DM_Item initializing; /* Software Initializing Status */ DM_Item canWaterAlarm; /* Housing Water Alarm Detected */ DM_Item localGfCurrent; /* Local Ground Fault Current */ DM_Item extGfCurrent; /* External Ground Fault Current */ DM_Item localGndFaultAlarm; /* Local Ground Fault Alarm */ DM_Item extGndFaultAlarm; /* External Ground Fault Alarm */ DM_Item gndFaultTestStatus; /* Ground Fault Test Status */ DM_Item gndFaultTest; /* Ground Fault Test Enable */ DM_Item microHumidity; /* Humidity Sensor Value % RH */ DM_Item humidityAlarm; /* Humidity Alarm Status */ DM_Item humidityWarnThresh; /* Humidity Warn Threshold */ DM_Item humidityAlarmThresh; /* Humidity Alarm Threshold */ DM_Item microTemperature; /* Temperature sensor value deg C */ DM_Item temperatureAlarm; /* Temperature Alarm Status */ DM_Item tempWarnThresh; /* Temperature Warn Threshold */ DM_Item tempAlarmThresh; /* Temperature Alarm Threshold */ DM_Item stepperTemp; /* Stepper Ctrl Temp sensor value */ DM_Item stepperTempAlarm; /* Stepper Ctrl Temp Alarm Status */ DM_Item stepperTempWarnThresh; /* Stepper Ctrl Temp Warn Threshold */ DM_Item stepperTempAlarmThresh; /* Stepper Ctrl Temp Alarm Threshold */ DM_Item velocityAllCmd; /* All commanded pan/tilt velocities */ panTiltItems ptItems[PT_AXIS_COUNT]; } panTiltDmItems; typedef struct { DM_Item powerReqDm; /* Power Manager Power Request Item */ DM_Item powerOkDm; /* Power Manager Power OK Item */ SEM_ID powerReqSem; /* Power Manager Wake up semaphore */ Nat16 power[PT_AXIS_COUNT]; /* Last Power Request made */ } panTiltPowerMgrStruct; /****************************************************************************/ Void panTiltInTask(Reg panTiltDmItems *dmItems, microIOControl *microIOCtrl); Void panTiltOutTask(Reg panTiltDmItems* dmItems, microIOControl *microIOCtrl, panTiltCanId can); MLocal Word processPanTiltSRQ( Reg panTiltDmItems* dmItems, microIOControl *microIOCtrl, panTiltCanId can); MLocal STATUS panTiltDmItemInit( panTiltDmItems* dmItems, sio32Chan* panTiltDataChan ); MLocal STATUS createPanTiltAxisDmItems( Nat16 panTilt, panTiltItems *dmItems, const char *panTiltDmPrefix, const char* panTiltName ); MLocal STATUS calibratePanTilt( sio32Chan *panTiltDataChan, Nat16 axis ); MLocal STATUS writePtCtrlString(sio32Chan *ptDataChan, Nat16 axis, DM_Item ctrlStringDm); MLocal STATUS sendPanTiltCommand( sio32Chan *sio32DataChan, Nat16 axis, char *ptCommand ); MLocal STATUS downloadInterpolationTable( sio32Chan *sio32DataChan, panTiltCanId can, Nat16 axis); /****************************************************************************/ /* Function : updateWaterAlarmDm */ /* Purpose : Reads micro Water Alarm status & updates dmgr items */ /* Inputs : Sio32 chan & water alarm status data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateWaterAlarmDm( sio32Chan *ptDataChan, DM_Item canWaterAlarmDm ) { Word waterAlarmStatus; /* Microcontroller Water Alarm Status */ /* Read Water Alarm Status */ if ( (microCommand(ptDataChan, PAN_TILT_APP | GET_WATER_ALARM, &waterAlarmStatus) ) == ERROR) return (ERROR); /* Map from a word to a boolean */ waterAlarmStatus = (waterAlarmStatus ? TRUE : FALSE); /* Write water alarm status to Dmgr */ return (writeDmItem(canWaterAlarmDm, &waterAlarmStatus, sizeof(waterAlarmStatus))); } /* updateWaterAlarmDm() */ /****************************************************************************/ /* Function : ptGetGndFltAlarm */ /* Purpose : Returns Groud Fault alarm status from pan/tilt micro */ /* Inputs : Sio32 channel, pointer to Ground Fault Alarm status */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS ptGetGndFltAlarm( sio32Chan *panTiltDataChan, gfStatus *localGfAlarm, gfStatus *extGfAlarm) { Int16 cmdStatus; /* Command Status */ Word reply[3]; /* Command Result from microcontroller */ Word localGfStatus, extGfStatus; /* Get Ground Fault Alarm Status */ /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, MICRO_APP | GET_GNDFLT_STATUS ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update GF Status */ wordsFromBuf(reply, 3, &cmdStatus, &localGfStatus, &extGfStatus); /* Map to vxWorks ENUM definition */ *localGfAlarm = (gfStatus) localGfStatus; *extGfAlarm = (gfStatus) extGfStatus; return(OK); } /* ptGetGndFltAlarm() */ /****************************************************************************/ /* Function : updateGndFltAlarmDm */ /* Purpose : Reads Ground Fault alarm status & updates dmgr item */ /* Inputs : Sio32 chan & Ground Fault alarm data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateGndFltAlarmDm( sio32Chan *sio32DataChan, DM_Item localGfAlarmDm, DM_Item extGfAlarmDm ) { gfStatus localGfAlarm; /* Local Ground Fault alarm */ gfStatus extGfAlarm; /* External Ground Fault alarm */ if (ptGetGndFltAlarm(sio32DataChan, &localGfAlarm, &extGfAlarm) != OK) return(ERROR); /* Write Ground Fault alarm to Dmgr */ writeDmItem(localGfAlarmDm, &localGfAlarm, sizeof(localGfAlarm)); writeDmItem(extGfAlarmDm, &extGfAlarm, sizeof(extGfAlarm)); return (OK); } /* updateGndFltAlarmDm() */ /****************************************************************************/ /* Function : updateGndFltTestDm */ /* Purpose : Reads Ground Fault Test status & updates dmgr item */ /* Inputs : Sio32 chan & Ground Fault test data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateGndFltTestDm( sio32Chan *sio32DataChan, DM_Item gndFltTestDm ) { Word gndFltTest; gfTestStatus gfTest; /* Ground Fault alarm status */ /* Get GF Test Status from micro */ if (microCommand(sio32DataChan, MICRO_APP | GET_GNDFLT_TEST, &gndFltTest) !=OK) return(ERROR); gfTest = (gfTestStatus) gndFltTest; /* Map to vxWorks definition */ /* Write Ground Fault alarm to Dmgr */ return(writeDmItem(gndFltTestDm, &gfTest, sizeof(gfTest))); } /* updateGndFltTestDm() */ /****************************************************************************/ /* Function : ptGetGndFltCurrents */ /* Purpose : Returns Groud Fault current measurements from micro */ /* Inputs : Sio32 channel, pointer to local and ext GF currents */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS ptGetGndFltCurrents( sio32Chan *panTiltDataChan, Int16 *localGfCurrent, Int16 *extGfCurrent) { Int16 cmdStatus; /* Command Status */ Word reply[3]; /* Command Result from microcontroller */ Word localGf, extGf; /* Get Ground Fault Currents from micro */ /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, MICRO_APP | GET_GNDFLT_VALUE ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update GF currents */ wordsFromBuf(reply, 3, &cmdStatus, &localGf, &extGf); *localGfCurrent = (Int16) localGf; *extGfCurrent = (Int16) extGf; return(OK); } /* ptGetGndFltCurrents() */ /****************************************************************************/ /* Function : updateGndFltCurrentsDm */ /* Purpose : Reads Ground Fault currents & updates dmgr items */ /* Inputs : Sio32 chan & Ground Fault current item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateGndFltCurrentsDm( sio32Chan *sio32DataChan, DM_Item localGfCurrentDm, DM_Item extGfCurrentDm ) { Int16 localGfCurrent; /* Local Ground Fault Current */ Int16 extGfCurrent; /* External Ground Fault Current */ if (ptGetGndFltCurrents(sio32DataChan, &localGfCurrent, &extGfCurrent) != OK) return(ERROR); /* Write Ground Fault current to DM */ writeDmItem(localGfCurrentDm, &localGfCurrent, sizeof(localGfCurrent)); writeDmItem(extGfCurrentDm, &extGfCurrent, sizeof(extGfCurrent)); return (OK); } /* updateGndFltCurrentDm() */ /****************************************************************************/ /* Function : initHumidityThreshDm */ /* Purpose : Read Humidity Thresholds and update data manager item */ /* Inputs : Sio32 channel, humidity alarm threshold dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS initHumidityThreshDm( sio32Chan *panTiltDataChan, DM_Item humidityWarnDm, DM_Item humidityAlarmDm ) { Int16 warnThresh; /* Humidity warning threshold */ Int16 alarmThresh; /* Humidity alarm Threshold */ Int16 cmdStatus; /* Command Status */ Word reply[3]; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, PAN_TILT_APP | GET_HUMIDITY_THRESH ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update currents */ wordsFromBuf(reply, 3, &cmdStatus, &warnThresh, &alarmThresh); /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem( humidityAlarmDm, &alarmThresh, sizeof(alarmThresh)); return (urgentWriteDmItem(humidityWarnDm, &warnThresh, sizeof(warnThresh))); } /* initHumidityThreshDm() */ /****************************************************************************/ /* Function : setHumidityThresh */ /* Purpose : Sets humidity alarm threshold values for 3x5 micro */ /* Inputs : Sio32 channel, humidity alarm threshold value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ MLocal STATUS setHumidityThresh(sio32Chan *panTiltDataChan, Int16 warnThresh, Int16 alarmThresh ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &status, sizeof(status), 3, PAN_TILT_APP | SET_HUMIDITY_THRESH, warnThresh, alarmThresh ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* setHumidityThresh() */ /****************************************************************************/ /* Function : writeHumidityThresh */ /* Purpose : Get humidity alarm thresholds from dm and send to micro */ /* Inputs : Sio32 channel, humidity threshold dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writeHumidityThresh( sio32Chan *panTiltDataChan, DM_Item humidityWarnDm, DM_Item humidityAlarmDm ) { Int16 warnThresh; /* Humidity warning threshold */ Int16 alarmThresh; /* Humidity alarm Threshold */ Errno status; /* Result of dm_read */ /* Read Humidity Thresholds from dm */ status = dm_read( humidityWarnDm, (char *) &warnThresh, sizeof(warnThresh), (DM_Time *) NULL); status = dm_read( humidityAlarmDm, (char *) &alarmThresh, sizeof(alarmThresh), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) /* Send command to microcontroller */ return (setHumidityThresh(panTiltDataChan, warnThresh, alarmThresh)); return(ERROR); } /* writeHumidityThresh() */ /****************************************************************************/ /* Function : updateHumidityDm */ /* Purpose : Reads micro humidity sensor value & updates dmgr item */ /* Inputs : Sio32 chan & humidity sensor data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateHumidityDm( sio32Chan *panTiltDataChan, DM_Item microHumidityDm ) { Int16 humidity; /* Micro Humidity Sensor Value */ /* Read humidity sensor value */ if (microCommand(panTiltDataChan, PAN_TILT_APP | GET_HUMIDITY, &humidity) == ERROR) return (ERROR); /* Write humidity to Data Manager */ return (writeDmItem(microHumidityDm, &humidity, sizeof(humidity))); } /* updateHumidityDm() */ /****************************************************************************/ /* Function : updateHumidityAlarmDm */ /* Purpose : Reads humidity alarm status & update dmgr item */ /* Inputs : Sio32 chan & humidity alarm data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateHumidityAlarmDm( sio32Chan *panTiltDataChan, DM_Item humidityAlarmDm ) { Word alarmStatus; /* Microcontroller Alarm Status Word */ microAlarm humidityAlarmStatus; /* Micro humidity alarm status */ /* Get Humidity Alarm Status from micro */ if (microCommand(panTiltDataChan, PAN_TILT_APP | GET_HUMIDITY_ALARM, &alarmStatus) == ERROR) return(ERROR); humidityAlarmStatus = (microAlarm) alarmStatus; /* Write humidity alarm to Dmgr */ return(writeDmItem(humidityAlarmDm, &humidityAlarmStatus, sizeof(humidityAlarmStatus))); } /* updateHumidityAlarmDm() */ /****************************************************************************/ /* Function : initTemperatureThreshDm */ /* Purpose : Read Temperature Thresholds and update data manager item */ /* Inputs : Sio32 channel, temperature alarm threshold dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS initTemperatureThreshDm( sio32Chan *panTiltDataChan, DM_Item temperatureWarnDm, DM_Item temperatureAlarmDm ) { Int16 warnThresh; /* Humidity warning threshold */ Int16 alarmThresh; /* Humidity alarm Threshold */ Int16 cmdStatus; /* Command Status */ Word reply[3]; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, PAN_TILT_APP | GET_TEMP_ALARM_THRESH ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update currents */ wordsFromBuf(reply, 3, &cmdStatus, &warnThresh, &alarmThresh); /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem( temperatureAlarmDm, &alarmThresh, sizeof(alarmThresh)); return (urgentWriteDmItem(temperatureWarnDm, &warnThresh, sizeof(warnThresh))); } /* initTemperatureThreshDm() */ /****************************************************************************/ /* Function : writeTemperatureThresh */ /* Purpose : Get temperature alarm thresholds from dm and send to micro */ /* Inputs : Sio32 channel, temperature threshold dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writeTemperatureThresh( sio32Chan *panTiltDataChan, DM_Item temperatureWarnDm, DM_Item temperatureAlarmDm ) { Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ Int16 warnThresh; /* Temperature warning threshold */ Int16 alarmThresh; /* Temperature alarm Threshold */ /* Read Temperature Thresholds from dm */ status = dm_read( temperatureWarnDm, (char *) &warnThresh, sizeof(warnThresh), (DM_Time *) NULL); status = dm_read( temperatureAlarmDm, (char *) &alarmThresh, sizeof(alarmThresh), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 3, PAN_TILT_APP | SET_TEMP_ALARM_THRESH, warnThresh, alarmThresh ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ return(ERROR); } /* writeTemperatureThresh() */ /****************************************************************************/ /* Function : updateTemperatureDm */ /* Purpose : Reads micro temperature sensor value & updates dmgr item */ /* Inputs : Sio32 chan & temperature sensor data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateTemperatureDm( sio32Chan *panTiltDataChan, DM_Item microTempDm ) { Int16 temperature; /* Micro Temperature Sensor Value */ /* Read temperature sensor value */ if (microCommand(panTiltDataChan, PAN_TILT_APP | GET_TEMPERATURE, &temperature) == ERROR) return (ERROR); /* Write temperature to Data Manager */ return (writeDmItem(microTempDm, &temperature, sizeof(temperature))); } /* updateTemperatureDm() */ /****************************************************************************/ /* Function : updateTempAlarmDm */ /* Purpose : Reads micro temperature alarm status & updates dmgr item */ /* Inputs : Sio32 chan & temperature alarm data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateTempAlarmDm( sio32Chan *panTiltDataChan, DM_Item tempAlarmDm ) { Word alarmStatus; /* Microcontroller Alarm Status Word */ microAlarm tempAlarmStatus; /* Micro temperature alarm status */ /* Get TempAlarm Status from micro */ if (microCommand(panTiltDataChan, PAN_TILT_APP | GET_TEMP_ALARM_STATUS, &alarmStatus) == ERROR) return(ERROR); tempAlarmStatus = (microAlarm) alarmStatus; /* Write temperature alarm to Dmgr */ return(writeDmItem(tempAlarmDm, &tempAlarmStatus, sizeof(tempAlarmStatus))); } /* updateTempAlarmDm() */ /****************************************************************************/ /* Function : updateStepperTempDm */ /* Purpose : Reads Pan/Tilt stepper ctrl temperature & updates dmgr item*/ /* Inputs : Sio32 chan & stepper temperature data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateStepperTempDm( sio32Chan *panTiltDataChan, DM_Item stepperTempDm ) { Int16 stepperTemp; /* Stepper Controller Temperature Value */ /* Read stepper temp sensor value */ if (microCommand(panTiltDataChan, PAN_TILT_APP | GET_STEPPER_TEMP, &stepperTemp) == ERROR) return (ERROR); /* Write stepper temp to Data Manager */ return (writeDmItem(stepperTempDm, &stepperTemp, sizeof(stepperTemp))); } /* updateStepperTempDm() */ /****************************************************************************/ /* Function : updateStepperTempAlarmDm */ /* Purpose : Reads Pan/Tilt stepper ctrl temp alarm & updates dmgr item */ /* Inputs : Sio32 chan & stepper temperature data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateStepperTempAlarmDm(sio32Chan *panTiltDataChan, DM_Item stepperTempAlarmDm) { Word alarmStatus; /* Microcontroller Alarm Status Word */ microAlarm stepperTempAlarm; /* Stepper temperature alarm status */ /* Get Stepper Temp Alarm Status */ if (microCommand(panTiltDataChan, PAN_TILT_APP | GET_STEPPER_TEMP_ALARM, &alarmStatus) == ERROR) return(ERROR); stepperTempAlarm = (microAlarm) alarmStatus; /* Write temperature alarm to Dmgr */ return(writeDmItem(stepperTempAlarmDm, &stepperTempAlarm, sizeof(stepperTempAlarm))); } /* updateStepperTempAlarmDm() */ /****************************************************************************/ /* Function : initStepperTempThreshDm */ /* Purpose : Read Stepper Temp Thresholds and update data manager item */ /* Inputs : Sio32 channel, temperature alarm threshold dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS initStepperTempThreshDm( sio32Chan *panTiltDataChan, DM_Item stepperTempWarnDm, DM_Item stepperTempAlarmDm ) { Int16 warnThresh; /* Temperature warning threshold */ Int16 alarmThresh; /* Temperature alarm Threshold */ Int16 cmdStatus; /* Command Status */ Word reply[3]; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, PAN_TILT_APP | GET_STEPPER_TEMP_ALARM_THRESH ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update currents */ wordsFromBuf(reply, 3, &cmdStatus, &warnThresh, &alarmThresh); /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem( stepperTempAlarmDm, &alarmThresh, sizeof(alarmThresh)); return (urgentWriteDmItem(stepperTempWarnDm, &warnThresh, sizeof(warnThresh))); } /* initStepperTempThreshDm() */ /****************************************************************************/ /* Function : writeStepperTempThresh */ /* Purpose : Get stepper temp alarm thresholds from dm and send to micro */ /* Inputs : Sio32 channel, temperature threshold dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writeStepperTempThresh( sio32Chan *panTiltDataChan, DM_Item stepperTempWarnDm, DM_Item stepperTempAlarmDm ) { Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ Int16 warnThresh; /* Temperature warning threshold */ Int16 alarmThresh; /* Temperature alarm Threshold */ /* Read Temperature Thresholds from dm */ status = dm_read( stepperTempWarnDm, (char *) &warnThresh, sizeof(warnThresh), (DM_Time *) NULL); status = dm_read( stepperTempAlarmDm, (char *) &alarmThresh, sizeof(alarmThresh), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Send command to microcontroller */ if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 3, PAN_TILT_APP | SET_STEPPER_TEMP_ALARM_THRESH, warnThresh, alarmThresh ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ return(ERROR); } /* writeStepperTempThresh() */ /****************************************************************************/ /* Function : initializePtCtrl */ /* Purpose : Send command to re-initialize Pan/Tilt Controller hardware */ /* Inputs : Sio32 channel, Pan/Tilt axis */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS initializePtCtrl( sio32Chan *panTiltDataChan, Nat16 axis) { Word cmdStatus; /* Command Result from microcontroller */ if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 2, PAN_TILT_APP | INITIALIZE_PT_CTRL, axis ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* initializePtCtrl() */ /****************************************************************************/ /* Function : resetPtCtrl */ /* Purpose : Send command to reset Pan/Tilt Stepper Controller hardware */ /* Inputs : Sio32 channel, Pan/Tilt axis */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS resetPtCtrl( sio32Chan *panTiltDataChan, Nat16 axis) { Word cmdStatus; /* Command Result from microcontroller */ if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 2, PAN_TILT_APP | RESET_PT_CTRL, axis ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* resetPtCtrl() */ /****************************************************************************/ /* Function : enablePanTiltCtrlCmd */ /* Purpose : Send command to enable/disable Pan/Tilt control */ /* Inputs : Sio32 channel, Pan/Tilt axis, Enable/Disable */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS enablePanTiltCtrlCmd( sio32Chan *panTiltDataChan, Nat16 axis, MBool enable) { Word cmdStatus; /* Command Result from microcontroller */ if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 3, PAN_TILT_APP | SET_PAN_TILT_AUTO_CTRL, axis, enable ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* enablePanTiltCtrlCmd() */ /****************************************************************************/ /* Function : homePtToZero */ /* Purpose : Send command to home Pan/Tilt to Zero Position */ /* Inputs : Sio32 channel, Pan/Tilt axis */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS homePtToZero( sio32Chan *panTiltDataChan, Nat16 axis) { Word cmdStatus; /* Command Result from microcontroller */ if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 2, PAN_TILT_APP | HOME_PT_TO_ZERO_POS, axis ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* homePtToZero() */ /****************************************************************************/ /* Function : writePtCtrlNVRam */ /* Purpose : Send command to write parameters to P/T Controller NV RAM */ /* Inputs : Sio32 channel, Pan/Tilt axis */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtCtrlNVRam(sio32Chan *panTiltDataChan, Nat16 axis) { Word cmdStatus; /* Command Result from microcontroller */ if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 2, PAN_TILT_APP | WRITE_PT_NVRAM, axis ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* writePtCtrlNVRam() */ /****************************************************************************/ /* Function : initPanTiltParamsDm */ /* Purpose : Read Pan/Tilt parameters & update data manager items */ /* Inputs : Sio32 channel, pan/tilt dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS initPanTiltParamsDm( sio32Chan *panTiltDataChan, panTiltItems *ptItems, Nat16 axis ) { Word reply[8]; /* Reply to command */ Int16 cmdStatus; /* Micro command execution status */ Nat16 holdCurrent, runCurrent, settleTime; Nat16 accelSlope, decelSlope, initialVel, slewVel; Int16 cwLimit, ccwLimit; Flt32 cwLimitPos, ccwLimitPos; /* Get parameters from pan/tilt */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 2, PAN_TILT_APP | GET_PAN_TILT_PARAMS, axis ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ wordsFromBuf(reply, 8, &cmdStatus, &holdCurrent, &runCurrent, &settleTime, &accelSlope, &decelSlope, &initialVel, &slewVel); /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem( ptItems->holdCurrent, &holdCurrent, sizeof(holdCurrent)); urgentWriteDmItem( ptItems->runCurrent, &runCurrent, sizeof(runCurrent)); urgentWriteDmItem( ptItems->settlingTime, &settleTime, sizeof(settleTime)); urgentWriteDmItem( ptItems->accelSlope, &accelSlope, sizeof(accelSlope)); urgentWriteDmItem( ptItems->decelSlope, &decelSlope, sizeof(decelSlope)); urgentWriteDmItem( ptItems->initialVel, &initialVel, sizeof(initialVel)); urgentWriteDmItem( ptItems->slewVelocity, &slewVel, sizeof(slewVel)); if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 2, PAN_TILT_APP | GET_PT_LIMIT_POS, axis ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ wordsFromBuf(reply, 3, &cmdStatus, &cwLimit, &ccwLimit); cwLimitPos = ((Flt32) cwLimit) / 100.0; ccwLimitPos = ((Flt32) ccwLimit) / 100.0; /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem(ptItems->cwLimitPos, &cwLimitPos, sizeof(cwLimitPos)); urgentWriteDmItem(ptItems->ccwLimitPos, &ccwLimitPos, sizeof(ccwLimitPos)); return (OK); } /* initPanTiltParamsDm() */ /****************************************************************************/ /* Function : writePtSettlingTime */ /* Purpose : Get settling time from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, settlint time dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtSettlingTime( sio32Chan *panTiltDataChan, DM_Item settlingTimeDm, Nat16 axis) { Errno status; /* Result of dm_read */ Int16 settlingTime; /* Settling Time */ char ptCommand[30]; /* Pan/Tilt Command */ /* Read Settling Time from dm */ status = dm_read( settlingTimeDm, (char *) &settlingTime, sizeof(settlingTime), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Build command string */ sprintf(ptCommand, "E %d", settlingTime); return( sendPanTiltCommand( panTiltDataChan, axis, ptCommand )); } /* if */ return (ERROR); } /* writePtSettlingTime() */ /****************************************************************************/ /* Function : writePtInitialVel */ /* Purpose : Get Initial Velocity from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, initial velocity dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtInitialVel( sio32Chan *panTiltDataChan, DM_Item initialVelDm, Nat16 axis) { Errno status; /* Result of dm_read */ Int16 initialVel; /* Initial Velocity Value */ char ptCommand[20]; /* Pan/Tilt Command */ /* Read Initial Velocity from dm */ status = dm_read( initialVelDm, (char *) &initialVel, sizeof(initialVel), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Build command string */ sprintf(ptCommand, "I %d", initialVel); return( sendPanTiltCommand( panTiltDataChan, axis, ptCommand )); } /* if */ return (ERROR); } /* writePtInitialVel() */ /****************************************************************************/ /* Function : writePtSlewVel */ /* Purpose : Get Slew Velocity from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, slew velocity dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtSlewVel( sio32Chan *panTiltDataChan, DM_Item initialVelDm, Nat16 axis) { Errno status; /* Result of dm_read */ Int16 initialVel; /* Slew Velocity */ char ptCommand[20]; /* Pan/Tilt Command */ /* Read Slew Velocity from dm */ status = dm_read( initialVelDm, (char *) &initialVel, sizeof(initialVel), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Build command string */ sprintf(ptCommand, "V %d", initialVel); return( sendPanTiltCommand( panTiltDataChan, axis, ptCommand )); } /* if */ return (ERROR); } /* writePtSlewVel() */ /****************************************************************************/ /* Function : panTiltVelocityCmd */ /* Purpose : Sends velocity move commands to p/t controller micro */ /* Inputs : Sio32 channel, axis, velocity, power manager structure */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS panTiltVelocityCmd(sio32Chan *panTiltDataChan, Nat16 axis, Int16 velocityCmd) { Int16 ptAxis; Word cmdStatus; /* Command Result from microcontroller */ powerRequest request; /* Power request data structure */ MBool requestOk; /* Result of request from power mgr */ DM_Time requestTime; /* Time of request for Data Manager */ #if 0 powerMgr->power[axis] = (velocityCmd == 0 ? 0 : PAN_TILT_MOTION_POWER); /* Fill in power request structure */ request.bus = A48; /* Bus power is to be drawn from */ request.division = HOTEL; /* Load division catagory */ request.power = 0; /* Calc power based on axies in motion */ for (ptAxis = 0; ptAxis < PT_AXIS_COUNT; ptAxis++) request.power += powerMgr->power[ptAxis]; /* Fill in time of request for DMgr */ gettimeofday(&requestTime, (struct timezone *) NULL); /* Send power request to power mgr */ dm_write( powerMgr->powerReqDm, &request, sizeof(request), &requestTime ); /* Wait for power manager to respond */ /* or timeout & read response */ if ( (semTake(powerMgr->powerReqSem, POWER_REQUEST_TIMEOUT) != OK ) || (dm_read(powerMgr->powerOkDm, &requestOk, sizeof(requestOk), (DM_Time *) NULL) != SUCCESS ) ) requestOk = ERROR; if (requestOk != TRUE) return (ERROR); #endif if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 3, PAN_TILT_APP | VELOCITY_MOVE, axis, velocityCmd ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* panTiltVelocityCmd() */ /****************************************************************************/ /* Function : writePtVelocityCmd */ /* Purpose : Get Velocity command from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, velocity command dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtVelocityCmd(sio32Chan *panTiltDataChan, DM_Item velCmdDm, Nat16 axis) { Errno status; /* Result of dm_read */ Int16 velocityCmd; /* Velocity Command Value */ /* Read Velocity from dm */ status = dm_read(velCmdDm, (char *) &velocityCmd, sizeof(velocityCmd), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { return (panTiltVelocityCmd(panTiltDataChan, axis, velocityCmd)); } /* if */ return(ERROR); } /* writePtVelocityCmd() */ /****************************************************************************/ /* Function : ptVelocityMoveCmd */ /* Purpose : Get Velocity command from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, velocity command dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS ptVelocityMoveCmd(sio32Chan *panTiltDataChan, panTiltDmItems *dmItems) { Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ Int16 panTilt; Int16 velocityCmd[PT_AXIS_COUNT]; /* Velocity Command Value */ for (panTilt = LIGHT_PAN; panTilt <= CAMERA_TILT; panTilt++) /* Read Velocity from dm */ status = dm_read(dmItems->ptItems[panTilt].velocityCmd, (char *) &velocityCmd[panTilt], sizeof(velocityCmd[panTilt]), (DM_Time *) NULL); if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 5, PAN_TILT_APP | VELOCITY_MOVE_ALL, velocityCmd[LIGHT_PAN], velocityCmd[LIGHT_TILT], velocityCmd[CAMERA_PAN], velocityCmd[CAMERA_TILT]) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* ptVelocityMoveCmd() */ /****************************************************************************/ /* Function : ptVelocityMoveWithFeedbackCmd */ /* Purpose : Get Velocity command from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, velocity command dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS ptVelocityMoveWithFeedbackCmd(sio32Chan *panTiltDataChan, panTiltDmItems *dmItems) { STATUS status; Nat16 axis; /* Pan & tilt axis index */ Word reply[PT_AXIS_COUNT + 1]; /* Reply to command */ Word cmdStatus; /* Command Result from micro */ Int16 velCmd[PT_AXIS_COUNT]; /* Velocity Command Value */ Int16 positions[PT_AXIS_COUNT]; /* Pan/Tilt analog positions */ Flt32 positionsFlt[PT_AXIS_COUNT]; /* Floating point positions */ /* Read Velocity values from DM */ status = dm_read(dmItems->velocityAllCmd, (char *) velCmd, sizeof(velCmd), (DM_Time *) NULL); /* Check for successful dm_read */ if (!((status == SUCCESS) || (status == EDM_NOPROVIDER))) return (ERROR); /* Send velocity commands and read */ /* back analog position data */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), PT_AXIS_COUNT + 1, PAN_TILT_APP | VELOCITY_MOVE_WITH_FB, velCmd[LIGHT_PAN], velCmd[LIGHT_TILT], velCmd[CAMERA_PAN], velCmd[CAMERA_TILT]) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ wordsFromBuf(reply, 5, &cmdStatus, &positions[0], &positions[1], &positions[2], &positions[3] ); for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) { positionsFlt[axis] = ((Flt32) positions[axis]) / 100.0; writeDmItem(dmItems->ptItems[axis].analogDegs, &positionsFlt[axis], sizeof(Flt32) ); } /* for */ return (OK); } /* ptVelocityMoveWithFeedbackCmd() */ LOCAL STATUS ptClearVelocityAllDmItem(panTiltDmItems *dmItems) { Int16 velCmd[PT_AXIS_COUNT] = { 0 }; /* Velocity Command Value */ return (urgentWriteDmItem(dmItems->velocityAllCmd, (void *) velCmd, sizeof(velCmd))); } /* ptClearVelocityAllDmItem() */ /****************************************************************************/ /* Function : writePtPositionCmd */ /* Purpose : Get Position command from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, velocity command dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtPositionCmd(sio32Chan *panTiltDataChan, DM_Item velCmdDm, Nat16 axis) { Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ Flt32 positionCmd; /* Position Command Value */ /* Read Position from dm */ status = dm_read( velCmdDm, (char *) &positionCmd, sizeof(positionCmd), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 3, PAN_TILT_APP | POSITION_MOVE, axis, (Int16) (positionCmd * 100.0) ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ return(ERROR); } /* writePtPositionCmd() */ /****************************************************************************/ /* Function : writePtCurrents */ /* Purpose : Get run/hold currents from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, run/hold current dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtCurrents( sio32Chan *panTiltDataChan, DM_Item holdCurrentDm, DM_Item runCurrentDm, Nat16 axis) { Errno status; /* Result of dm_read */ Int16 runCurrent; /* Run Current value */ Int16 holdCurrent; /* Hold Current value */ char ptCommand[20]; /* Pan/Tilt Command */ /* Read Currents from dm */ status = dm_read( holdCurrentDm, (char *) &holdCurrent, sizeof(holdCurrent), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { status = dm_read( runCurrentDm, (char *) &runCurrent, sizeof(runCurrent), (DM_Time *) NULL); if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Build command string */ sprintf(ptCommand, "Y %d %d", holdCurrent, runCurrent); return( sendPanTiltCommand( panTiltDataChan, axis, ptCommand )); } /* if */ } /* if */ return (ERROR); } /* writePtCurrents() */ /****************************************************************************/ /* Function : writePtSlopes */ /* Purpose : Get accel/decel slopes from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, accel/decel slope dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtSlopes( sio32Chan *panTiltDataChan, DM_Item accelSlopeDm, DM_Item decelSlopeDm, Nat16 axis) { Errno status; /* Result of dm_read */ Int16 accelSlope; /* Run Current value */ Int16 decelSlope; /* Hold Current value */ char ptCommand[20]; /* Pan/Tilt Command */ /* Read Currents from dm */ status = dm_read( accelSlopeDm, (char *) &accelSlope, sizeof(accelSlope), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { status = dm_read( decelSlopeDm, (char *) &decelSlope, sizeof(decelSlope), (DM_Time *) NULL); if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Build command string */ sprintf(ptCommand, "K %d %d", accelSlope, decelSlope); return( sendPanTiltCommand( panTiltDataChan, axis, ptCommand )); } /* if */ } /* if */ return (ERROR); } /* writePtSlopes() */ /****************************************************************************/ /* Function : writePtLimitPos */ /* Purpose : Get Limit Positions from dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, Limit Positions dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writePtLimitPos( sio32Chan *panTiltDataChan, DM_Item cwLimitPosDm, DM_Item ccwLimitPosDm, Nat16 axis) { Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ Flt32 cwLimitPos; /* Clockwise Limit Position */ Flt32 ccwLimitPos; /* Counter Clockwise Limit Position */ /* Read Currents from dm */ status = dm_read( cwLimitPosDm, (char *) &cwLimitPos, sizeof(cwLimitPos), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { status = dm_read( ccwLimitPosDm, (char *) &ccwLimitPos, sizeof(ccwLimitPos), (DM_Time *) NULL); if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { if ( sendMicroCommand(panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 4, PAN_TILT_APP | SET_PT_LIMIT_POS, axis, (Int16) (cwLimitPos * 100), (Int16) (ccwLimitPos *100)) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ } /* if */ return (ERROR); } /* writePtLimitPos() */ /****************************************************************************/ /* Function : setPtLimitPositions */ /* Purpose : Write Limit Positions to dm and send to pan/tilt unit */ /* Inputs : Sio32 channel, Limit Positions dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS setPtLimitPositions( sio32Chan *panTiltDataChan, DM_Item cwLimitPosDm, DM_Item ccwLimitPosDm, panTiltCanId can, Nat16 axis) { Word cmdStatus; /* Command Result from microcontroller */ Flt32 cwLimitPos; /* Clockwise Limit Position */ Flt32 ccwLimitPos; /* Counter Clockwise Limit Position */ ccwLimitPos = softPtLimits[(can == PAN_TILT_CAN0 ? 0 : 1)][axis][0]; cwLimitPos = softPtLimits[(can == PAN_TILT_CAN0 ? 0 : 1)][axis][1]; urgentWriteDmItem(cwLimitPosDm, &cwLimitPos, sizeof(cwLimitPos)); urgentWriteDmItem(ccwLimitPosDm, &ccwLimitPos, sizeof(ccwLimitPos)); if ( sendMicroCommand(panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 4, PAN_TILT_APP | SET_PT_LIMIT_POS, axis, (Int16) (cwLimitPos * 100), (Int16) (ccwLimitPos * 100)) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* setPtLimitPositions() */ /****************************************************************************/ /* Function : updateStepperPanTiltPositionsDm */ /* Purpose : Reads Pan/Tilt stepper positions & updates dmgr items */ /* Inputs : Sio32 chan & pan & tilt data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateStepperPanTiltPositionsDm( sio32Chan *panTiltDataChan, panTiltDmItems *dmItems ) { Word cmdStatus; /* Command Result from micro */ Word reply[5]; /* Reply to command */ Nat16 axis; /* Pan & tilt axis index */ Int16 positions[PT_AXIS_COUNT]; /* Pan/Tilt stepper positions */ Flt32 positionsFlt[PT_AXIS_COUNT]; /* Floating point positions */ /* Read Pan/Tilt position data */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, PAN_TILT_APP | GET_PT_STEPPER_POSITIONS ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ wordsFromBuf(reply, 5, &cmdStatus, &positions[0], &positions[1], &positions[2], &positions[3] ); for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) { positionsFlt[axis] = ((Flt32) positions[axis]) / 100.0; writeDmItem(dmItems->ptItems[axis].stepperDegs, &positionsFlt[axis], sizeof(Flt32) ); } /* for */ return (OK); } /* updateStepperPanTiltPositionsDm() */ /****************************************************************************/ /* Function : updateAnalogPanTiltPositionsDm */ /* Purpose : Reads analog Pan/Tilt positions & updates dmgr item */ /* Inputs : Sio32 chan & pan & tilt data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateAnalogPanTiltPositionsDm( sio32Chan *panTiltDataChan, panTiltDmItems *dmItems ) { Word cmdStatus; /* Command Result from micro */ Word reply[5]; /* Reply to command */ Nat16 axis; /* Pan & tilt axis index */ Int16 positions[PT_AXIS_COUNT]; /* Pan/Tilt analog positions */ Flt32 positionsFlt[PT_AXIS_COUNT]; /* Floating point positions */ /* Read Pan/Tilt position data */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, PAN_TILT_APP | GET_PT_ANALOG_POSITIONS ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ wordsFromBuf(reply, 5, &cmdStatus, &positions[0], &positions[1], &positions[2], &positions[3] ); for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) { positionsFlt[axis] = ((Flt32) positions[axis]) / 100.0; writeDmItem(dmItems->ptItems[axis].analogDegs, &positionsFlt[axis], sizeof(Flt32) ); } /* for */ return (OK); } /* updateAnalogPanTiltPositionsDm() */ /****************************************************************************/ /* Function : updatePanTiltLimitSwitchDm */ /* Purpose : Reads Pan/Tilt Limit Switch Status & updates dmgr item */ /* Inputs : Sio32 chan & pan & tilt data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updatePanTiltLimitSwitchDm(sio32Chan *panTiltDataChan, panTiltDmItems *dmItems) { Word cmdStatus; /* Command Result from micro */ Word reply[5]; /* Reply to command */ Nat16 panTilt; /* Pan & tilt axis index */ Nat16 limitSw[PT_AXIS_COUNT]; /* Pan/Tilt limit switch status */ /* Read Pan/Tilt position data */ if ( sendMicroCommand( panTiltDataChan, &reply, sizeof(reply), 1, PAN_TILT_APP | GET_PT_LIMIT_SWITCHES ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ wordsFromBuf(reply, 5, &cmdStatus, &limitSw[0], &limitSw[1], &limitSw[2], &limitSw[3] ); for (panTilt = LIGHT_PAN; panTilt <= CAMERA_TILT; panTilt++) { writeBooleanDmItem(dmItems->ptItems[panTilt].cwLimit, (PT_CW_LIMIT(limitSw[panTilt]) ? TRUE : FALSE)); writeBooleanDmItem(dmItems->ptItems[panTilt].ccwLimit, (PT_CCW_LIMIT(limitSw[panTilt]) ? TRUE : FALSE)); } /* for */ return (OK); } /* updatePanTiltLimitSwitchDm() */ /*****************************************************************************/ /* Function : writePtCtrlString */ /* Purpose : Sends command to micro to send string to P/T controller */ /* Inputs : SIO32 serial channel, P/T axis, string to send */ /* Outputs : Returns length of reply or ERROR */ /*****************************************************************************/ MLocal STATUS writePtCtrlString(sio32Chan *panTiltDataChan, Nat16 axis, DM_Item ctrlStringDm) { Errno status; /* Result of dm_read */ Nat16 nBytes; char ctrlString[ CTRL_STRING_LEN]; Byte packet[MAX_PACKET_LEN]; /* Command Result from microcontroller */ /* Read Position from dm */ status = dm_read( ctrlStringDm, ctrlString, CTRL_STRING_LEN, (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { nBytes = strlen(ctrlString) + 1; wordsToBuf(packet, 3, PAN_TILT_APP | CONTROLLER_STRING, axis, nBytes); bcopy( ctrlString, packet + sizeof(Word) * 3, nBytes); return (microWriteBufReply( panTiltDataChan, nBytes + sizeof(Word) * 3, packet)); } /* if */ return (ERROR); } /* writePtCtrlString() */ /****************************************************************************/ /* Function : sendPanTiltCommand */ /* Purpose : Sends a command directly to the specified P/T controller */ /* Inputs : sio32 Serial channel, axis, command string */ /* Outputs : Returns number of bytes sent or ERROR */ /****************************************************************************/ LOCAL STATUS sendPanTiltCommand( sio32Chan *sio32DataChan, Nat16 axis, char *ptCommand ) { char command[30]; /* Buffer for micro command data */ Word cmdStatus; /* Result of microcontroller command*/ STATUS status; /* Return status of read call */ Int16 len; /* put micro command into buffer */ /* Append pan/tilt axis address */ wordsToBuf(command, 2, PAN_TILT_APP | SEND_PAN_TILT_COMMAND, axis); /* Append Pan/Tilt Command String */ strcpy(command + sizeof(Word) * 2, ptCommand); len = strlen(ptCommand) + sizeof(Word) * 2; command[len++] = CARRIAGE_RET; command[len++] = '\0'; return (microWriteBufReply(sio32DataChan, len, command)); } /* sendPanTiltCommand() */ /****************************************************************************/ /* Function : downloadInterpolationTable */ /* Purpose : Sends calibration data to the specified P/T controller */ /* Inputs : sio32 Serial channel, can Id, pan/Tilt axis */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS downloadInterpolationTable( sio32Chan *panTiltDataChan, panTiltCanId can, Nat16 panTilt ) { Word cmdStatus; /* Result of microcontroller command*/ Int32 count, numPoints, ignore; Flt32 analog, ccwLimit, cwLimit; Int16 analogInt, pnt; Byte buffer[MAX_PACKET_LEN]; char calDataFile[132]; FILE* ptCalFd = NULL; /* File descriptor for cal data logging */ sprintf(calDataFile, "%s/%s/%s", PT_CAL_DATA_DIRECTORY, canName[can], panTiltFileName[panTilt]); if ((ptCalFd = fopen(calDataFile, "r")) == NULL) { printf("Error opening cal data file: %s\n", calDataFile); return (ERROR); } /* if */ fscanf(ptCalFd, "%*s%*s%*s%*s%*s%*s%*s%*s%*s%*s%d%*s%f%*s%f", &numPoints, &ccwLimit, &cwLimit); if (numPoints > PT_CAL_TABLE_SIZE) { printf("Too many table entries (%d). Maximum is %d\n", numPoints, PT_CAL_TABLE_SIZE); fclose (ptCalFd); return (ERROR); } /* if */ pnt = 0; while (pnt < numPoints) { wordsToBuf(buffer, 5, PAN_TILT_APP | SEND_INTERP_TABLE, panTilt, pnt, (Int16) (ccwLimit * 100.0), (Int16) (cwLimit * 100.0)); count = 0; while ((count < PT_CAL_ENTRIES_PER_PACKET) && (pnt < numPoints)) { fscanf(ptCalFd, "%d%f\n", &ignore, &analog); analogInt = (Int16) (analog * 100.0); wordToBuf(buffer + (sizeof(Word) * (3 + count)), analogInt); count++, pnt++; } /* while */ microWriteBufReply(panTiltDataChan, sizeof(Word) * (5 + count), buffer); } /* while */ fclose(ptCalFd); if ( sendMicroCommand( panTiltDataChan, &cmdStatus, sizeof(cmdStatus), 3, PAN_TILT_APP | INTERP_TABLE_LOADED, panTilt, TRUE) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* downloadInterpolationTable() */ /****************************************************************************/ /* Function : createPanTiltAppDmItems */ /* Purpose : Create Data Mgr Items specific to Pan & Tilt Application */ /* Inputs : Pointer to structure of DM item handles, Pan & Tilt DM */ /* prefix, Pan & Tilt Microcontroller Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS createPanTiltAppDmItems (panTiltDmItems *dmItems, const char* panTiltAppPrefix, const char* panTiltName ) { Nat16 panTilt; /* Pan & Tilt axis index */ char dmPrefix[80]; /* Data Manager Item prefix for axis */ dmItemList panTiltDmInit[] = { { &dmItems->microDm.resetEvent, "Reset Event", MICRO_RESET_EVENT_DM, DM_EMPTY, 1 }, { &dmItems->microDm.resetCmd, "Reset Command", MICRO_RESET_CMD_DM, DM_EMPTY, 1 }, { &dmItems->microDm.idString, "Identification", MICRO_IDENT_DM, DM_CHAR, MICRO_ID_LEN }, { &dmItems->microDm.serialNo, "Micro Serial Number", MICRO_SERIAL_NO_DM, DM_CHAR, MICRO_SERNO_LEN }, { &dmItems->microDm.softwareRev, "Software Revision", SOFTWARE_REV_DM, DM_CHAR, SOFTWARE_REV_LEN }, { &dmItems->microDm.ramTest, "Micro RAM Test", RAM_TEST_STATUS_DM, DM_MBOOL, 1 }, { &dmItems->initializing, "Pan/Tilt Micro Initializing", INITIALIZING_DM, DM_MBOOL, 1 }, { &dmItems->microTemperature, "Micro Temperature", TEMPERATURE_DM, DM_INT16, 1 }, { &dmItems->temperatureAlarm, "Temperature Alarm", TEMPERATURE_ALARM_DM, DM_ENUM, 1 }, { &dmItems->tempWarnThresh, "Temperature Warn Threshold", TEMP_WARN_THRESH_DM, DM_INT16, 1 }, { &dmItems->tempAlarmThresh, "Temperature Alarm Threshold", TEMP_ALARM_THRESH_DM, DM_INT16, 1 }, { &dmItems->microHumidity, "Humidity Value", HUMIDITY_DM, DM_NAT16, 1 }, { &dmItems->humidityAlarm, "Humidity Alarm", HUMIDITY_ALARM_DM, DM_ENUM, 1 }, { &dmItems->humidityWarnThresh, "Humidity Warn Threshold", HUMIDITY_WARN_THRESH_DM, DM_NAT16, 1 }, { &dmItems->humidityAlarmThresh, "Humidity Alarm Threshold", HUMIDITY_ALARM_THRESH_DM, DM_NAT16, 1 }, { &dmItems->canWaterAlarm, "Housing Water Alarm", HOUSING_WATER_ALARM_DM, DM_MBOOL, 1 }, { &dmItems->localGfCurrent, "Local Ground Fault Current", LOCAL_GND_FLT_CURRENT_DM, DM_INT16, 1 }, { &dmItems->localGndFaultAlarm, "Local Ground Fault Alarm", LOCAL_GND_FLT_ALARM_DM, DM_ENUM, 1 }, { &dmItems->extGfCurrent, "External Ground Fault Current", EXT_GND_FLT_CURRENT_DM, DM_INT16, 1 }, { &dmItems->extGndFaultAlarm, "External Ground Fault Alarm", EXT_GND_FLT_ALARM_DM, DM_ENUM, 1 }, { &dmItems->gndFaultTest, "Ground Fault Test Enable", GND_FLT_TEST_DM, DM_MBOOL, 1 }, { &dmItems->gndFaultTestStatus, "Ground Fault Test Status", GND_FLT_TEST_STATUS_DM, DM_ENUM, 1 }, { &dmItems->stepperTemp, "Stepper Temperature", STEPPER_TEMP_DM, DM_INT16, 1 }, { &dmItems->stepperTempAlarm, "Stepper Temperature Alarm", STEPPER_TEMP_ALARM_DM, DM_ENUM, 1 }, { &dmItems->stepperTempWarnThresh, "Stepper Temp Warn Threshold", STEPPER_TEMP_WARN_THRESH_DM, DM_INT16, 1 }, { &dmItems->velocityAllCmd, "All Commanded Velocities", PT_VELOCITY_ALL_CMD_DM, DM_INT16, PT_AXIS_COUNT }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; if (initMicroDmItemList(panTiltDmInit, panTiltAppPrefix, panTiltName) != OK) return(ERROR); for (panTilt = LIGHT_PAN; panTilt <= CAMERA_TILT; panTilt++) { strcpy(dmPrefix, panTiltAppPrefix); strcat(dmPrefix, ptAxisDmPrefix[panTilt]); createPanTiltAxisDmItems(panTilt, &dmItems->ptItems[panTilt], dmPrefix, dmPrefix); } /* for */ return(OK); } /* createPanTiltAppDmItems() */ /****************************************************************************/ /* Function : createPanTiltAxisDmItems */ /* Purpose : Create Data Mgr Items for a Pan/Tilt Axis */ /* Inputs : Pointer to structure of DM item handles, Pan & Tilt DM */ /* prefix, Pan & Tilt Microcontroller Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS createPanTiltAxisDmItems( Nat16 panTilt, panTiltItems *dmItems, const char* panTiltDmPrefix, const char* panTiltName ) { Int32 item; /* Counter for DM Item Initialization */ Errno status; /* Result of dm_create function call */ struct /* Data Manager Item Table for table */ { /* driven item initialization */ DM_Item *dmItem; /* pointer to actual dm item handle */ char *errorMsg; /* Message to display of dmCreate fails */ char *name; /* Data Manager Item Name */ DM_Type type; /* Data Manager Item Type */ DM_Num len; /* Data Manager Item Element Count */ } panTiltDmInit[] = { { &dmItems->ctrlFault, "Controller Fault", PT_CTRL_FAULT_DM, DM_MBOOL, 1 }, { &dmItems->moveError, "Move Error", PT_MOVE_ERROR_DM, DM_EMPTY, 1 }, { &dmItems->zeroPosError, "Zero Position Error", PT_ZERO_POS_ERROR_DM, DM_EMPTY, 1 }, { &dmItems->analogDegs, "Analog Position", PT_ANALOG_POS_DM, DM_FLT32, 1 }, { &dmItems->stepperDegs, "Stepper Position", PT_STEPPER_POS_DM, DM_FLT32, 1 }, { &dmItems->analogPosReq, "Analog Position Update Request", PT_ANALOG_POS_REQ_DM, DM_EMPTY, 1 }, { &dmItems->stepperPosReq, "Stepper Position Update Request", PT_STEPPER_POS_REQ_DM, DM_EMPTY, 1 }, { &dmItems->holdCurrent, "Hold Current", PT_HOLD_CURRENT_DM, DM_INT16, 1 }, { &dmItems->runCurrent, "Run Current", PT_RUN_CURRENT_DM, DM_INT16, 1 }, { &dmItems->settlingTime, "Settling Time", PT_SETTLING_TIME_DM, DM_INT16, 1 }, { &dmItems->accelSlope, "Acceleration Slope", PT_ACCEL_SLOPE_DM, DM_INT16, 1 }, { &dmItems->decelSlope, "Deceleration Slope", PT_DECEL_SLOPE_DM, DM_INT16, 1 }, { &dmItems->initialVel, "Initial Velocity", PT_INITIAL_VEL_DM, DM_INT16, 1 }, { &dmItems->slewVelocity, "Slew Velocity", PT_SLEW_VEL_DM, DM_INT16, 1 }, { &dmItems->cwLimitPos, "Clockwise Limit", PT_CW_LIMIT_POS_DM, DM_FLT32, 1 }, { &dmItems->ccwLimitPos, "Counter Clockwise Limit", PT_CCW_LIMIT_POS_DM, DM_FLT32, 1 }, { &dmItems->positionCmd, "Commanded Position", PT_POSITION_CMD_DM, DM_FLT32, 1 }, { &dmItems->velocityCmd, "Commanded Velocity", PT_VELOCITY_CMD_DM, DM_INT16, 1 }, { &dmItems->cwLimit, "Clockwise Limit", PT_CW_LIMIT_DM, DM_MBOOL, 1 }, { &dmItems->ccwLimit, "Counter clockwise Limit", PT_CCW_LIMIT_DM, DM_MBOOL, 1 }, { &dmItems->initPtCtrl, "Initialize Pan/Tilt Controller", PT_INITIALIZE_CTRL_DM, DM_EMPTY, 1 }, { &dmItems->homeToZero, "Home Pan/Tilt to Zero", PT_HOME_TO_ZERO_DM, DM_EMPTY, 1 }, { &dmItems->writePtNVRam, "Write Pan/Tilt NV RAM", PT_WRITE_CTRL_NVRAM_DM, DM_EMPTY, 1 }, { &dmItems->resetPtCtrl, "Reset Stepper Motor Controller", PT_RESET_CTRL_DM, DM_EMPTY, 1 }, { &dmItems->ctrlString, "Controller String", PT_CTRL_STRING_DM, DM_CHAR, CTRL_STRING_LEN }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Initialize Pan & Tilt Data Manager Items*/ for (item = 0; panTiltDmInit[item].dmItem != NO_ITEM; item++) { if ((*panTiltDmInit[item].dmItem = initMicroDmItem(panTiltDmPrefix, panTiltDmInit[item].name, panTiltDmInit[item].type, panTiltDmInit[item].len)) == ERROR) { /* Error occurred so print message & exit */ logMsg("%s: Error Initializing %s DM Item\n", panTiltName, panTiltDmInit[item].errorMsg); return(ERROR); } /* if */ } /* for */ return(OK); } /* createPanTiltAxisDmItems() */ /****************************************************************************/ /* Function : createPanTiltPwrManager */ /* Purpose : Create Data Structure for Power Manager Interface to P/T */ /* Inputs : Pointer to power mgr structure and can ID */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS createPanTiltPwrManager(panTiltPowerMgrStruct *panTiltPwrMgr, panTiltCanId can) { Errno status; Int16 ptAxis; for (ptAxis = 0; ptAxis < PT_AXIS_COUNT; ptAxis++) panTiltPwrMgr->power[ptAxis] = 0; status = dm_create((char *) panTiltPowerDmNames[can][0], 1, &panTiltPwrMgr->powerReqDm, DM_NAT16, 1, DM_ENUM, 1, DM_ENUM, 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 Power Ok Data Manager Item */ if ( (panTiltPwrMgr->powerOkDm = initMicroDmItem("", (char *) panTiltPowerDmNames[can][1], DM_MBOOL, 1)) == ERROR) return(ERROR); /* An error occurred so return ERROR */ if ( (panTiltPwrMgr->powerReqSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) return (ERROR); /* Register us as a provider & consumer */ dm_start_provider( panTiltPwrMgr->powerReqDm, DM_STATIC ); dm_start_consumer( panTiltPwrMgr->powerOkDm, DM_STATIC, panTiltPwrMgr->powerReqSem ); return(OK); } /* createPanTiltPwrManager() */ /****************************************************************************/ /* Function : panTiltDmItemInit */ /* Purpose : Read parameters from P/T microcontroller and update DMgr */ /* Inputs : Pointer to DM item handles, P/T micro serial channel */ /* Outputs : None, but data manager items change */ /****************************************************************************/ LOCAL STATUS panTiltDmItemInit(panTiltDmItems* dmItems, sio32Chan* panTiltDataChan) { Int16 tries; Nat16 axis; /* Send Command Enable Command to IBC Micro */ for (tries = 0; ((microCmdEnable(panTiltDataChan) == ERROR) && tries < MICRO_ENABLE_TRYS); tries++) taskDelay(sysClkRateGet() / 5); if (tries == MICRO_ENABLE_TRYS) { logMsg("Pan/Tilt: Error Enabling Pan/Tilt Microcontroller\n"); return(ERROR); } /* if */ /* Set Micro Identification initial value */ if (updateMicroIdentDm(panTiltDataChan, dmItems->microDm.idString) == ERROR) logMsg("Pan/Tilt: Error reading Identification String\n"); /* Set Micro Serial Number initial value */ if (updateSerialNoDm(panTiltDataChan, dmItems->microDm.serialNo) == ERROR) logMsg("Pan/Tilt: Error reading Micro Serial Number\n"); /* Set Software Revision initial value */ if (updateSoftwareRevDm(panTiltDataChan, dmItems->microDm.softwareRev) == ERROR) logMsg("Pan/Tilt: Error reading Software Revision\n"); /* Set Micro Ram Test Result value */ if (updateMicroRamTestDm(panTiltDataChan, dmItems->microDm.ramTest) == ERROR) logMsg("Pan/Tilt: Error checking micro RAM Test\n"); /* Set Micro Temperature Alarm initial value */ if (updateTempAlarmDm(panTiltDataChan, dmItems->temperatureAlarm) == ERROR) logMsg("Pan/Tilt: Error checking temperature alarm status\n"); /* Set Stepper Temp Alarm initial value */ if (updateStepperTempAlarmDm(panTiltDataChan, dmItems->stepperTempAlarm) == ERROR) logMsg("Pan/Tilt: Error checking stepper temp alarm status\n"); if (setHumidityThresh(panTiltDataChan, ROV_HUMIDITY_WARN_THRESH, ROV_HUMIDITY_ALARM_THRESH ) == ERROR) logMsg("Pan/Tilt: Error setting humidity alarm thresholds\n"); /* Set Humidity Alarm initial value */ if (updateHumidityAlarmDm(panTiltDataChan, dmItems->humidityAlarm) == ERROR) logMsg("Pan/Tilt: Error checking humidity alarm status\n"); /* Set Micro Water Alarm initial value */ if (updateWaterAlarmDm(panTiltDataChan, dmItems->canWaterAlarm) == ERROR) logMsg("Pan/Tilt: Error checking water alarm status\n"); /* Set Ground Fault Alarm initial values */ if (updateGndFltAlarmDm(panTiltDataChan, dmItems->localGndFaultAlarm, dmItems->extGndFaultAlarm) == ERROR) logMsg("Pan/Tilt: Error checking ground fault alarm status\n"); /* Set Ground Fault Test initial values */ if (updateGndFltTestDm(panTiltDataChan, dmItems->gndFaultTestStatus) == ERROR) logMsg("Pan/Tilt: Error checking ground fault test status\n"); for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) { if ( initPanTiltParamsDm(panTiltDataChan, &dmItems->ptItems[axis], axis) == ERROR ) { logMsg("Pan/Tilt: Error reading parameters for %s\n", panTiltId[axis]); } /* if */ } /* for */ updatePanTiltLimitSwitchDm(panTiltDataChan, dmItems); return(OK); } /* panTiltDmItemInit() */ /****************************************************************************/ /* Function : panTiltTask */ /* Purpose : Initializes communication with Pan & Tilt Microcontroller */ /* Provides interface between Pan & Tilt and Data Manager */ /* Inputs : SIO32 serial channel number for Pan & Tilt Microcontrller */ /* Physically maps the Pan & Tilt to a serial channel */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS panTiltTask( panTiltCanId can, Nat16 serialChannel ) { panTiltDmItems dmItems; /* Pan & Tilt Application Data Mgr items */ microIOControl microIOCtrl; /* Microcontroller IO Task control */ Nat16 panTilt; /* Pan & Tilt axis index */ /* Initialize serial channel and task */ /* synchronization resources */ if (initMicroTaskIO( µIOCtrl, NUM_TASKS, serialChannel, panTiltDmPrefix[can], panTiltChanName[can], panTiltSwitchDmName[can]) == ERROR) { logMsg("Error initializing IO for %s pan/tilts", canName[can]); return(ERROR); } /* if */ sio32ChanSetRxTimeout(µIOCtrl.sio32DataChan, sysClkRateGet() * 5); /* Clear Pan & Tilt Data Manager handles */ bzero((char *)&dmItems, sizeof(dmItems)); /* Create Pan & Tilt Application Items */ if (createPanTiltAppDmItems(&dmItems, panTiltDmPrefix[can], panTiltMicroName) == ERROR) { /* Error occurred so print message & exit */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ /* Start Provider for Static Data & Alarms*/ dm_start_provider(dmItems.microDm.resetEvent, DM_STATIC); dm_start_provider(dmItems.microDm.idString, DM_STATIC); dm_start_provider(dmItems.microDm.serialNo, DM_STATIC); dm_start_provider(dmItems.microDm.softwareRev, DM_STATIC); dm_start_provider(dmItems.microDm.ramTest, DM_STATIC); dm_start_provider(dmItems.initializing, DM_STATIC); dm_start_provider(dmItems.canWaterAlarm, DM_STATIC); dm_start_provider(dmItems.localGndFaultAlarm, DM_STATIC); dm_start_provider(dmItems.extGndFaultAlarm, DM_STATIC); dm_start_provider(dmItems.gndFaultTestStatus, DM_STATIC); dm_start_provider(dmItems.humidityAlarm, DM_STATIC); dm_start_provider(dmItems.temperatureAlarm, DM_STATIC); dm_start_provider(dmItems.stepperTempAlarm, DM_STATIC); for (panTilt = LIGHT_PAN; panTilt <= CAMERA_TILT; panTilt++) { dm_start_provider(dmItems.ptItems[panTilt].ctrlFault, DM_STATIC); dm_start_provider(dmItems.ptItems[panTilt].moveError, DM_STATIC); dm_start_provider(dmItems.ptItems[panTilt].zeroPosError, DM_STATIC); dm_start_provider(dmItems.ptItems[panTilt].cwLimit, DM_STATIC); dm_start_provider(dmItems.ptItems[panTilt].ccwLimit, DM_STATIC); } /* for */ /* Initialize Pan & Tilt Data Output Task */ if ((microIOCtrl.childTasks[OUT_TASK].taskId = taskSpawn( "panTiltOut", PAN_TILT_OUT_PRIORITY, VX_FP_TASK, PAN_TILT_OUT_STACKSIZE, (FUNCPTR) panTiltOutTask, (int) &dmItems, (int) µIOCtrl, can, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); logMsg("Pan/Tilt: Error spawning data output task\n"); return(ERROR); } /* if */ /* Initialize Pan & Tilt Data Input Task */ if ((microIOCtrl.childTasks[IN_TASK].taskId = taskSpawn( "panTiltIn", PAN_TILT_IN_PRIORITY, VX_FP_TASK, PAN_TILT_IN_STACKSIZE, (FUNCPTR) panTiltInTask, (int) &dmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); logMsg("Pan/Tilt: Error spawning data input task\n"); return(ERROR); } /* if */ FOREVER { /* Check if Thruster is switched on */ if ((microDevPowerSwitchState(µIOCtrl) != SWITCH_ON) || ((microDevPowerSwitchState(µIOCtrl) == SWITCH_ON) && (microIOCtrl.resetEventCount != 0))) /* Process SRQ's from micro */ while(processPanTiltSRQ(&dmItems, µIOCtrl, can) != MICRO_RESET_SRQ); microIOCtrl.resetEventCount++; /* Count reset events from micro */ /* Reset event received from Micro so */ /* Mark serial coms link as up */ setSerialLinkState(µIOCtrl, LINK_UP); /* Initialize Pan & Tilt DM Item values */ if (panTiltDmItemInit( &dmItems, µIOCtrl.sio32DataChan ) == OK) /* Wake up output task to configure micro*/ semGive(microIOCtrl.childTasks[OUT_TASK].taskSyncSem); else setSerialLinkState(µIOCtrl, LINK_DOWN); } /* FOREVER */ /* Delete Child Tasks */ taskDelete(microIOCtrl.childTasks[IN_TASK].taskId); taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); /* Close serial communication chan */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); /* dm_stop_provider, dm_stop_consumer, etc are NOT */ /* required when the task exits as the Data Manager */ /* does this automatically when the task is deleted */ /* via taskDeleteHookAdd() and dm_task_exit functions*/ return(OK); /* That's all folks */ } /* panTiltTask() */ /****************************************************************************/ /* Function : initPanTiltAxisConsumer */ /* Purpose : Initializes DM interface for P/T axis for out task */ /* Inputs : P/T Axis, pointer to DM items, pt group struct, update sem */ /* Outputs : None, but Data Manager calls are made */ /****************************************************************************/ MLocal Void initPanTiltAxisConsumer(Nat16 axis, Reg panTiltDmItems *dmItems, Reg ptOutGroup *ptGroup, SEM_ID dmUpdateSem) { dm_start_consumer(dmItems->ptItems[axis].initPtCtrl, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].resetPtCtrl, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].homeToZero, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].writePtNVRam, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].ctrlString, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].analogPosReq, DM_ASYNC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].stepperPosReq, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].holdCurrent, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].runCurrent, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].accelSlope, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].decelSlope, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].settlingTime, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].initialVel, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].slewVelocity, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].cwLimitPos, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].ccwLimitPos, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].positionCmd, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->ptItems[axis].velocityCmd, DM_ASYNC, dmUpdateSem); /* Create DM Group for each axis */ ptGroup->dmGroup = dm_create_group(); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].initPtCtrl, &ptGroup->initPtCtrlBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].resetPtCtrl, &ptGroup->resetPtCtrlBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].writePtNVRam, &ptGroup->writeNVRamBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].homeToZero, &ptGroup->homeToZeroBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].ctrlString, &ptGroup->ctrlStringBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].analogPosReq, &ptGroup->analogPosReqBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].stepperPosReq, &ptGroup->stepperPosReqBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].holdCurrent, &ptGroup->holdCurrentBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].runCurrent, &ptGroup->runCurrentBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].accelSlope, &ptGroup->accelSlopeBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].decelSlope, &ptGroup->decelSlopeBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].settlingTime, &ptGroup->settlingTimeBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].initialVel, &ptGroup->initialVelBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].slewVelocity, &ptGroup->slewVelocityBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].velocityCmd, &ptGroup->velocityCmdBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].positionCmd, &ptGroup->positionCmdBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].cwLimitPos, &ptGroup->cwLimitPosBit); dm_group_add_item(ptGroup->dmGroup, dmItems->ptItems[axis].ccwLimitPos, &ptGroup->ccwLimitPosBit); } /* initPanTiltAxisConsumer() */ /****************************************************************************/ /* Function : processPtAxisDmItems */ /* Purpose : Processes DM Item chane requests for P/T axis for out task */ /* Inputs : P/T Axis, pointer to DM items, pt group struct, */ /* microIOControl structure, pointers to velocity move and */ /* digital position update request flags */ /* Outputs : velocity move and digital position update request flags */ /****************************************************************************/ MLocal Void processPtAxisDmItems(Nat16 axis, Reg panTiltDmItems *dmItems, ptOutGroup *ptGroup, microIOControl *microIOCtrl, MBool *analogPosReq, MBool *digitalPosReq) { DWord ptBits; /* Holds pan/tilt axis changes bit vector*/ /* Ask Data Manager which items changed */ if ((ptBits = dm_get_group_changes(ptGroup->dmGroup)) == 0) return; if ((ptBits & ptGroup->holdCurrentBit) || (ptBits & ptGroup->runCurrentBit)) writePtCurrents(SERIAL_CHAN, dmItems->ptItems[axis].holdCurrent, dmItems->ptItems[axis].runCurrent, axis); if ((ptBits & ptGroup->accelSlopeBit) || (ptBits & ptGroup->decelSlopeBit)) writePtSlopes(SERIAL_CHAN, dmItems->ptItems[axis].accelSlope, dmItems->ptItems[axis].decelSlope, axis); if (ptBits & ptGroup->settlingTimeBit) writePtSettlingTime(SERIAL_CHAN, dmItems->ptItems[axis].settlingTime, axis); if (ptBits & ptGroup->slewVelocityBit) writePtSlewVel(SERIAL_CHAN, dmItems->ptItems[axis].slewVelocity, axis); if (ptBits & ptGroup->initialVelBit) writePtInitialVel(SERIAL_CHAN, dmItems->ptItems[axis].initialVel, axis); if ((ptBits & ptGroup->cwLimitPosBit) || (ptBits & ptGroup->ccwLimitPosBit)) writePtLimitPos(SERIAL_CHAN, dmItems->ptItems[axis].cwLimitPos, dmItems->ptItems[axis].ccwLimitPos, axis); if (ptBits & ptGroup->velocityCmdBit) writePtVelocityCmd(SERIAL_CHAN, dmItems->ptItems[axis].velocityCmd, axis); if (ptBits & ptGroup->analogPosReqBit) *analogPosReq = TRUE; if (ptBits & ptGroup->stepperPosReqBit) *digitalPosReq = TRUE; if (ptBits & ptGroup->positionCmdBit) writePtPositionCmd(SERIAL_CHAN, dmItems->ptItems[axis].positionCmd, axis); if (ptBits & ptGroup->initPtCtrlBit) { if (initializePtCtrl(SERIAL_CHAN, axis) == OK) { if ( initPanTiltParamsDm(SERIAL_CHAN, &dmItems->ptItems[axis], axis) == ERROR ) logMsg("Pan/Tilt: Error reading parameters for %s\n", panTiltId[axis]); } /* if */ } /* if */ if (ptBits & ptGroup->resetPtCtrlBit) resetPtCtrl(SERIAL_CHAN, axis); if (ptBits & ptGroup->homeToZeroBit) homePtToZero(SERIAL_CHAN, axis); if (ptBits & ptGroup->writeNVRamBit) writePtCtrlNVRam(SERIAL_CHAN, axis); if (ptBits & ptGroup->ctrlStringBit) writePtCtrlString(SERIAL_CHAN, axis, dmItems->ptItems[axis].ctrlString); } /* processPtAxisDmItems() */ /****************************************************************************/ /* Function : initPtMicroDmItems */ /* Purpose : Initializes DM Interrface for P/T microcontroller out task */ /* Inputs : Pointer to DM items, pt group struct, DM update semaphore */ /* microIOControl structure */ /* Outputs : None, but Data Manager calls are made */ /****************************************************************************/ MLocal Void initPtMicroDmItems(Reg panTiltDmItems *dmItems, ptMicroGroup *ptGroup, SEM_ID updateSem, microIOControl *microIOCtrl) { dm_start_consumer(microIOCtrl->powerSwitchDm, DM_STATIC, updateSem); /* Declare this task a consumer of items */ /* which are written to the pan & tilt */ dm_start_consumer(dmItems->microDm.resetEvent, DM_STATIC, updateSem); dm_start_consumer(dmItems->microDm.resetCmd, DM_STATIC, updateSem); dm_start_consumer(dmItems->gndFaultTest, DM_STATIC, updateSem); dm_start_consumer(dmItems->humidityWarnThresh, DM_STATIC, updateSem); dm_start_consumer(dmItems->humidityAlarmThresh, DM_STATIC, updateSem); dm_start_consumer(dmItems->tempWarnThresh, DM_STATIC, updateSem); dm_start_consumer(dmItems->tempAlarmThresh, DM_STATIC, updateSem); dm_start_consumer(dmItems->stepperTempWarnThresh, DM_STATIC, updateSem); dm_start_consumer(dmItems->stepperTempAlarmThresh, DM_STATIC, updateSem); dm_start_consumer(dmItems->velocityAllCmd, DM_ASYNC, updateSem); ptGroup->dmGroup = dm_create_group(); /* Create DM Group for micro items */ dm_group_add_item(ptGroup->dmGroup, microIOCtrl->powerSwitchDm, &ptGroup->panTiltSwitchBit); dm_group_add_item(ptGroup->dmGroup, dmItems->microDm.resetEvent, &ptGroup->resetEventBit); dm_group_add_item(ptGroup->dmGroup, dmItems->microDm.resetCmd, &ptGroup->resetCmdBit); dm_group_add_item(ptGroup->dmGroup, dmItems->gndFaultTest, &ptGroup->gndFltTestBit); dm_group_add_item(ptGroup->dmGroup, dmItems->humidityWarnThresh, &ptGroup->humidityWarnBit); dm_group_add_item(ptGroup->dmGroup, dmItems->humidityAlarmThresh, &ptGroup->humidityAlarmBit); dm_group_add_item(ptGroup->dmGroup, dmItems->tempWarnThresh, &ptGroup->tempWarnBit); dm_group_add_item(ptGroup->dmGroup, dmItems->tempAlarmThresh, &ptGroup->tempAlarmBit); dm_group_add_item(ptGroup->dmGroup, dmItems->stepperTempWarnThresh, &ptGroup->stepperTempWarnBit); dm_group_add_item(ptGroup->dmGroup, dmItems->stepperTempAlarmThresh, &ptGroup->stepperTempAlarmBit); dm_group_add_item(ptGroup->dmGroup, dmItems->velocityAllCmd, &ptGroup->velocityAllCmdBit); } /* initPtMicroDmItems() */ /****************************************************************************/ /* Function : processMicroDmItems */ /* Purpose : Processes DM Item change requests for P/T microcontroller */ /* Inputs : micro Group, pointer to DM items, microIOControl structure */ /* P/T can ID */ /* Outputs : Returns TRUE if micro was reset or switched off, else FALSE*/ /****************************************************************************/ MLocal MBool processMicroDmItems(ptMicroGroup *microGroup, Reg panTiltDmItems *dmItems, microIOControl *microIOCtrl, panTiltCanId can, MBool *velocityMove) { DWord changedBits; /* Holds dm_group_changes bit vector */ /* Ask Data Manager which items changed */ if ((changedBits = dm_get_group_changes(microGroup->dmGroup)) == 0) return(FALSE); /* No items have changed */ /* Check if micro Reset Event occurred */ if (changedBits & microGroup->resetEventBit) return(TRUE); /* Reset event occurred */ if (changedBits & microGroup->resetCmdBit) { /* Mark serial link as down */ setSerialLinkState(microIOCtrl, LINK_DOWN); /* Set keep alive mode to continuous so */ /* we can reset the link to the micro */ if (sio32ChanSetKeepAliveMode(microIOCtrl->sio32DataChan.channel, 0, 0) == ERROR) logMsg("Error clearing KA Mode for %s pan/tilt", canName[can]); /* Manual reset so ammounce it */ microIOCtrl->announceResets = TRUE; /* Send Reset Command to Microcontroller */ microResetCmd(SERIAL_CHAN); return (TRUE); /* Exit loop and wait for RESET */ } /* if */ /* Check if pan/tilt power was switch off*/ if ( (changedBits & microGroup->panTiltSwitchBit) && (microDevPowerSwitchState(microIOCtrl) != SWITCH_ON) ) { /* Pan/Tilt switched off so link is down */ setSerialLinkState(microIOCtrl, LINK_DOWN); if (sio32ChanSetKeepAliveMode(microIOCtrl->sio32DataChan.channel, 0, 0) == ERROR) logMsg("Error changing KA Mode for %s pan/tilt", canName[can]); return(TRUE); /* Exit loop and wait for power up */ } /* if */ if ( (changedBits & microGroup->humidityWarnBit) || (changedBits & microGroup->humidityAlarmBit) ) writeHumidityThresh(SERIAL_CHAN, dmItems->humidityWarnThresh, dmItems->humidityAlarmThresh); if ( (changedBits & microGroup->tempWarnBit) || (changedBits & microGroup->tempAlarmBit) ) writeTemperatureThresh(SERIAL_CHAN, dmItems->tempWarnThresh, dmItems->tempAlarmThresh); if ( (changedBits & microGroup->stepperTempWarnBit) || (changedBits & microGroup->stepperTempAlarmBit) ) writeStepperTempThresh(SERIAL_CHAN, dmItems->stepperTempWarnThresh, dmItems->stepperTempAlarmThresh); if (changedBits & microGroup->gndFltTestBit) microWriteGndFaultTest(SERIAL_CHAN, dmItems->gndFaultTest); *velocityMove = /* velocity command for all axies */ (changedBits & microGroup->velocityAllCmdBit ? TRUE : FALSE); return (FALSE); } /* processMicroDmItems() */ /*****************************************************************************/ /* Function : panTiltOutTask */ /* Purpose : Pan & Tilt Out Task. Read DM items & write to Pan & Tilt */ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void panTiltOutTask(Reg panTiltDmItems *dmItems, microIOControl *microIOCtrl, panTiltCanId can) { Int16 axis; /* Pan/Tilt axis index */ DWord dmBits; /* used for clearing dm group bits */ SEM_ID dmUpdateSem; /* Data Manager wakes up task when an */ ptMicroGroup microGroup; /* Micro data manager item group */ Nat32 ticks; /* For subtacting loop time from timer */ Int32 delay; /* Calculated loop delay */ Nat32 sampleTicks; /* Period in ticks for Analog Feedback */ MBool velocityMove; /* All velocity Move Command request */ MBool analogPosReq; /* Analog position feedback request */ MBool digitalPosReq; ptOutGroup ptGroup[PT_AXIS_COUNT]; panTiltPowerMgrStruct panTiltPowerMgr; if (createPanTiltPwrManager(&panTiltPowerMgr, can) == ERROR) { logMsg("Pan/Tilt: Error initializing power manager structure\n"); return; } /* if */ /* Create switch & power sync semaphores */ if ((dmUpdateSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Pan/Tilt: Error initializing Input Task Resources\n"); return; } /* if */ initPtMicroDmItems(dmItems, µGroup, dmUpdateSem, microIOCtrl); for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) { initPanTiltAxisConsumer(axis, dmItems, &ptGroup[axis], dmUpdateSem); dm_start_provider(dmItems->ptItems[axis].analogDegs, DM_ASYNC); dm_start_provider(dmItems->ptItems[axis].stepperDegs, DM_ASYNC); } /* for */ FOREVER { /* Mark micro configuration as invalid */ microIOCtrl->microConfigured = FALSE; /* Wait for micro reset via SRQ Task */ semTake(microIOCtrl->childTasks[OUT_TASK].taskSyncSem, WAIT_FOREVER); /* We need to initialize default state of Data manager Items here */ /* Disable Ground Fault Testing, if on */ microWriteGndFaultTest(SERIAL_CHAN, dmItems->gndFaultTest); initHumidityThreshDm(SERIAL_CHAN, dmItems->humidityWarnThresh, dmItems->humidityAlarmThresh); initTemperatureThreshDm(SERIAL_CHAN, dmItems->tempWarnThresh, dmItems->tempAlarmThresh); initStepperTempThreshDm(SERIAL_CHAN, dmItems->stepperTempWarnThresh, dmItems->stepperTempAlarmThresh); for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) { setPtLimitPositions(SERIAL_CHAN, dmItems->ptItems[axis].cwLimitPos, dmItems->ptItems[axis].ccwLimitPos, can, axis); downloadInterpolationTable(SERIAL_CHAN, can, axis); } /* if */ /* Set Velocities to zero */ ptClearVelocityAllDmItem(dmItems); /* Clear item changed bits after reset */ dmBits = dm_get_group_changes(microGroup.dmGroup); for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) dmBits = dm_get_group_changes(ptGroup[axis].dmGroup); /* Flag that micro has been configured */ microIOCtrl->microConfigured = TRUE; /* Wake up input task to read from micro */ semGive(microIOCtrl->childTasks[IN_TASK].taskSyncSem); /* Change Serial I/O Keep Alive Mode so */ /* motor disables if communication stops */ if (sio32ChanSetKeepAliveMode(microIOCtrl->sio32DataChan.channel, KEEP_ALIVE_TICKS, KEEP_ALIVE_COUNT) == ERROR) logMsg("Error setting Keep Alive Mode for %s pan/tilt", canName[can]); sampleTicks = sysClkRateGet() / (USECS_PER_SEC / PT_POSITION_DM_PERIOD); ticks = tickGet(); /* Update Rate is not guaranteed due to */ /* the possibility of task preemption */ digitalPosReq = TRUE; /* Initialize digital position data */ while (microIOCtrl->serialLinkState == LINK_UP) { /* Wait for any data items to change */ analogPosReq = TRUE; if ((delay = sampleTicks - min(tickGet() - ticks, sampleTicks)) > 0) analogPosReq = (semTake(dmUpdateSem, delay) == ERROR); ticks = tickGet(); /* Reschedule analog position feedback */ taskSafe(); /* Prevent task from being deleted during*/ /* serial IO and Data Manager updates */ if (processMicroDmItems(µGroup, dmItems, microIOCtrl, can, &velocityMove) == TRUE) { taskUnsafe(); break; /* Exit loop and re-initialize DM items */ } /* if */ for (axis = LIGHT_PAN; axis <= CAMERA_TILT; axis++) processPtAxisDmItems(axis, dmItems, &ptGroup[axis], microIOCtrl, &analogPosReq, &digitalPosReq); if (velocityMove) /* velocity command for all axies */ ptVelocityMoveWithFeedbackCmd(SERIAL_CHAN, dmItems); else if (analogPosReq) updateAnalogPanTiltPositionsDm( SERIAL_CHAN, dmItems ); if (digitalPosReq) /* Update digital position feedback */ updateStepperPanTiltPositionsDm( SERIAL_CHAN, dmItems ); velocityMove = FALSE; digitalPosReq = FALSE; taskUnsafe(); /* Task may now be safely deleted */ } /* while (LINK_UP) */ } /* FOREVER */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are NOT required when the task exits as the */ /* Data Manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ /* That's all folks ! */ } /* panTiltOutTask() */ /*****************************************************************************/ /* Function : panTiltInTask */ /* Purpose : Pan & Tilt Input Task. Wakes up periodically, read data */ /* from the Pan & Tilt and write Data Manager Items */ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void panTiltInTask(Reg panTiltDmItems* dmItems, microIOControl *microIOCtrl) { SEM_ID wakeupSem; /* Used by watch dog to wake up this task*/ Int32 updates = 0; /* Counts dm updates for rate calculation*/ Nat16 panTilt; /* Pan & Tilt axis index */ /* Create wakeup semaphore and watch dog */ /* timer for provider rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Pan/Tilt: Error initializing Input Task Resources\n"); return; } /* if */ /* Provider Task for non-static data items*/ dm_start_provider(dmItems->microHumidity, HUMIDITY_DM_PERIOD); dm_start_provider(dmItems->microTemperature, HUMIDITY_DM_PERIOD); dm_start_provider(dmItems->stepperTemp, HUMIDITY_DM_PERIOD); dm_start_provider(dmItems->localGfCurrent, GF_DM_UPDATE_PERIOD); dm_start_provider(dmItems->extGfCurrent, GF_DM_UPDATE_PERIOD); FOREVER { /* Wait for Motor Coms to be initialized */ semTake(microIOCtrl->childTasks[IN_TASK].taskSyncSem, WAIT_FOREVER); while (microIOCtrl->serialLinkState == LINK_UP) { taskSafe(); /* Prevent task from being deleted during */ /* serial IO and Data Manager updates */ /* Update Ground Fault Current */ updateGndFltCurrentsDm(SERIAL_CHAN, dmItems->localGfCurrent, dmItems->extGfCurrent ); if ((updates % (HUMIDITY_DM_PERIOD / GF_DM_UPDATE_PERIOD)) == 0) { updateHumidityDm (SERIAL_CHAN, dmItems->microHumidity); updateTemperatureDm(SERIAL_CHAN, dmItems->microTemperature); updateStepperTempDm(SERIAL_CHAN, dmItems->stepperTemp); } /* if */ taskUnsafe(); /* task may now be deleted safely */ /* increment & range check update counter*/ if (++updates == 10000) updates = 0; /* pend until watch dog timer wakes us up*/ semTake(wakeupSem, sysClkRateGet() / (USECS_PER_SEC / GF_DM_UPDATE_PERIOD)); } /* while */ } /* FOREVER */ semDelete(wakeupSem); /* semaphore resources */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are not required when the task exits as the */ /* data manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ /* that's all folks ! */ } /* panTiltInTask() */ /*****************************************************************************/ /* Function : processPanTiltSRQ */ /* Purpose : Decode Service Request and take appropriate action */ /* Inputs : SRQ code, array of DM items, serial channel */ /* Outputs : Returns Reset Status & many DM items are changed */ /*****************************************************************************/ LOCAL Word processPanTiltSRQ( Reg panTiltDmItems* dmItems, microIOControl *microIOCtrl, panTiltCanId can ) { Byte buffer[SRQ_PACKET_LEN]; /* Buffer for micro communications data */ Word srqRequest; /* Service Request Value */ Word alarmStatus; /* Microcontroller Alarm Status Word */ microAlarm alarm; /* Alarm Status */ gfStatus gfAlarm; /* Ground Fault Alarm Status */ gfTestStatus gfTest; /* Ground Fault Test Status */ Int16 panTilt; /* Pan & tilt axis index */ Nat16 limits; /* Limit Switch Status */ Int16 breakPts, pnt; Int16 analogDegs; Int16 cwLimit, ccwLimit; Flt32 degs; char calDataFile[80]; static FILE* ptCalFd; /* File descriptor for cal data logging */ if (readSRQPacket(SERIAL_CHAN, buffer, SRQ_PACKET_LEN, WAIT_FOREVER) == ERROR) return (ERROR); /* Service Request received so process it*/ srqRequest = wordFromBuf(buffer, 0); switch (srqRequest) /* Decode Service Request from micro */ { case MICRO_RESET_SRQ: /* Microcontroller Reset Occurred */ /* Post Reset Event DM Item */ if (microIOCtrl->announceResets) microResetEventDm(dmItems->microDm.resetEvent); writeBooleanDmItem(dmItems->initializing, TRUE); break; case INITIALIZED_SRQ: /* Pan/tilt micro re-initialized */ writeBooleanDmItem(dmItems->initializing, FALSE); break; case WATER_ALARM_ON_SRQ: /* Housing Water Alarm Active */ writeBooleanDmItem(dmItems->canWaterAlarm, TRUE); break; case WATER_ALARM_OFF_SRQ: /* Housing Water Alarm Inactive */ writeBooleanDmItem(dmItems->canWaterAlarm, FALSE); break; case GF_TEST_FAIL_ON_SRQ: gfTest = GF_TEST_FAIL_ON; writeDmItem(dmItems->gndFaultTestStatus, &gfTest, sizeof(gfTest)); break; case GF_TEST_FAIL_OFF_SRQ: gfTest = GF_TEST_FAIL_OFF; writeDmItem(dmItems->gndFaultTestStatus, &gfTest, sizeof(gfTest)); break; case LOCAL_GNDFLT_SRQ: wordsFromBuf(buffer, 2, &srqRequest, &alarmStatus); gfAlarm = (gfStatus) alarmStatus; writeDmItem(dmItems->localGndFaultAlarm, &gfAlarm, sizeof(gfAlarm)); break; case EXTERNAL_GNDFLT_SRQ: wordsFromBuf(buffer, 2, &srqRequest, &alarmStatus); gfAlarm = (gfStatus) alarmStatus; writeDmItem(dmItems->extGndFaultAlarm, &gfAlarm, sizeof(gfAlarm)); break; case HUMIDITY_ALARM_SRQ: wordsFromBuf(buffer, 2, &srqRequest, &alarmStatus); alarm = (microAlarm) alarmStatus; writeDmItem(dmItems->humidityAlarm, &alarm, sizeof(alarm)); break; case TEMP_ALARM_ON: wordsFromBuf(buffer, 2, &srqRequest, &alarmStatus); alarm = (microAlarm) alarmStatus; writeDmItem(dmItems->temperatureAlarm, &alarm, sizeof(alarm)); break; case STEPPER_TEMP_ALARM_SRQ: wordsFromBuf(buffer, 2, &srqRequest, &alarmStatus); alarm = (microAlarm) alarmStatus; writeDmItem(dmItems->stepperTempAlarm, &alarm, sizeof(alarm)); break; case PT_FAULT_SRQ: wordsFromBuf(buffer, 3, &srqRequest, &panTilt, &alarmStatus); alarmStatus = (alarmStatus ? TRUE : FALSE); writeDmItem(dmItems->ptItems[panTilt].ctrlFault, &alarmStatus, sizeof(alarmStatus)); break; case PT_MOVE_ERROR_SRQ: wordsFromBuf(buffer, 2, &srqRequest, &panTilt); writeDmItem(dmItems->ptItems[panTilt].moveError, NULL, 0); break; case PT_ZERO_POS_ERROR_SRQ: wordsFromBuf(buffer, 2, &srqRequest, &panTilt); writeDmItem(dmItems->ptItems[panTilt].zeroPosError, NULL, 0); break; case PT_LIMIT_SWITCH_SRQ: wordsFromBuf(buffer, 3, &srqRequest, &panTilt, &limits); writeBooleanDmItem(dmItems->ptItems[panTilt].cwLimit, (PT_CW_LIMIT(limits) ? TRUE : FALSE)); writeBooleanDmItem(dmItems->ptItems[panTilt].ccwLimit, (PT_CCW_LIMIT(limits) ? TRUE : FALSE)); break; default: printf("Pan/Tilt: Unrecognized SRQ 0x%x\n", srqRequest); break; } /* switch */ return(srqRequest); /* Return SRQ received to caller */ } /* processPanTiltSRQ() */